From d3d17f04620b3216af0279e24d4ce326955707e5 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 5 May 2015 19:19:05 +0200 Subject: [PATCH] add support of new gps (ubx) --- imuboard/Makefile | 10 + imuboard/gps_ubx.c | 838 +++++++++++++++++++++++++++++++++++++++++ imuboard/gps_ubx.h | 6 + imuboard/imu.c | 1 + imuboard/main.c | 21 ++ imuboard/uart_config.h | 2 +- 6 files changed, 877 insertions(+), 1 deletion(-) create mode 100644 imuboard/gps_ubx.c create mode 100644 imuboard/gps_ubx.h diff --git a/imuboard/Makefile b/imuboard/Makefile index cdc4913..ad4d255 100644 --- a/imuboard/Makefile +++ b/imuboard/Makefile @@ -2,6 +2,9 @@ TARGET = main AVERSIVE_DIR ?= ../.. +#GPS_VENUS=1 +GPS_UBX=1 + # List C source files here. (C dependencies are automatically generated.) SRC = $(TARGET).c SRC += commands.c @@ -18,7 +21,14 @@ SRC += sd_main.c SRC += partition.c SRC += sd_raw.c SRC += gps.c +ifeq ($(GPS_VENUS), 1) SRC += gps_venus.c +CFLAGS += -DGPS_VENUS +endif +ifeq ($(GPS_UBX), 1) +SRC += gps_ubx.c +CFLAGS += -DGPS_UBX +endif SRC += sd_log.c SRC += i2c_protocol.c diff --git a/imuboard/gps_ubx.c b/imuboard/gps_ubx.c new file mode 100644 index 0000000..379e2f3 --- /dev/null +++ b/imuboard/gps_ubx.c @@ -0,0 +1,838 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "main.h" +#include "gps.h" +#include "gps_ubx.h" + +#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ +#define letoh16(x) (x) +#define letoh32(x) (x) +#define letoh64(x) (x) +#define htole16(x) (x) +#define htole32(x) (x) +#define htole64(x) (x) +#else +#error todo +#endif + +/* 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(fmt, ##__VA_ARGS__) + +#define GPS_UART 0 +#define GPS_PERIOD_MS 2 + +#define START1 0xB5 +#define START2 0x62 + +struct gps_ubx_cksum { + uint8_t ck_a; + uint8_t ck_b; +}; + +#define MAX_LEN 0x200 +struct gps_frame { + uint8_t class; + uint8_t id; + uint16_t len; + uint16_t cur_len; + uint8_t data[MAX_LEN]; +}; + +static struct gps_frame rxframe; +static struct gps_ubx_cksum rxcksum; +static struct callout gps_timer; + +/* Navigation Results: Position, Speed, Time, Acc, Heading, DOP, SVs + * used */ +#define GPS_UBX_CLASS_NAV 0x01 + +#define GPS_UBX_ID_NAV_POSECEF 0x01 +#define GPS_UBX_ID_NAV_POSLLH 0x02 +#define GPS_UBX_ID_NAV_STATUS 0x03 +#define GPS_UBX_ID_NAV_DOP 0x04 +#define GPS_UBX_ID_NAV_SOL 0x06 +#define GPS_UBX_ID_NAV_VELECEF 0x11 +#define GPS_UBX_ID_NAV_VELNED 0x12 +#define GPS_UBX_ID_NAV_TIMEGPS 0x20 +#define GPS_UBX_ID_NAV_TIMEUTC 0x21 +#define GPS_UBX_ID_NAV_CLOCK 0x22 +#define GPS_UBX_ID_NAV_SVINFO 0x30 +#define GPS_UBX_ID_NAV_DGPS 0x31 +#define GPS_UBX_ID_NAV_SBAS 0x32 +#define GPS_UBX_ID_NAV_EKFSTATUS 0x40 +#define GPS_UBX_ID_NAV_AOPSTATUS 0x60 + +/* Receiver Manager Messages: Satellite Status, RTC Status */ +#define GPS_UBX_CLASS_RXM 0x02 + +#define GPS_UBX_ID_RXM_ALM 0x30 +#define GPS_UBX_ID_RXM_EPH 0x31 +#define GPS_UBX_ID_RXM_PMREQ 0x41 +#define GPS_UBX_ID_RXM_RAW 0x10 +#define GPS_UBX_ID_RXM_SFRB 0x11 +#define GPS_UBX_ID_RXM_SVSI 0x20 + +/* Information Messages: Printf-Style Messages, with IDs such as Error, Warning, + * Notice */ +#define GPS_UBX_CLASS_INF 0x04 + +/* Ack/Nack Messages: as replies to CFG Input Messages */ +#define GPS_UBX_CLASS_ACK 0x05 + +/* Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set + * Baud Rate, etc. */ +#define GPS_UBX_CLASS_CFG 0x06 + +#define GPS_UBX_ID_CFG_MSG 0x01 +#define GPS_UBX_ID_CFG_RATE 0x08 +#define GPS_UBX_ID_CFG_SBAS 0x16 + +/* Monitoring Messages: Comunication Status, CPU Load, Stack Usage, Task + * Status */ +#define GPS_UBX_CLASS_MON 0x0A + +/* AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data + * input */ +#define GPS_UBX_CLASS_AID 0x0B + +/* Timing Messages: Timepulse Output, Timemark Results */ +#define GPS_UBX_CLASS_TIM 0x0D + +#define GPS_UBX_CLASS_NMEA_STD 0xF0 +#define GPS_UBX_CLASS_NMEA_UBX 0xF1 + +#define GPS_UBX_ID_NMEA_GGA 0x00 +#define GPS_UBX_ID_NMEA_GLL 0x01 +#define GPS_UBX_ID_NMEA_GSA 0x02 +#define GPS_UBX_ID_NMEA_GSV 0x03 +#define GPS_UBX_ID_NMEA_RMC 0x04 +#define GPS_UBX_ID_NMEA_VTG 0x05 + +/********************* NAV class */ + +/* Position Solution in ECEF */ +struct gps_ubx_posecef { + uint32_t iTOW; /* ms - GPS Millisecond Time of Week */ + int32_t ecefX; /* cm - ECEF X coordinate */ + int32_t ecefY; /* cm - ECEF Y coordinate */ + int32_t ecefZ; /* cm - ECEF Z coordinate */ + uint32_t pAcc; /* cm - Position Accuracy Estimate */ +}; + +/* Geodetic Position Solution */ +struct gps_ubx_posllh { + uint32_t iTOW; /* ms - GPS Millisecond Time of Week */ + int32_t lon; /* 1e-7 deg - Longitude */ + int32_t lat; /* 1e-7 deg - Latitude */ + int32_t height; /* mm - Height above ellipsoid */ + int32_t heightMSL; /* mm - Height above mean sea level */ + uint32_t hAcc; /* mm - Horizontal Accuracy Estimate */ + uint32_t vAcc; /* mm - Vertical Accuracy Estimate */ +}; + +/* Velocity solution in NED */ +struct gps_ubx_velned { + uint32_t iTOW; /* GPS Millisecond Time of Week */ + int32_t velN; /* NED north velocity */ + int32_t velE; /* NED east velocity */ + int32_t velD; /* NED down velocity */ + uint32_t speed; /* Speed (3-D) */ + uint32_t gSpeed; /* Ground Speed (2-D) */ + int32_t heading; /* eading of motion 2-D */ + uint32_t sAcc; /* Speed Accuracy Estimate */ + uint32_t cAcc; /* Course / Heading Accuracy Estimate */ +}; + +/* This message combines Position, velocity and time solution in ECEF, including + * accuracy figures */ +struct gps_ubx_nav_sol { + uint32_t iTOW; + int32_t fTOW; /* Fractional Nanoseconds remainder of rounded ms above, + * range -500000 .. 500000 */ + int16_t week; /* GPS week (GPS time) */ + uint8_t gpsFix; /* GPSfix Type, 0x00 = No Fix, 0x01 = Dead Reckoning only, + * 0x02 = 2D-Fix 0x03 = 3D-Fix, 0x04 = GPS + dead reckoning + * combined, 0x05 = Time only fix, 0x06..0xff: reserved */ + uint8_t flags; /* Fix Status Flags (see graphic below) */ + int32_t ecefX; /* cm - ECEF X coordinate */ + int32_t ecefY; /* cm - ECEF Y coordinate */ + int32_t ecefZ; /* cm - ECEF Z coordinate */ + uint32_t pAcc; /* cm - 3D Position Accuracy Estimate */ + int32_t ecefVX; /* cm/s - ECEF X velocity */ + int32_t ecefVY; /* cm/s - ECEF Y velocity */ + int32_t ecefVZ; /* cm/s - ECEF Z velocity */ + uint32_t sAcc; /* cm/s - Speed Accuracy Estimate */ + uint16_t pDOP; /* Position DOP */ + uint8_t reserved1; + uint8_t numSV; /* Number of SVs used in Nav Solution */ + uint32_t reserved2; +}; + +/********************* CFG class */ + +/* Get/Set Port Configuration for UART + * Several configurations can be concatenated to one input message. In + * this case the payload length can be a multiple of the normal length + * (see the other versions of CFG-PRT). Output messages from the module + * contain only one configuration unit. */ +struct gps_ubx_prt { + uint8_t portID; /* Port Identifier Number (= 1 or 2 for UART ports) */ + uint8_t reserved0; +#define GPS_UBX_PRT_TXREADY_EN_MASK 0x0001 +#define GPS_UBX_PRT_TXREADY_POL_MASK 0x0002 +#define GPS_UBX_PRT_TXREADY_PIN_MASK 0x007c +#define GPS_UBX_PRT_TXREADY_THRES_MASK 0xff80 + uint16_t txReady; /* reserved (Alwyas set to zero) up to Firmware 7. + * TX ready PIN configuration (since Firmware 7). */ +#define GPS_UBX_PRT_MODE_RESERVED1_MASK 0x00000010 +#define GPS_UBX_PRT_MODE_CHARLEN_MASK 0x000000c0 +#define GPS_UBX_PRT_MODE_CHARLEN_5BITS 0x00000000 +#define GPS_UBX_PRT_MODE_CHARLEN_6BITS 0x00000040 +#define GPS_UBX_PRT_MODE_CHARLEN_7BITS 0x00000080 +#define GPS_UBX_PRT_MODE_CHARLEN_8BITS 0x000000c0 +#define GPS_UBX_PRT_MODE_PARITY_MASK 0x00000e00 +#define GPS_UBX_PRT_MODE_PARITY_EVEN 0x00000000 +#define GPS_UBX_PRT_MODE_PARITY_ODD 0x00000200 +#define GPS_UBX_PRT_MODE_PARITY_NONE 0x00000800 +#define GPS_UBX_PRT_MODE_NSTOP_MASK 0x00003000 +#define GPS_UBX_PRT_MODE_NSTOP_1 0x00000000 +#define GPS_UBX_PRT_MODE_NSTOP_1_5 0x00001000 +#define GPS_UBX_PRT_MODE_NSTOP_2 0x00002000 +#define GPS_UBX_PRT_MODE_NSTOP_0_5 0x00003000 + uint32_t mode; /* A bit mask describing the UART mode */ + +#define GPS_UBX_PRT_PROTOMASK_UBX 0x0001 +#define GPS_UBX_PRT_PROTOMASK_NMEA 0x0002 + /* A mask describing which input protocols are active. Each bit + * of this mask is used for a protocol. Through that, multiple + * protocols can be defined on a single port. */ + uint16_t inProtoMask; + /* A mask describing which output protocols are active */ + uint16_t outProtoMask; +}; + + +/* Set/Get message rate configuration to/from the receiver. */ +struct gps_ubx_msg { + uint8_t class; + uint8_t id; + uint8_t rate; /* Send rate */ + +}; + +/* Configure the SBAS (Satellite-based augmentation systems) receiver subsystem + * (i.e. WAAS, EGNOS, MSAS). */ +struct gps_ubx_sbas { +#define GPS_UBX_SBAS_MODE_ENABLED 0x01 /* SBAS Enabled */ +#define GPS_UBX_SBAS_MODE_TEST 0x02 /* SBAS Testbed: Use data anyhow */ + uint8_t mode; /* bitfield */ +#define GPS_UBX_SBAS_USAGE_RANGE 0x01 /* Use SBAS GEOs as a ranging source */ +#define GPS_UBX_SBAS_USAGE_DIFFCORR 0x02 /* Use SBAS Differential Corrections */ +#define GPS_UBX_SBAS_USAGE_INTEGRITY 0x04 /* Use SBAS Integrity Information */ + uint8_t usage; /* bitfield */ + uint8_t maxSBAS; /* Maximum Number of SBAS prioritized tracking + * channels (valid range: 0 - 3) to use */ + uint8_t scanmode2; /* PRN152 -> PRN156 (bit 0 to 6) */ + uint32_t scanmode1; /* PRN120 -> PRN151 (bit 0 to 31) */ +}; + +/* The u-blox positioning technology supports navigation update rates higher or + * lower than 1 update per second. The calculation of the navigation solution + * will always be aligned to the top of a second. + * + * • The update rate has a direct influence on the power consumption. The more + * fixes that are required, the more CPU power and communication resources + * are required. + * • For most applications a 1 Hz update rate would be sufficient. */ +struct gps_ubx_rate { + uint16_t measRate; /* ms - Measurement Rate */ + uint16_t navRate; /* cycles - Navigation Rate, in number of measurement + cycles. On u-blox 5 and u-blox 6, this parameter + cannot be changed, and is always equals 1. */ + uint16_t timeRef; /* Alignment to reference time: 0 = UTC time, + * 1 = GPS time */ +}; + + + +/********************* ACK class */ + +/* When messages from the Class CFG are sent to the receiver, the +* receiver will send an Acknowledge (ACK-ACK) or a Not Acknowledge +* (ACK-NAK) message back to the sender, depending on whether or not the +* message was processed correctly. There is no ACK/NAK mechanism for +* message poll requests outside Class CFG. */ + +#define GPS_UBX_ID_NACK 0x00 +#define GPS_UBX_ID_ACK 0x01 + +struct gps_ubx_ack { + uint8_t class; /* GPS_UBX_CLASS_ACK */ + uint8_t id; /* GPS_UBX_ID_ACK or GPS_UBX_ID_NACK */ + uint8_t ack_class; + uint8_t ack_id; +}; + +/********************* RXM class */ + +struct gps_ubx_rxm_svsi { + int32_t iTOW; /* Measurement integer millisecond GPS time of week */ + int16_t week; /* Measurement GPS week number. */ + uint8_t numVis; /* Number of visible satellites */ + uint8_t numSV; /* Number of per-SV data blocks following */ + struct { + uint8_t svid; /* Satellite ID */ + uint8_t svFlag; /* Information Flags */ + int16_t azim; /* Azimuth */ + int8_t elev; /* Elevation */ + uint8_t age; /* Age of Almanach and Ephemeris */ + } sv[]; /* reapeated numSV times */ +}; + + + +static void gps_ubx_cksum_update(struct gps_ubx_cksum *cksum, uint8_t byte) +{ + cksum->ck_a = cksum->ck_a + byte; + cksum->ck_b = cksum->ck_b + cksum->ck_a; +} + +static void serial1_tx_cout(uint8_t c) +{ + debug_printf("%.2X ", c); + uart_send(GPS_UART, c); +} + + +void gps_ubx_send(uint8_t class, uint8_t id, void *payload, uint16_t len) +{ + struct gps_ubx_cksum cksum = { 0, 0 }; + uint16_t n; + uint8_t byte; + + debug_printf("SEND "); + + serial1_tx_cout(START1); + serial1_tx_cout(START2); + serial1_tx_cout(class); + gps_ubx_cksum_update(&cksum, class); + serial1_tx_cout(id); + gps_ubx_cksum_update(&cksum, id); + serial1_tx_cout(len & 0xff); + gps_ubx_cksum_update(&cksum, len & 0xff); + serial1_tx_cout((len >> 8) & 0xff); + gps_ubx_cksum_update(&cksum, (len >> 8) & 0xff); + /* payload */ + for (n = 0; n < len; n++) { + byte = *(uint8_t *)payload; + serial1_tx_cout(byte); + gps_ubx_cksum_update(&cksum, byte); + payload++; + } + /* checksum and tail */ + serial1_tx_cout(cksum.ck_a); + serial1_tx_cout(cksum.ck_b); + + debug_printf("\n"); +} + +/* frequency of messages */ +void gps_ubx_set_msg_rate(uint8_t class, uint8_t id, uint8_t rate) +{ + struct gps_ubx_msg msg_rate; + + memset(&msg_rate, 0, sizeof(msg_rate)); + msg_rate.class = class; + msg_rate.id = id; + msg_rate.rate = rate; + + gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_MSG, + &msg_rate, sizeof(msg_rate)); +} + +/* update rate */ +void gps_ubx_set_rate(uint16_t measRate, uint16_t navRate, uint16_t timeRef) +{ + struct gps_ubx_rate rate; + + memset(&rate, 0, sizeof(rate)); + rate.measRate = htole16(measRate); + rate.navRate = htole16(navRate); + rate.timeRef = htole16(timeRef); + + gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_RATE, + &rate, sizeof(rate)); +} + +/* set satellite-based augmentation system (differential gps) */ +void gps_ubx_set_sbas(uint8_t mode, uint8_t usage, uint8_t maxSBAS, + uint8_t scanmode2, uint32_t scanmode1) +{ + struct gps_ubx_sbas msg_sbas; + + memset(&msg_sbas, 0, sizeof(msg_sbas)); + msg_sbas.mode = mode; + msg_sbas.usage = usage; + msg_sbas.maxSBAS = maxSBAS; + msg_sbas.scanmode2 = scanmode2; + msg_sbas.scanmode1 = htole32(scanmode1); + + gps_ubx_send(GPS_UBX_CLASS_CFG, GPS_UBX_ID_CFG_SBAS, + &msg_sbas, sizeof(msg_sbas)); +} + +int8_t recv_cb(uint8_t byte) +{ + uint16_t i; + + /* bytes 0 and 1 are start bytes */ + if (rxframe.cur_len == 0) { + debug_printf("start1 %x\n", byte); + if (byte != START1) { + debug_printf("bad start1 %.2X\n", byte); + goto reset_buf; + } + } + else if (rxframe.cur_len == 1) { + debug_printf("start2 %x\n", byte); + if (byte != START2) { + debug_printf("bad start2 %.2X\n", byte); + goto reset_buf; + } + } + /* bytes 2 and 3 are the class and id */ + else if (rxframe.cur_len == 2) { + debug_printf("class %x\n", byte); + rxframe.class = byte; + } + else if (rxframe.cur_len == 3) { + debug_printf("id %x\n", byte); + rxframe.id = byte; + } + /* bytes 4 and 5 are the length of frame in little endian order */ + else if (rxframe.cur_len == 4) { + debug_printf("len_l %x\n", byte); + rxframe.len = (uint16_t)byte; + } + else if (rxframe.cur_len == 5) { + debug_printf("len_h %x\n", byte); + rxframe.len |= (uint16_t)byte << 8; + if (rxframe.len > MAX_LEN) { + debug_printf("bad len %d\n", rxframe.len); + goto reset_buf; + } + } + /* next bytes are data (the 6 below is the size of header) */ + else if ((rxframe.cur_len - 6) < rxframe.len) { + debug_printf("data %x\n", byte); + rxframe.data[rxframe.cur_len - 6] = byte; + } + /* then it's the cksum */ + else if ((rxframe.cur_len - 6) == rxframe.len) { + debug_printf("ck_a %x\n", byte); + rxcksum.ck_a = byte; + } + else if ((rxframe.cur_len - 6) == rxframe.len + 1) { + struct gps_ubx_cksum cksum = { 0, 0 }; + + debug_printf("ck_b %x\n", byte); + rxcksum.ck_b = byte; + + /* check crc */ + gps_ubx_cksum_update(&cksum, rxframe.class); + gps_ubx_cksum_update(&cksum, rxframe.id); + gps_ubx_cksum_update(&cksum, rxframe.len & 0xff); + gps_ubx_cksum_update(&cksum, rxframe.len >> 8); + for (i = 0; i < rxframe.len; i++) + gps_ubx_cksum_update(&cksum, rxframe.data[i]); + if (cksum.ck_a != rxcksum.ck_a || + cksum.ck_b != rxcksum.ck_b) { + debug_printf("invalid checksum %2.2x%2.2x != %2.2x%2.2x\n", + cksum.ck_a, cksum.ck_b, + rxcksum.ck_a, rxcksum.ck_a); + 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; + uint8_t irq_flags; + int16_t c; + uint16_t t1, t2, diff; + + IRQ_LOCK(irq_flags); + t1 = global_ms; + IRQ_UNLOCK(irq_flags); + + while (1) { + 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 (rxframe.data[1] != msg_type) + continue; + + if (rxframe.data[0] == 0x83) + printf_P(PSTR("ACK\n")); + else if (rxframe.data[0] == 0x84) + printf_P(PSTR("NACK\n")); + else + printf_P(PSTR("ZARB\n")); + break; + } + + return 0; +} + +static int decode_gps_posllh(const struct gps_frame *frame) +{ + const struct gps_ubx_posllh *posllh = + (const struct gps_ubx_posllh *)&frame->data; + int32_t height, heightMSL; + uint8_t irq_flags; + + //printf("%s\n", __FUNCTION__); + /* pos struct is in cm, convert */ + height = letoh32(posllh->height) / 10; + heightMSL = letoh32(posllh->heightMSL) / 10; + + IRQ_LOCK(irq_flags); + gps_pos.tow = letoh32(posllh->iTOW); + gps_pos.longitude = letoh32(posllh->lon); + gps_pos.latitude = letoh32(posllh->lat); + gps_pos.altitude = height; + gps_pos.sea_altitude = heightMSL; + IRQ_UNLOCK(irq_flags); + + return 0; +} + +static int decode_gps_posecef(const struct gps_frame *frame) +{ + const struct gps_ubx_posecef *posecef = + (const struct gps_ubx_posecef *)&frame->data; + uint8_t irq_flags; + + //printf("%s\n", __FUNCTION__); + IRQ_LOCK(irq_flags); + gps_pos.tow = letoh32(posecef->iTOW); + gps_pos.ecef_x = letoh32(posecef->ecefX); + gps_pos.ecef_y = letoh32(posecef->ecefY); + gps_pos.ecef_z = letoh32(posecef->ecefZ); + IRQ_UNLOCK(irq_flags); + + return 0; +} + +static int decode_gps_velned(const struct gps_frame *frame) +{ + const struct gps_ubx_velned *velned = + (const struct gps_ubx_velned *)&frame->data; + uint8_t irq_flags; + + //printf("%s\n", __FUNCTION__); + IRQ_LOCK(irq_flags); + gps_pos.tow = letoh32(velned->iTOW); + //gps_pos.speed = letoh32(velned->speed); //XXX add speed ? + IRQ_UNLOCK(irq_flags); + + return 0; +} + +static int decode_gps_nav_sol(const struct gps_frame *frame) +{ + const struct gps_ubx_nav_sol *nav_sol = + (const struct gps_ubx_nav_sol *)&frame->data; + uint8_t irq_flags; + + //printf("%s\n", __FUNCTION__); + IRQ_LOCK(irq_flags); + gps_pos.tow = letoh32(nav_sol->iTOW); + gps_pos.gps_week = letoh16(nav_sol->week); + gps_pos.mode = nav_sol->gpsFix; + gps_pos.sv_num = nav_sol->numSV; + gps_pos.ecef_x = letoh32(nav_sol->ecefX); + gps_pos.ecef_y = letoh32(nav_sol->ecefY); + gps_pos.ecef_z = letoh32(nav_sol->ecefZ); + gps_pos.ecef_vx = letoh32(nav_sol->ecefVX); + gps_pos.ecef_vy = letoh32(nav_sol->ecefVY); + gps_pos.ecef_vz = letoh32(nav_sol->ecefVZ); + IRQ_UNLOCK(irq_flags); + + return 0; +} + +static int decode_gps_rxm_svsi(const struct gps_frame *frame) +{ + const struct gps_ubx_rxm_svsi *rxm_svsi = + (const struct gps_ubx_rxm_svsi *)&frame->data; + uint8_t irq_flags; + + //printf("%s\n", __FUNCTION__); + IRQ_LOCK(irq_flags); + gps_pos.tow = letoh32(rxm_svsi->iTOW); + gps_pos.gps_week = letoh16(rxm_svsi->week); + gps_pos.sv_num = rxm_svsi->numVis; + IRQ_UNLOCK(irq_flags); + + return 0; +} + +static int decode_gps(const struct gps_frame *frame) +{ + int ret = -1; + + switch (frame->class) { + case GPS_UBX_CLASS_NAV: + switch (frame->id) { + case GPS_UBX_ID_NAV_POSLLH: + ret = decode_gps_posllh(frame); + break; + case GPS_UBX_ID_NAV_POSECEF: + ret = decode_gps_posecef(frame); + break; + case GPS_UBX_ID_NAV_VELNED: + ret = decode_gps_velned(frame); + break; + case GPS_UBX_ID_NAV_SOL: + ret = decode_gps_nav_sol(frame); + break; + default: + break; + } + break; + case GPS_UBX_CLASS_RXM: + switch (frame->id) { + case GPS_UBX_ID_RXM_SVSI: + ret = decode_gps_rxm_svsi(frame); + break; + default: + break; + } + default: + break; + } + + if (ret < 0) + debug_printf("error processing message class=%x id=%x\n", + frame->class, frame->id); + + return ret; +#if 0 + /* fix endianness inside the frame */ + pos->gps_week = ntohs(pos->gps_week); + pos->tow = ntohl(pos->tow); + + pos->latitude = ntohl(pos->latitude); + pos->longitude = ntohl(pos->longitude); + pos->altitude = ntohl(pos->altitude); + + pos->sea_altitude = ntohl(pos->sea_altitude); + + 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); + + 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); + memcpy(&gps_pos, pos, sizeof(gps_pos)); + IRQ_UNLOCK(irq_flags); +#endif + + return 0; +} + +/* 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, + 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_ubx_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; + 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 */ + break; + + ret = recv_cb(c); + if (ret == 0) { + decode_gps(&rxframe); + if (0) + display_gps(); + + last_valid = ms; + is_valid = 1; + } + } + + 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); +} + +int gps_ubx_init(void) +{ + printf_P(PSTR("gps_ubx_init\n")); + + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GGA, 0); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GLL, 0); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GSA, 0); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_GSV, 0); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_RMC, 0); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NMEA_STD, GPS_UBX_ID_NMEA_VTG, 0); + + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_POSLLH, 1); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_STATUS, 1); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_SOL, 1); + gps_ubx_set_msg_rate(GPS_UBX_CLASS_NAV, GPS_UBX_ID_NAV_VELNED, 1); + //gps_ubx_set_msg_rate(GPS_UBX_CLASS_RXM, GPS_UBX_ID_RXM_SVSI, 1); + + /* use EGNOS */ + gps_ubx_set_sbas(GPS_UBX_SBAS_MODE_ENABLED, + GPS_UBX_SBAS_USAGE_RANGE | GPS_UBX_SBAS_USAGE_DIFFCORR | + GPS_UBX_SBAS_USAGE_INTEGRITY, 3, 0x0, 0x851); + + gps_ubx_set_rate(500, 1, 1); /* 2hz */ + + printf_P(PSTR("GPS configuration done !\n")); + + callout_init(&gps_timer, gps_ubx_cb, NULL, GPS_PRIO); + callout_schedule(&imuboard.intr_cm, &gps_timer, GPS_PERIOD_MS); /* every 2ms */ + + return 0; +} diff --git a/imuboard/gps_ubx.h b/imuboard/gps_ubx.h new file mode 100644 index 0000000..f6e4fd9 --- /dev/null +++ b/imuboard/gps_ubx.h @@ -0,0 +1,6 @@ +#ifndef _GPS_UBX_H +#define _GPS_UBX_H + +int gps_ubx_init(void); + +#endif diff --git a/imuboard/imu.c b/imuboard/imu.c index 00a61dd..1e8f3cc 100644 --- a/imuboard/imu.c +++ b/imuboard/imu.c @@ -86,6 +86,7 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) void imu_init(void) { + printf_P(PSTR("imu_init\n")); mpu6050_init(); callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO); diff --git a/imuboard/main.c b/imuboard/main.c index 25a1662..2de634a 100644 --- a/imuboard/main.c +++ b/imuboard/main.c @@ -55,7 +55,12 @@ #include "eeprom_config.h" #include "gps.h" +#ifdef GPS_VENUS #include "gps_venus.h" +#endif +#ifdef GPS_UBX +#include "gps_ubx.h" +#endif #include "sd_log.h" #include "../common/i2c_commands.h" #include "i2c_protocol.h" @@ -154,6 +159,17 @@ int main(void) callout_mgr_init(&imuboard.intr_cm, get_time_ms); +#if 0 + sei(); //XXX + printf_P(PSTR("starting...\r\n")); + while (1) { + int16_t c; + c = uart_recv_nowait(0); + if (c >= 0) + printf("%x\n", (uint8_t)c); + } +#endif + cmdline_init(); /* LOGS */ error_register_emerg(mylog); @@ -181,7 +197,12 @@ int main(void) rdline_newline(&imuboard.rdl, imuboard.prompt); imu_init(); +#ifdef GPS_VENUS gps_venus_init(); +#endif +#ifdef GPS_UBX + gps_ubx_init(); +#endif imuboard.flags |= IMUBOARD_F_BOOT_OK; diff --git a/imuboard/uart_config.h b/imuboard/uart_config.h index 3785e57..34ecd1c 100644 --- a/imuboard/uart_config.h +++ b/imuboard/uart_config.h @@ -39,7 +39,7 @@ /* enable uart0 interrupts if == 1, disable if == 0 */ #define UART0_INTERRUPT_ENABLED 1 -#define UART0_BAUDRATE 57600 +#define UART0_BAUDRATE 38400 /* * if you enable this, the maximum baudrate you can reach is -- 2.39.5