mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-16 08:29:15 +01:00
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
This commit is contained in:
Alessio Morale
parent
28f3ca9a17
commit
c232985baa
@ -40,6 +40,7 @@
|
||||
|
||||
#include <openpilot.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include <pios_helpers.h>
|
||||
// private includes
|
||||
#include "inc/gpsdsysmod.h"
|
||||
#include <pios_hmc5x83.h>
|
||||
@ -49,6 +50,7 @@
|
||||
#include <systemstats.h>
|
||||
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);
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user