--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <inttypes.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.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;
+}
git.droids-corp.org / fpv.git / commitdiff