+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <callout.h>
+#include <uart.h>
+
+#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;
+}
git.droids-corp.org / protos / imu.git / commitdiff