From: Olivier Matz Date: Thu, 10 Jul 2014 16:30:00 +0000 (+0200) Subject: add gps_venus X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=commitdiff_plain;h=80d45bc2bf74e4f2833ec9427d145ecd7d8c13b7 add gps_venus --- diff --git a/Makefile b/Makefile index ade8bae..ec375bc 100644 --- a/Makefile +++ b/Makefile @@ -18,6 +18,7 @@ SRC += fat.c SRC += sd_main.c SRC += partition.c SRC += sd_raw.c +SRC += gps_venus.c CFLAGS += -W -Wall -Werror diff --git a/gps_venus.c b/gps_venus.c new file mode 100644 index 0000000..7119f8c --- /dev/null +++ b/gps_venus.c @@ -0,0 +1,696 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "main.h" +#include "gps_venus.h" +#include "fat.h" +#include "fat_config.h" +#include "partition.h" +#include "sd_raw.h" +#include "sd_raw_config.h" + +#define debug_printf(args...) do { } while (0) +/* #define debug_printf(args...) printf(args) */ + +#define GPS_UART 0 + +/* + https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12 + */ + +/* some useful defines */ +#define START1 0xA0 +#define START2 0xA1 +#define END1 0x0D +#define END2 0x0A + +/* proccessing states */ + +#define S_RESET 0 +#define S_POSTRESET 1 +#define S_START1 2 +#define S_START2 3 +#define S_LENGTH1 4 +#define S_LENGTH2 5 +#define S_DISCARD 6 +#define S_COPY 7 +#define S_ENDCOPY 8 +#define S_END 9 +#define S_END1 10 + +#define MAX_LEN 0x200 +struct gps_frame { + uint16_t len; + uint16_t cur_len; + uint8_t data[MAX_LEN]; +}; + +static struct gps_frame rxframe; + +static struct gps_pos gps_pos; + +static struct callout gps_timer; + +/* INPUT FORMATS */ + +/* packet types */ +/* 0x01 - System Restart */ +#define M_RESTART 0x01 +typedef struct { + uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */ + uint16_t utc_year; /* >= 1980 */ + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + uint8_t utc_second; + int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */ + int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */ + int16_t altitude; /* -1000 to 18300, meters */ +} m_restart_t; + + +/* 0x02 - Query Software Version */ +#define M_QUERYVER 0x02 +typedef struct { + uint8_t software_type; /* 01 = System Code */ +} m_queryver_t; + + +/* 0x05 - Serial Port Configuration */ +#define M_SERCONF 0x05 +typedef struct { + uint8_t port; /* 00 = COM1 (the only port) */ + uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400; + 04 = 57600; 05 = 115200; */ + uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */ +} m_serconf_t; + + + +/* 0x08 - nema interval */ +#define M_NEMAINTERVAL 0x08 +typedef struct { + uint8_t gga; + uint8_t gsa; + uint8_t gsv; + uint8_t gll; + uint8_t rmc; + uint8_t vtg; + uint8_t zda; + uint8_t attributes; /* + 0 - sram + 1 - flash + */ + +} m_nemainterval_t; + + +/* 0x09 - Query Software Version */ +#define M_OUTPUT 0x09 +typedef struct { + uint8_t msg_type; /* + 00 = no output + 01 = nema output + 02 = binary output + */ +} m_output_t; + + +/* 0x0E - Update Rage */ +#define M_RATE 0x0E +typedef struct { + uint8_t rate; /* Hz */ + uint8_t attributes; /* + 0 - update ram + 1 - update ram & flash + */ +} m_rate_t; + + + +/* 0x37 - configure waas */ +#define M_WAAS 0x37 +typedef struct { + uint8_t enable; /* 01 = enable */ + uint8_t attributes; +} m_waas_t; + +/* 0x3C - nav mode */ +#define M_NAVMODE 0x3c +typedef struct { + uint8_t navmode; /* 00 = car + 01 = pedestrian + */ + uint8_t attributes; +} m_navmode_t; + + +#define DBG_SEND +#define DBG_RECV + +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) +{ + uint8_t crc = 0, n; + + debug_printf("SEND "); + + /* now send the message */ + /* header */ + serial1_tx_cout(START1); + serial1_tx_cout(START2); + serial1_tx_cout(((len+1) >> 8) & 0xff); + serial1_tx_cout((len+1) & 0xff); + /* type and payload */ + serial1_tx_cout(type); + crc ^= type; + for (n = 0; n < len; n++) { + serial1_tx_cout(*(uint8_t *)payload); + crc ^= *(uint8_t *)payload; + payload++; + } + /* checksum and tail */ + serial1_tx_cout(crc); + serial1_tx_cout(END1); + serial1_tx_cout(END2); + + debug_printf("\n"); +} + +void venus634_restart(void) +{ + m_restart_t restart; + + memset(&restart,0,sizeof(m_restart_t)); + restart.start_mode = 0x03; /* COLD */ + + venus634_send(M_RESTART,&restart,sizeof(m_restart_t)); + + return; +} + +void venus634_config_serial(void) +{ + m_serconf_t serconf; + + 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)); + + return; +} + + +void venus634_msg_type(void) +{ + m_output_t output; + + memset(&output,0,sizeof(m_output_t)); + output.msg_type = 0x02; /* binary msg */ + + venus634_send(M_OUTPUT,&output,sizeof(m_output_t)); + + return; +} + + +void venus634_rate(void) +{ + m_rate_t rate; + + memset(&rate,0,sizeof(m_rate_t)); + rate.rate = 20; + //rate.rate = 0; /* ram */ + + venus634_send(M_RATE,&rate,sizeof(m_rate_t)); + + return; +} + +void venus634_waas(void) +{ + m_waas_t waas; + + memset(&waas,0,sizeof(m_waas_t)); + waas.enable = 1; + waas.attributes = 0; /* ram */ + + venus634_send(M_WAAS,&waas,sizeof(m_waas_t)); + + return; +} + + +void venus634_navmode(void) +{ + m_navmode_t navmode; + + memset(&navmode,0,sizeof(m_navmode_t)); + navmode.navmode = 0; /* car */ + navmode.attributes = 0; /* ram */ + + venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t)); + + return; +} + + +void venus634_nema_interval(void) +{ + m_nemainterval_t nemainterval; + + memset(&nemainterval,0,sizeof(m_nemainterval_t)); + nemainterval.gga = 1; + nemainterval.gsa = 1; + nemainterval.gsv = 1; + nemainterval.gll = 1; + nemainterval.rmc = 1; + nemainterval.vtg = 1; + nemainterval.zda = 1; + + nemainterval.attributes = 1; /* ram flash */ + + venus634_send(M_NEMAINTERVAL,&nemainterval,sizeof(m_nemainterval_t)); + + return; +} + +int8_t recv_cb(uint8_t byte) +{ + uint16_t i; + + /* bytes 0 and 1 are start bytes */ + if (rxframe.cur_len == 0) { + if (byte != START1) { + debug_printf("bad start1 %.2X\n", byte); + goto reset_buf; + } + } + else if (rxframe.cur_len == 1) { + if (byte != START2) { + debug_printf("bad start2 %.2X\n", byte); + goto reset_buf; + } + } + /* bytes 2 and 3 are the length of frame in network order */ + else if (rxframe.cur_len == 2) { + rxframe.len = (uint16_t)byte << 8; + } + else if (rxframe.cur_len == 3) { + rxframe.len |= (uint16_t)byte; + if (rxframe.len > MAX_LEN) { + debug_printf("bad len %d\n", rxframe.len); + goto reset_buf; + } + } + /* next bytes are data (the 4 below is the size of header) */ + else if ((rxframe.cur_len - 4) < rxframe.len) { + rxframe.data[rxframe.cur_len - 4] = byte; + } + /* then it's the crc */ + else if ((rxframe.cur_len - 4) == rxframe.len) { + uint8_t crc = 0; + + for (i = 0; i < rxframe.len; i++) + crc ^= rxframe.data[i]; + if (byte != crc) { + debug_printf("invalid crc\n"); + goto reset_buf; + } + + } + /* and the last 2 bytes are stop bytes */ + else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) { + if (byte != END1) { + debug_printf("bad end1 %.2X\n", byte); + goto reset_buf; + } + } + else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) { + if (byte != END2) { + debug_printf("bad end2 %.2X\n", byte); + goto reset_buf; + } + debug_printf("valid frame received\n"); + rxframe.cur_len = 0; + return 0; + } + else /* should not happen */ + goto reset_buf; + + rxframe.cur_len ++; + return 1; + + reset_buf: + rxframe.cur_len = 0; + return 1; +} + +int recv_msg(void) +{ + int ret; + int16_t c; + + while (1) { + /* XXX use select for timeout */ + c = uart_recv(GPS_UART); + ret = recv_cb(c); + if (ret == 0) + return 0; + } +} + +int wait_ack(int msg_type) +{ + int ret; + + while (1) { + ret = recv_msg(); + if (ret < 0) + return -1; + + /* retry if it's not the expected message */ + if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84) + continue; + if (rxframe.data[1] != msg_type) + continue; + + if (rxframe.data[0] == 0x83) + printf("ACK\n"); + else if (rxframe.data[0] == 0x84) + printf("NACK\n"); + else + printf("ZARB\n"); + break; + } + + return 0; +} + +static int decode_gps_pos(uint8_t *buf, uint16_t len) +{ + struct gps_pos *pos = (struct gps_pos *)buf; + + if (len != sizeof(*pos)) + return -1; + + gps_pos.gps_week = ntohs(pos->gps_week); + gps_pos.tow = ntohl(pos->tow); + + gps_pos.latitude = ntohl(pos->latitude); + gps_pos.longitude = ntohl(pos->longitude); + gps_pos.altitude = ntohl(pos->altitude); + + gps_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); + + gps_pos.ecef_vx = ntohl(pos->ecef_vx); + gps_pos.ecef_vy = ntohl(pos->ecef_vy); + gps_pos.ecef_vz = ntohl(pos->ecef_vz); + + return 0; +} + +/* display current GPS position stored in the global variable */ +static void display_gps(void) +{ + printf("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("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n", + gps_pos.latitude, + gps_pos.longitude, + gps_pos.altitude); + + printf("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("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("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.); +} + +static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg) +{ + int16_t c; + int ret; + + (void)cm; + (void)tim; + (void)arg; + + while (1) { + c = uart_recv_nowait(GPS_UART); + if (c < 0) /* no more char */ + goto resched; + + ret = recv_cb(c); + if (ret == 0) { + decode_gps_pos(rxframe.data, rxframe.len); + if (0) + display_gps(); + } + } + + resched: + callout_schedule(cm, tim, 2); +} + +static void venus634_configure(void) +{ + /* ask the GPS to reset */ + printf("init..."); + 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("binmode..."); + venus634_msg_type(); + wait_ack(M_OUTPUT); + + printf("waas..."); + venus634_waas(); + wait_ack(M_WAAS); + + printf("rate..."); + venus634_rate(); + wait_ack(M_RATE); + + printf("GPS configuration done !\n"); +} + +/* + https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf +*/ + +int gps_venus_init(void) +{ + venus634_configure(); + + callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO); + callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */ + + return 0; +} + +static uint8_t find_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name, struct fat_dir_entry_struct* dir_entry) +{ + (void)fs; + + while(fat_read_dir(dd, dir_entry)) + { + if(strcmp(dir_entry->long_name, name) == 0) + { + fat_reset_dir(dd); + return 1; + } + } + + return 0; +} + +static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name) +{ + struct fat_dir_entry_struct file_entry; + if(!find_file_in_dir(fs, dd, name, &file_entry)) + return 0; + + return fat_open_file(fs, &file_entry); +} + +static struct fat_file_struct *open_log_file(void) +{ + struct fat_file_struct *fd; + struct fat_fs_struct *fs; + struct partition_struct *partition ; + struct fat_dir_struct *dd; + struct fat_dir_entry_struct directory; + struct fat_dir_entry_struct file_entry; + int16_t i = 0; + char name[16]; + + /* setup sd card slot */ + if (!sd_raw_init()) { +#if SD_DEBUG + printf_P(PSTR("MMC/SD initialization failed\n")); +#endif + return NULL; + } + + /* open first partition */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, +#if SD_RAW_WRITE_SUPPORT + sd_raw_write, sd_raw_write_interval, +#else + 0, 0, +#endif + 0); + + if (!partition) { + /* If the partition did not open, assume the storage device + * is a "superfloppy", i.e. has no MBR. + */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, +#if SD_RAW_WRITE_SUPPORT + sd_raw_write, + sd_raw_write_interval, +#else + 0, + 0, +#endif + -1); + if (!partition) { +#if SD_DEBUG + printf_P(PSTR("opening partition failed\n")); +#endif + return NULL; + } + } + + /* open file system */ + fs = fat_open(partition); + if (!fs) { +#if SD_DEBUG + printf_P(PSTR("opening filesystem failed\n")); +#endif + return NULL; + } + + /* open root directory */ + fat_get_dir_entry_of_path(fs, "/", &directory); + dd = fat_open_dir(fs, &directory); + if (!dd) { +#if SD_DEBUG + printf_P(PSTR("opening root directory failed\n")); +#endif + return NULL; + } + + /* print some card information as a boot message */ + //print_disk_info(fs); + + printf("choose log file name\n"); + while (1) { + snprintf(name, sizeof(name), "log%.4d", i++); + if (!find_file_in_dir(fs, dd, name, &file_entry)) + break; + } + + printf("create log file %s\n", name); + if (!fat_create_file(dd, name, &file_entry)) { + printf_P(PSTR("error creating file: ")); + } + + fd = open_file_in_dir(fs, dd, name); + if (!fd) { + printf_P(PSTR("error opening ")); + return NULL; + } + + return fd; +} + +int gps_loop(void) +{ + struct fat_file_struct *fd = NULL; + uint32_t ms; + uint8_t flags; + int16_t len; + char buf[128]; + + if (1) { + fd = open_log_file(); + if (fd == NULL) + printf("open log failed\r\n"); + } + + while (1) { + + IRQ_LOCK(flags); + ms = global_ms; + IRQ_UNLOCK(flags); + + if (fd != NULL) { + + /* 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 (fat_write_file(fd, (unsigned char *)buf, len) != len) { + printf_P(PSTR("error writing to file\n")); + return -1; + } + } + } + + return 0; +} diff --git a/gps_venus.h b/gps_venus.h new file mode 100644 index 0000000..18327c8 --- /dev/null +++ b/gps_venus.h @@ -0,0 +1,40 @@ +#ifndef _GPS_VENUS_H +#define _GPS_VENUS_H + +#include + +/* A GPS position structure. It also contains some information about the number + * of seen satellites, the message ID, the date, .. */ +struct gps_pos { + uint8_t msg_id; /* */ + uint8_t mode; + uint8_t sv_num; + uint16_t gps_week; + uint32_t tow; + + int32_t latitude; + int32_t longitude; + uint32_t altitude; + + uint32_t sea_altitude; + + uint16_t gdop; + uint16_t pdop; + uint16_t hdop; + uint16_t vdop; + uint16_t tdop; + + int32_t ecef_x; + int32_t ecef_y; + int32_t ecef_z; + + int32_t ecef_vx; + int32_t ecef_vy; + int32_t ecef_vz; + +} __attribute__ ((packed)); + +int gps_venus_init(void); +int gps_loop(void); + +#endif diff --git a/main.c b/main.c index 91ee7b7..454b54b 100644 --- a/main.c +++ b/main.c @@ -53,6 +53,7 @@ #include #include "eeprom_config.h" +#include "gps_venus.h" #include "main.h" struct imuboard imuboard; @@ -169,9 +170,11 @@ int main(void) printf_P(PSTR("\r\n")); rdline_newline(&imuboard.rdl, imuboard.prompt); - //sd_main(); - imu_loop(); + //sd_main(); + //imu_loop(); + gps_venus_init(); + gps_loop(); while (1) { cmdline_poll(); diff --git a/main.h b/main.h index ed8f435..d0e7b65 100644 --- a/main.h +++ b/main.h @@ -58,6 +58,7 @@ /* highest priority */ #define LED_PRIO 160 #define TIME_PRIO 140 +#define GPS_PRIO 80 #define LOW_PRIO 60 /* lowest priority */