#include <uart.h>
#include "main.h"
+#include "gps.h"
#include "gps_venus.h"
-#include "fat.h"
-#include "fat_config.h"
-#include "partition.h"
-#include "sd_raw.h"
-#include "sd_raw_config.h"
-#include "sd_log.h"
+/* note: enabling debug will make the code fail due to rx queue full */
#define debug_printf(fmt, a...) do { } while (0)
/* #define debug_printf(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) */
#define GPS_UART 0
+#define GPS_PERIOD_MS 2
+
/*
https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
*/
static struct gps_frame rxframe;
-static struct gps_pos gps_pos;
-
static struct callout gps_timer;
/* INPUT FORMATS */
uint8_t attributes;
} m_waas_t;
+/* 0x39 - configure position pinning */
+#define M_PINNING 0x39
+typedef struct {
+ uint8_t enable; /* 0 = default, 1 = enable, 2 = disable */
+ uint8_t attributes; /* 0 = ram, 1 = ram+flash */
+} m_pinning_t;
+
+/* 0x3B - configure position pinning */
+#define M_PINNING_PARAMS 0x3B
+typedef struct {
+ uint16_t pin_speed;
+ uint16_t pin_cnt;
+ uint16_t unpin_speed;
+ uint16_t unpin_cnt;
+ uint16_t unpin_dist;
+ uint8_t attributes; /* 0 = ram, 1 = ram+flash */
+} m_pinning_params_t;
+
/* 0x3C - nav mode */
#define M_NAVMODE 0x3c
typedef struct {
} m_navmode_t;
-#define DBG_SEND
-#define DBG_RECV
-
-void serial1_tx_cout(uint8_t c)
+static void serial1_tx_cout(uint8_t c)
{
debug_printf("%.2X ", c);
uart_send(GPS_UART, c);
}
-void venus634_send(uint8_t type, void *payload, uint16_t len)
+static void venus634_send(uint8_t type, void *payload, uint16_t len)
{
uint8_t crc = 0, n;
debug_printf("\n");
}
-void venus634_restart(void)
+static void venus634_restart(void)
{
m_restart_t restart;
- memset(&restart,0,sizeof(m_restart_t));
+ memset(&restart, 0, sizeof(m_restart_t));
restart.start_mode = 0x03; /* COLD */
- venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
+ venus634_send(M_RESTART, &restart, sizeof(m_restart_t));
return;
}
-void venus634_config_serial(void)
+__attribute__((unused))
+static void venus634_config_serial(void)
{
m_serconf_t serconf;
- memset(&serconf,0,sizeof(m_serconf_t));
+ memset(&serconf, 0, sizeof(m_serconf_t));
serconf.port = 0;
serconf.baud = 4;
serconf.update = 1;
- venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
+ venus634_send(M_SERCONF, &serconf, sizeof(m_serconf_t));
return;
}
-
-void venus634_msg_type(void)
+static void venus634_msg_type(uint8_t mode)
{
m_output_t output;
- memset(&output,0,sizeof(m_output_t));
- output.msg_type = 0x02; /* binary msg */
+ memset(&output, 0, sizeof(m_output_t));
+ output.msg_type = mode;
output.attributes = 0; /* ram, init may freeze when update in flash */
- venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
+ venus634_send(M_OUTPUT, &output, sizeof(m_output_t));
return;
}
-void venus634_rate(void)
+static void venus634_rate(void)
{
m_rate_t rate;
- memset(&rate,0,sizeof(m_rate_t));
- rate.rate = 20;
- rate.attributes = 0; /* ram */
+ memset(&rate, 0, sizeof(m_rate_t));
+ rate.rate = 5;
+ rate.attributes = 0; /* ram, init may freeze when update in flash */
- venus634_send(M_RATE,&rate,sizeof(m_rate_t));
+ venus634_send(M_RATE, &rate, sizeof(m_rate_t));
return;
}
/* Wide Area Augmentation System: use ground stations to increase the precision
* of gps position */
-void venus634_waas(void)
+static void venus634_waas(void)
{
m_waas_t waas;
- memset(&waas,0,sizeof(m_waas_t));
+ memset(&waas, 0, sizeof(m_waas_t));
waas.enable = 1;
- waas.attributes = 0; /* ram */
+ waas.attributes = 0; /* ram, init may freeze when update in flash */
- venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
+ venus634_send(M_WAAS, &waas, sizeof(m_waas_t));
return;
}
+/* Configure position pinning – Enable or disable position pinning of GNSS
+ * receiver */
+static void venus634_pinning(void)
+{
+ m_pinning_t pinning;
+
+ memset(&pinning, 0, sizeof(m_pinning_t));
+ pinning.enable = 2;
+ pinning.attributes = 0; /* ram */
+
+ venus634_send(M_PINNING, &pinning, sizeof(m_pinning_t));
+}
+
+/* Configure position pinning parameters */
+static void venus634_pinning_params(void)
+{
+ m_pinning_params_t pinning_params;
+
+ /* just set everything to 0 */
+ memset(&pinning_params, 0, sizeof(m_pinning_params_t));
+ pinning_params.attributes = 0; /* ram */
+
+ venus634_send(M_PINNING_PARAMS, &pinning_params,
+ sizeof(m_pinning_params_t));
+}
+
+
/* Tell we are a car instead of a pedestrian */
-void venus634_navmode(void)
+__attribute__((unused))
+static void venus634_navmode(void)
{
m_navmode_t navmode;
- memset(&navmode,0,sizeof(m_navmode_t));
+ memset(&navmode, 0, sizeof(m_navmode_t));
navmode.navmode = 0; /* car */
- navmode.attributes = 0; /* ram */
+ navmode.attributes = 0; /* ram, init may freeze when update in flash */
- venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
+ venus634_send(M_NAVMODE, &navmode, sizeof(m_navmode_t));
return;
}
/* frequency of NMEA messages */
-void venus634_nmea_interval(void)
+__attribute__((unused))
+static void venus634_nmea_interval(void)
{
m_nmeainterval_t nmeainterval;
- memset(&nmeainterval,0,sizeof(m_nmeainterval_t));
+ memset(&nmeainterval, 0, sizeof(m_nmeainterval_t));
/* set frequency for nmea: 1 = once every one position fix, 2 = once
* every two position fix, ... */
nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
nmeainterval.zda = 1; /* Time & Date*/
- nmeainterval.attributes = 1; /* ram flash */
+ nmeainterval.attributes = 1; /* ram & flash */
- venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t));
+ venus634_send(M_NMEAINTERVAL, &nmeainterval, sizeof(m_nmeainterval_t));
return;
}
-int8_t recv_cb(uint8_t byte)
+static int8_t recv_cb(uint8_t byte)
{
uint16_t i;
return 1;
}
-int recv_msg(void)
+static int recv_msg(void)
{
int ret;
+ uint8_t irq_flags;
int16_t c;
+ uint16_t t1, t2, diff;
+ IRQ_LOCK(irq_flags);
+ t1 = global_ms;
+ IRQ_UNLOCK(irq_flags);
+
+ printf_P(PSTR("recv_msg()\n"));
while (1) {
- /* XXX use select for timeout */
- c = uart_recv(GPS_UART);
+ IRQ_LOCK(irq_flags);
+ t2 = global_ms;
+ IRQ_UNLOCK(irq_flags);
+ diff = t2 - t1;
+ if (diff > 1000) {
+ printf_P(PSTR("recv_msg timeout\n"));
+ return -1;
+ }
+
+ c = uart_recv_nowait(GPS_UART);
+ if (c < 0)
+ continue;
+
+ debug_printf("%2.2x\n", c);
+
ret = recv_cb(c);
if (ret == 0)
return 0;
}
}
-int wait_ack(int msg_type)
+static int wait_ack(int msg_type)
{
+ uint8_t irq_flags;
int ret;
+ uint16_t t1, t2, diff;
+
+ IRQ_LOCK(irq_flags);
+ t1 = global_ms;
+ IRQ_UNLOCK(irq_flags);
+
while (1) {
ret = recv_msg();
if (ret < 0)
return -1;
+ IRQ_LOCK(irq_flags);
+ t2 = global_ms;
+ IRQ_UNLOCK(irq_flags);
+ diff = t2 - t1;
+ if (diff > 1000) {
+ printf_P(PSTR("wait_ack timeout\n"));
+ return -1;
+ }
+
/* retry if it's not the expected message */
if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
continue;
if (len != sizeof(*pos))
return -1;
- if (pos->msg_id != 0xA8) /* XXX not tested */
+ if (pos->msg_id != 0xA8)
return -1;
- gps_pos.gps_week = ntohs(pos->gps_week);
- gps_pos.tow = ntohl(pos->tow);
+ /* fix endianness inside the frame */
+ pos->gps_week = ntohs(pos->gps_week);
+ pos->tow = ntohl(pos->tow);
- gps_pos.latitude = ntohl(pos->latitude);
- gps_pos.longitude = ntohl(pos->longitude);
- gps_pos.altitude = ntohl(pos->altitude);
+ pos->latitude = ntohl(pos->latitude);
+ pos->longitude = ntohl(pos->longitude);
+ pos->altitude = ntohl(pos->altitude);
- gps_pos.sea_altitude = ntohl(pos->sea_altitude);
+ pos->sea_altitude = ntohl(pos->sea_altitude);
- gps_pos.gdop = ntohs(pos->gdop);
- gps_pos.pdop = ntohs(pos->pdop);
- gps_pos.hdop = ntohs(pos->hdop);
- gps_pos.vdop = ntohs(pos->vdop);
- gps_pos.tdop = ntohs(pos->tdop);
+ pos->gdop = ntohs(pos->gdop);
+ pos->pdop = ntohs(pos->pdop);
+ pos->hdop = ntohs(pos->hdop);
+ pos->vdop = ntohs(pos->vdop);
+ pos->tdop = ntohs(pos->tdop);
- gps_pos.ecef_vx = ntohl(pos->ecef_vx);
- gps_pos.ecef_vy = ntohl(pos->ecef_vy);
- gps_pos.ecef_vz = ntohl(pos->ecef_vz);
+ pos->ecef_vx = ntohl(pos->ecef_vx);
+ pos->ecef_vy = ntohl(pos->ecef_vy);
+ pos->ecef_vz = ntohl(pos->ecef_vz);
/* update global structure */
IRQ_LOCK(irq_flags);
return 0;
}
-/* display current GPS position stored in the global variable */
-static void display_gps(void)
-{
- printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
- "tow %.10"PRIu32"\t"),
- gps_pos.msg_id,
- gps_pos.mode,
- gps_pos.sv_num,
- gps_pos.gps_week,
- gps_pos.tow);
-
- printf_P(PSTR("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n"),
- gps_pos.latitude,
- gps_pos.longitude,
- gps_pos.altitude);
-
- printf_P(PSTR("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f "
- "tdop %3.3f\n"),
- (double)gps_pos.gdop/100.,
- (double)gps_pos.pdop/100.,
- (double)gps_pos.hdop/100.,
- (double)gps_pos.vdop/100.,
- (double)gps_pos.tdop/100.);
-
- printf_P(PSTR("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n"),
- (double)gps_pos.latitude/10000000.,
- (double)gps_pos.longitude/10000000.,
- (double)gps_pos.altitude/100.,
- (double)gps_pos.sea_altitude/100.);
-
- printf_P(PSTR("vx %3.3f vy %3.3f vz %3.3f\n"),
- (double)gps_pos.ecef_vx/100.,
- (double)gps_pos.ecef_vy/100.,
- (double)gps_pos.ecef_vz/100.);
-}
-
+/* called on timer event */
static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
{
+ static int16_t last_valid = 0;
+ static uint8_t is_valid = 0;
+ int16_t ms, diff;
int16_t c;
- int ret;
+ int8_t ret;
+ uint8_t irq_flags;
(void)cm;
(void)tim;
(void)arg;
+ IRQ_LOCK(irq_flags);
+ ms = global_ms;
+ IRQ_UNLOCK(irq_flags);
+
while (1) {
c = uart_recv_nowait(GPS_UART);
if (c < 0) /* no more char */
- goto resched;
+ break;
ret = recv_cb(c);
if (ret == 0) {
decode_gps_pos(rxframe.data, rxframe.len);
if (0)
- display_gps();
+ display_gps_pos();
+
+ last_valid = ms;
+ is_valid = 1;
}
}
- resched:
- callout_schedule(cm, tim, 2);
+ diff = ms - last_valid;
+ if (is_valid == 1 && diff > 500) {
+
+ is_valid = 0;
+ /* update global structure */
+ IRQ_LOCK(irq_flags);
+ memset(&gps_pos, 0, sizeof(gps_pos));
+ gps_pos.latitude = 0xffffffff;
+ IRQ_UNLOCK(irq_flags);
+ }
+
+ callout_schedule(cm, tim, GPS_PERIOD_MS);
}
static void venus634_configure(void)
venus634_restart();
wait_ack(M_RESTART);
- /* it seems we must wait that the GPS is restarted, else it doesn't work
- * properly */
- wait_ms(500);
-
printf_P(PSTR("binmode..."));
- venus634_msg_type();
+ venus634_msg_type(2); /* binary */
wait_ack(M_OUTPUT);
printf_P(PSTR("waas..."));
venus634_waas();
wait_ack(M_WAAS);
+ printf_P(PSTR("pinning..."));
+ venus634_pinning();
+ wait_ack(M_PINNING);
+
+ printf_P(PSTR("pinning parameters..."));
+ venus634_pinning_params();
+ wait_ack(M_PINNING_PARAMS);
+
printf_P(PSTR("rate..."));
venus634_rate();
wait_ack(M_RATE);
venus634_configure();
callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
- callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
-
- return 0;
-}
-
-void gps_get_pos(struct gps_pos *pos)
-{
- memcpy(pos, &gps_pos, sizeof(*pos));
-}
-
-int gps_loop(void)
-{
- uint32_t ms;
- uint8_t flags, prio;
- int16_t len;
- char buf[128];
- struct gps_pos pos;
-
- while (1) {
-
- IRQ_LOCK(flags);
- ms = global_ms;
- IRQ_UNLOCK(flags);
-
- /* get position (prevent modification of gps pos during copy) */
- prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
- gps_get_pos(&pos);
- callout_mgr_restore_prio(&imuboard.intr_cm, prio);
-
- /* XXX copy */
- len = snprintf(buf, sizeof(buf),
- "%"PRIu32" "
- "svnum %.2X lat %3.5f long %3.5f "
- "alt %3.5f sea_alt %3.5f\n",
- ms, gps_pos.sv_num,
- (double)gps_pos.latitude/10000000.,
- (double)gps_pos.longitude/10000000.,
- (double)gps_pos.altitude/100.,
- (double)gps_pos.sea_altitude/100.);
-
-
- if (sd_log_enabled()) {
-
- if (sd_log_write(buf, len) != len) {
- printf_P(PSTR("error writing to file\n"));
- return -1;
- }
- }
- else
- printf_P(PSTR("%s"), buf);
-
- }
+ callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
return 0;
}
git.droids-corp.org / fpv.git / blobdiff