From c232985baa522237aff45bdf69a5b02200c607c4 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 15:14:51 +0200 Subject: [PATCH] OP-1477 - initial framer + autoconfiguration and some fixes - Added a framing code for gps data to be able to interleave mag sentences. - Fix fields definition, set structures used to send UBX data as packed to fix issues with alignments, - change axis orientation to match revo when both boards conector sides are are aligned --- flight/modules/gpsp/gpsdsysmod.c | 265 +++++++++++++++++++++++++++---- 1 file changed, 231 insertions(+), 34 deletions(-) diff --git a/flight/modules/gpsp/gpsdsysmod.c b/flight/modules/gpsp/gpsdsysmod.c index 17e0c5ca6..9997af14d 100644 --- a/flight/modules/gpsp/gpsdsysmod.c +++ b/flight/modules/gpsp/gpsdsysmod.c @@ -40,6 +40,7 @@ #include #include +#include // private includes #include "inc/gpsdsysmod.h" #include @@ -49,6 +50,7 @@ #include SystemStatsData systemStats; +extern uintptr_t flash_id; #define DEBUG_THIS_FILE extern uint32_t pios_com_main_id; // #define PIOS_COM_DEBUG pios_com_main_id @@ -58,17 +60,19 @@ extern uint32_t pios_com_main_id; #define DEBUG_MSG(format, ...) #endif +static bool getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *lenght); +extern struct pios_flash_driver pios_jedec_flash_driver; +extern uintptr_t flash_id; // Private constants #define SYSTEM_UPDATE_PERIOD_MS 2 #define STACK_SIZE_BYTES 450 - +#define MAG_RATE 50 +#define STAT_RATE 1 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define BUFFER_SIZE 64 +#define BUFFER_SIZE 200 uint8_t buffer[BUFFER_SIZE]; -char outbuf[40]; - // Private types // Private variables @@ -77,12 +81,89 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C static bool mallocFailed; static void ReadGPS(); static void ReadMag(); - +static void SetupGPS(); // Private functions static void updateStats(); static void gpspSystemTask(void *parameters); +const char cfg_settings[] = + // cfg-prt I2C. In UBX+RTCM, Out UBX, Slave Addr 0x42 + "\xB5\x62\x06\x00\x14\x00\x00\x00\x00\x00\x84\x00\x00\x00\x00\x00\x00\x00\x07\x00\x01\x00\x00\x00\x00\x00\xA6\xC6" + // cfg-msg: nav-pvt rate 1 + "\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51" + // cfg-msg: nav-svinfo rate 10 + "\xB5\x62\x06\x01\x03\x00\x01\x30\x0A\x45\xAC" + // CFG-RATE meas period 100ms, nav rate 1 + "\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x01\x00\x7A\x12"; + +typedef struct { + uint8_t syn1; + uint8_t syn2; + uint8_t class; + uint8_t id; + uint16_t len; +} __attribute__((packed)) UBXHeader_t; + +typedef struct { + uint8_t chk1; + uint8_t chk2; +} __attribute__((packed)) UBXFooter_t; + +typedef union { + uint8_t bynarystream[0]; + struct { + UBXHeader_t header; + uint8_t payload[0]; + } packet; +} UBXPacket_t; + + +typedef struct { + int16_t X; + int16_t Y; + int16_t Z; + uint16_t status; +} __attribute__((packed)) MagData; + +typedef union { + struct { + UBXHeader_t header; + MagData data; + UBXFooter_t footer; + } __attribute__((packed)) fragments; + UBXPacket_t packet; +} MagUbxPkt; + +typedef struct { + uint32_t flightTime; + uint16_t HeapRemaining; + uint16_t IRQStackRemaining; + uint16_t SystemModStackRemaining; + uint16_t options; +} __attribute__((packed)) SysData; + +typedef union { + struct { + UBXHeader_t header; + SysData data; + UBXFooter_t footer; + } fragments; + UBXPacket_t packet; +} SysUbxPkt; + +void ubx_appendChecksum_message(UBXPacket_t *pkt); +void ubx_build_packet(UBXPacket_t *pkt, uint8_t packetClass, uint8_t packetId, uint16_t len); + +#define SYS_DATA_OPTIONS_FLASH 0x01 +#define UBX_HEADER_LEN (sizeof(UBXHeader_t)) + +#define UBX_SYN1 0xB5 +#define UBX_SYN2 0x62 + +#define UBX_OP_CUST_CLASS 0x99 +#define UBX_OP_SYS 0x01 +#define UBX_OP_MAG 0x02 /** * Create the module task. @@ -127,18 +208,24 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters) PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) + /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif - + static TickType_t lastUpdate; + SetupGPS(); + uint8_t counter = 0; while (1) { // NotificationUpdateStatus(); // Update the system statistics - updateStats(); - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); - vTaskDelay(SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000); + if (!(++counter & 0x7F)) { + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); + } + vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000); + ReadGPS(); ReadMag(); + updateStats(); } } @@ -146,7 +233,7 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters) /** * Called periodically to update the system stats */ -static uint16_t GetFreeIrqStackSize(void) +uint16_t GetFreeIrqStackSize(void) { uint32_t i = 0x150; @@ -181,21 +268,23 @@ static uint16_t GetFreeIrqStackSize(void) /** * Called periodically to update the system stats */ +SysUbxPkt sysPkt; static void updateStats() { - // Get stats and update - systemStats.FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; - systemStats.HeapRemaining = xPortGetFreeHeapSize(); - systemStats.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4; + static uint32_t lastUpdate; - // Get Irq stack status - systemStats.IRQStackRemaining = GetFreeIrqStackSize(); - uint8_t len = snprintf(outbuf, 40, "$POPSY,%u,%d,%d,%d*0\n", - (uint)systemStats.FlightTime, - systemStats.HeapRemaining, - systemStats.IRQStackRemaining, - systemStats.SystemModStackRemaining); - PIOS_COM_SendBuffer(pios_com_main_id, (uint8_t *)outbuf, len); + if (PIOS_DELAY_DiffuS(lastUpdate) < 1000 * configTICK_RATE_HZ / STAT_RATE) { + return; + } + lastUpdate = PIOS_DELAY_GetRaw(); + // Get stats and update + sysPkt.fragments.data.flightTime = xTaskGetTickCount() * portTICK_RATE_MS; + sysPkt.fragments.data.HeapRemaining = xPortGetFreeHeapSize(); + sysPkt.fragments.data.IRQStackRemaining = GetFreeIrqStackSize(); + sysPkt.fragments.data.SystemModStackRemaining = uxTaskGetStackHighWaterMark(NULL) * 4; + sysPkt.fragments.data.options = flash_id > 0 ? SYS_DATA_OPTIONS_FLASH : 0; + ubx_build_packet(&sysPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_SYS, sizeof(SysData)); + PIOS_COM_SendBuffer(pios_com_main_id, sysPkt.packet.bynarystream, sizeof(SysUbxPkt)); } /** @@ -242,31 +331,139 @@ void vApplicationMallocFailedHook(void) } -int16_t mag[3]; -uint8_t data[4]; - void ReadMag() { - PIOS_HMC5x83_ReadID(data); + static uint32_t lastUpdate; + + if (PIOS_DELAY_DiffuS(lastUpdate) < 1000 * configTICK_RATE_HZ / MAG_RATE) { + return; + } + lastUpdate = PIOS_DELAY_GetRaw(); + static int16_t mag[3]; if (PIOS_HMC5x83_ReadMag(mag) == 0) { - uint8_t len = snprintf(outbuf, 40, "$POPMG,%s,%d,%d,%d,*0\n", data, mag[0], mag[1], mag[2]); - PIOS_COM_SendBuffer(pios_com_main_id, (uint8_t *)outbuf, len); + MagUbxPkt magPkt; + // swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned + magPkt.fragments.data.X = mag[0]; + magPkt.fragments.data.Y = - mag[1]; + magPkt.fragments.data.Z = mag[2]; + magPkt.fragments.data.status = 1; + ubx_build_packet(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData)); + PIOS_COM_SendBuffer(pios_com_main_id, magPkt.packet.bynarystream, sizeof(MagUbxPkt)); return; } } +uint32_t lastUnsentData = 0; void ReadGPS() { - int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS); + bool completeSentenceSent = false; + int8_t maxCount = 3; - if (datacounter > 0) { - uint8_t toread = datacounter > BUFFER_SIZE ? BUFFER_SIZE : (uint8_t)datacounter; - PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toread); - PIOS_COM_SendBuffer(pios_com_main_id, buffer, toread); - } + do { + int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS); + if (datacounter > 0) { + uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter; + uint8_t toSend = toRead; + PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead); + + uint8_t *lastSentence; + static uint16_t lastSentenceLenght; + completeSentenceSent = getLastSentence(buffer, toRead, &lastSentence, &lastSentenceLenght); + if (completeSentenceSent) { + toSend = (uint8_t)(lastSentence - buffer + lastSentenceLenght); + } else { + lastUnsentData = 0; + } + + PIOS_COM_SendBuffer(pios_com_main_id, buffer, toSend); + if (toRead > toSend) { + // move unsent data at the beginning of buffer to be sent next time + lastUnsentData = toRead - toSend; + memcpy(buffer, (buffer + toSend), lastUnsentData); + } + } + + datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, buffer, BUFFER_SIZE, 0); + if (datacounter > 0) { + PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter); + } + } while (maxCount--); } + +bool getLastSentence(uint8_t *data, uint16_t bufferCount, uint8_t * *lastSentence, uint16_t *lenght) +{ + const uint8_t packet_overhead = UBX_HEADER_LEN + 2; + uint8_t *current = data + bufferCount - packet_overhead; + + while (current >= data) { + // look for a ubx a sentence + if (current[0] == UBX_SYN1 && current[1] == UBX_SYN2) { + // check whether it fits the current buffer (whole sentence is into buffer) + uint16_t len = current[4] + (current[5] << 8); + if (len + packet_overhead + current <= data + bufferCount) { + *lastSentence = current; + *lenght = len + packet_overhead; + return true; + } + } + current--; + } + // no complete sentence found + return false; +} + + +typedef struct { + uint8_t size; + const uint8_t *sentence; +} ubx_init_sentence; + + + +const ubx_init_sentence gps_config[] = { + [0] = { + .sentence = (uint8_t *)cfg_settings, + .size = sizeof(cfg_settings), + }, +}; +void ubx_build_packet(UBXPacket_t *pkt, uint8_t packetClass, uint8_t packetId, uint16_t len) +{ + pkt->packet.header.syn1 = UBX_SYN1; + pkt->packet.header.syn2 = UBX_SYN2; + + // don't make any assumption on alignments... + ((uint8_t *)&pkt->packet.header.len)[0] = len & 0xFF; + ((uint8_t *)&pkt->packet.header.len)[1] = (len >> 8) & 0xFF; + + pkt->packet.header.class = packetClass; + pkt->packet.header.id = packetId; + ubx_appendChecksum_message(pkt); +} + +void ubx_appendChecksum_message(UBXPacket_t *pkt) +{ + uint8_t chkA = 0; + uint8_t chkB = 0; + uint16_t len = ((uint8_t *)&pkt->packet.header.len)[0] | ((uint8_t *)&pkt->packet.header.len)[1] << 8; + + // From class field to the end of payload + for (uint8_t i = 2; i < len + UBX_HEADER_LEN; i++) { + chkA += pkt->bynarystream[i]; + chkB += chkA; + } + ; + pkt->packet.payload[len] = chkA; + pkt->packet.payload[len + 1] = chkB; +} + +void SetupGPS() +{ + for (uint8_t i = 0; i < NELEMENTS(gps_config); i++) { + PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, gps_config[i].sentence, gps_config[i].size); + } +} /** * @} * @}