OP-1370 - Add "management tasks" support for UBX, used for self configuration.

Initial state machine asking for gps version. Configuration messages definition

flight/modules/GPS/GPS.c

@@ -43,6 +43,7 @@
 #include "auxmagsensor.h"
 #include "WorldMagModel.h"
 #include "CoordinateConversions.h"
+#include <pios_com.h>
 
 #include "GPS.h"
 #include "NMEA.h"
@@ -228,7 +229,17 @@ static void gpsTask(__attribute__((unused)) void *parameters)
     // Loop forever
     while (1) {
         uint8_t c;
-
+#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
+        if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
+            char *buffer   = 0;
+            uint16_t count = 0;
+            ubx_run_management_tasks(&buffer, &count);
+            // Something to send?
+            if (count) {
+                PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
+            }
+        }
+#endif
         // This blocks the task until there is something on the buffer
         while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) {
             int res;

flight/modules/GPS/UBX.c

@@ -33,6 +33,7 @@
 #include "CoordinateConversions.h"
 #include "pios_math.h"
 #include <pios_helpers.h>
+#include <pios_delay.h>
 #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
 #include "inc/UBX.h"
 #include "inc/GPS.h"
@@ -43,12 +44,16 @@ static float mag_bias[3] = { 0, 0, 0 };
 static float mag_transform[3][3] = {
     { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
 };
+
 static bool useMag = false;
 static GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
 static struct UBX_ACK_ACK last_ack;
 static struct UBX_ACK_NAK last_nak;
 static bool usePvt = false;
 static uint32_t lastPvtTime = 0;
+
+
+// parse table item
 typedef struct {
     uint8_t msgClass;
     uint8_t msgID;
@@ -95,6 +100,21 @@ const ubx_message_handler ubx_handler_table[] = {
     { .msgClass = UBX_CLASS_MON,     .msgID = UBX_ID_MON_VER,     .handler = &parse_ubx_mon_ver     },
 };
 #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
+
+// COnfiguration support
+typedef enum {
+    INIT_STEP_ASK_VER   = 0,
+    INIT_STEP_WAIT_VER  = 1,
+    INIT_STEP_CONFIGURE = 2,
+    INIT_STEP_DONE = 3,
+} initSteps;
+
+static initSteps currentConfigurationStep;
+// timestamp of last operation
+static uint32_t lastStepTimestampRaw = 0;
+// detected hw version
+static int32_t hwVersion = -1;
+
 // If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC
 #define UBX_PVT_TIMEOUT (1000)
 // parse incoming character stream for messages in UBX binary format
@@ -445,10 +465,11 @@ static void parse_ubx_ack_nak(struct UBXPacket *ubx, __attribute__((unused)) GPS
 static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
 {
     struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
-    uint32_t hwVersion = atoi(mon_ver->hwVersion);
 
-    sensorType = (hwVersion >= 8000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
+    hwVersion  = atoi(mon_ver->hwVersion);
-                 ((hwVersion >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
+
+    sensorType = (hwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
+                 ((hwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
 }
 
 
@@ -547,4 +568,62 @@ void load_mag_settings()
         useMag = true;
     }
 }
+
+UBXSentPacket_t working_packet;
+void append_checksum(UBXSentPacket_t *packet)
+{
+    uint8_t i;
+    uint8_t ck_a = 0;
+    uint8_t ck_b = 0;
+    uint16_t len = packet->message.header.len + sizeof(UBXSentHeader_t);
+
+    for (i = 2; i < len; i++) {
+        ck_a += packet->buffer[i];
+        ck_b += ck_a;
+    }
+
+    packet->buffer[len]     = ck_a;
+    packet->buffer[len + 1] = ck_b;
+}
+void prepare_packet(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t len)
+{
+    packet->message.header.prolog[0] = UBX_SYNC1;
+    packet->message.header.prolog[1] = UBX_SYNC2;
+    packet->message.header.class     = classID;
+    packet->message.header.id  = messageID;
+    packet->message.header.len = len;
+    append_checksum(packet);
+}
+
+void build_request(UBXSentPacket_t *packet, uint8_t classID, uint8_t messageID, uint16_t *count)
+{
+    prepare_packet(packet, classID, messageID, 0);
+    *count = packet->message.header.len + sizeof(UBXSentHeader_t) + 2;
+}
+
+void ubx_run_management_tasks(char * *buffer, uint16_t *count)
+{
+    uint32_t elapsed = PIOS_DELAY_DiffuS(lastStepTimestampRaw);
+
+    switch (currentConfigurationStep) {
+    case INIT_STEP_ASK_VER:
+        lastStepTimestampRaw     = PIOS_DELAY_GetRaw();
+        build_request(&working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, count);
+        *buffer = (char *)working_packet.buffer;
+        currentConfigurationStep = INIT_STEP_WAIT_VER;
+        break;
+    case INIT_STEP_WAIT_VER:
+        if (hwVersion > 0) {
+            currentConfigurationStep = INIT_STEP_CONFIGURE;
+        } else if (elapsed > UBX_REPLY_TIMEOUT) {
+            currentConfigurationStep = INIT_STEP_ASK_VER;
+        }
+        return;
+
+    case INIT_STEP_CONFIGURE:
+    case INIT_STEP_DONE:
+        break;
+    }
+}
+
 #endif // PIOS_INCLUDE_GPS_UBX_PARSER

flight/modules/GPS/inc/UBX.h

@@ -36,6 +36,7 @@
 #include "auxmagsensor.h"
 #include "GPS.h"
 
+#define UBX_REPLY_TIMEOUT (500 * 1000)
 
 #define UBX_SYNC1         0xb5 // UBX protocol synchronization characters
 #define UBX_SYNC2         0x62
@@ -46,6 +47,7 @@
 typedef enum {
     UBX_CLASS_NAV     = 0x01,
     UBX_CLASS_ACK     = 0x05,
+    UBX_CLASS_CFG     = 0x06,
     UBX_CLASS_MON     = 0x0A,
     UBX_CLASS_OP_CUST = 0x99,
 } ubx_class;
@@ -71,6 +73,13 @@ typedef enum {
     UBX_ID_MON_VER = 0x04,
 } ubx_class_mon_id;
 
+
+typedef enum {
+    UBX_ID_CFG_NAV5 = 0x24,
+    UBX_ID_CFG_RATE = 0x08,
+    UBX_ID_CFG_MSG  = 0x01,
+} ubx_class_cfg_id;
+
 typedef enum {
     UBX_ID_ACK_ACK = 0x01,
     UBX_ID_ACK_NAK = 0x00,
@@ -281,7 +290,7 @@ struct UBX_ACK_NAK {
 };
 
 // MON message Class
-#define UBX_MON_MAX_EXT 1
+#define UBX_MON_MAX_EXT 5
 struct UBX_MON_VER {
     char swVersion[30];
     char hwVersion[10];
@@ -343,8 +352,66 @@ struct UBXPacket {
     UBXPayload payload;
 };
 
+// Sent messages for configuration support
+
+typedef struct {
+    uint16_t mask;
+    uint8_t  dynModel;
+    uint8_t  fixMode;
+    int32_t  fixedAlt;
+    uint32_t fixedAltVar;
+    int8_t   minElev;
+    uint8_t  drLimit;
+    uint16_t pDop;
+    uint16_t tDop;
+    uint16_t pAcc;
+    uint16_t tAcc;
+    uint8_t  staticHoldThresh;
+    uint8_t  dgpsTimeOut;
+    uint8_t  cnoThreshNumSVs;
+    uint8_t  cnoThresh;
+    uint16_t reserved2;
+    uint32_t reserved3;
+    uint32_t reserved4;
+} ubx_cfg_nav5_t;
+
+typedef struct {
+    uint16_t measRate;
+    uint16_t navRate;
+    uint16_t timeRef;
+} ubx_cfg_rate_t;
+
+typedef struct {
+    uint8_t msgClass;
+    uint8_t msgID;
+    uint8_t rate;
+} ubx_cfg_msg_t;
+
+typedef struct {
+    uint8_t  prolog[2];
+    uint8_t  class;
+    uint8_t  id;
+    uint16_t len;
+} UBXSentHeader_t;
+
+typedef union {
+    uint8_t buffer[0];
+    struct {
+        UBXSentHeader_t header;
+        union {
+            ubx_cfg_nav5_t cfg_nav5;
+            ubx_cfg_rate_t cfg_rate;
+            ubx_cfg_msg_t  cfg_msg;
+        } payload;
+        uint8_t resvd[2]; // added space for checksum bytes
+    } message;
+} UBXSentPacket_t;
+
+
 bool checksum_ubx_message(struct UBXPacket *);
 uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
+
+void ubx_run_management_tasks(char * *buffer, uint16_t *count);
 int parse_ubx_stream(uint8_t, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
 void load_mag_settings();