#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
*/
01 = nmea output
02 = binary output
*/
+ uint8_t attributes; /*
+ 0 - update ram
+ 1 - update ram & flash
+ */
} m_output_t;
} m_navmode_t;
-#define DBG_SEND
-#define DBG_RECV
-
void serial1_tx_cout(uint8_t c)
{
debug_printf("%.2X ", c);
{
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;
}
{
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)
+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;
}
{
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;
}
{
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;
}
{
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;
}
{
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;
}
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)
{
+ 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);
/* display current GPS position stored in the global variable */
static void display_gps(void)
{
+ /* no need to call ntohs/ntohl because the frame is already fixed by
+ * decode_gps_pos() */
printf_P(PSTR("id %.2X mode %.2X svnum %.2X gpsw %.4X "
"tow %.10"PRIu32"\t"),
gps_pos.msg_id,
(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();
+
+ last_valid = ms;
+ is_valid = 1;
}
}
- resched:
- callout_schedule(cm, tim, 2);
+ diff = ms - last_valid;
+ if (is_valid == 1 && diff > 100) {
+
+ 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_configure();
callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
- callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
+ callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */
return 0;
}
memcpy(pos, &gps_pos, sizeof(*pos));
}
-int gps_loop(void)
+int gps_log(uint8_t to_stdout)
{
uint32_t ms;
uint8_t flags, prio;
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;
- }
+ 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 (!to_stdout && 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);
-
+ }
+ else if (to_stdout) {
+ printf_P(PSTR("%s"), buf);
}
return 0;
git.droids-corp.org / fpv.git / blobdiff