rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

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

Browse Source 
Initial state machine asking for gps version. Configuration messages definition
This commit is contained in:
Alessio Morale 2014-08-22 02:43:30 +02:00
parent e64ab1f1ff
commit 903a77669e
3 changed files with 165 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/modules/GPS/GPS.c
View File

@ -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;

87
flight/modules/GPS/UBX.c
View File

@ -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,8 +100,23 @@ 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)
#define UBX_PVT_TIMEOUT (1000)
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t c, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
@ -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 >= 7000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
hwVersion = atoi(mon_ver->hwVersion);
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

73
flight/modules/GPS/inc/UBX.h
View File

@ -36,9 +36,10 @@
#include "auxmagsensor.h"
#include "GPS.h"
#define UBX_REPLY_TIMEOUT (500 * 1000)
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
// From u-blox6 receiver protocol specification
@ -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();