rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +01:00
Code Issues Releases Activity

LP-212 changes from code review

Browse Source
This commit is contained in:
Cliff Geerdes 2016-02-11 00:55:47 -05:00
parent fad0e9f24c
commit baeb379a82
8 changed files with 226 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

288
flight/modules/GPS/DJI.c
View File

@ -33,47 +33,47 @@
#include "pios_math.h" #include "pios_math.h"
#include <pios_helpers.h> #include <pios_helpers.h>
#include <pios_delay.h> #include <pios_delay.h>
// dji parser is required for sensorType
#if (defined(PIOS_INCLUDE_GPS_DJI_PARSER) && defined(PIOS_INCLUDE_GPS_DJI_PARSER)) #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
#include "inc/DJI.h" #include "inc/DJI.h"
#include "inc/GPS.h" #include "inc/GPS.h"
#include <string.h> #include <string.h>
#include <auxmagsupport.h> #include <auxmagsupport.h>
bool useMag = false;
// this is defined in DJI.c
extern GPSPositionSensorSensorTypeOptions sensorType;
// parsing functions, roughly ordered by reception rate (higher rate messages on top) // parsing functions, roughly ordered by reception rate (higher rate messages on top)
static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition); static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition); static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition); static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
static bool checksum_dji_message(struct DJIPacket *dji);
static uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
// parse table item // parse table item
typedef struct { typedef struct {
uint8_t msgID; uint8_t msgId;
void (*handler)(struct DJIPacket *, GPSPositionSensorData *GpsPosition); void (*handler)(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
} dji_message_handler; } djiMessageHandler;
const dji_message_handler dji_handler_table[] = { const djiMessageHandler djiHandlerTable[] = {
{ .msgID = DJI_ID_GPS, .handler = &parse_dji_gps }, { .msgId = DJI_ID_GPS, .handler = &parse_dji_gps },
{ .msgID = DJI_ID_MAG, .handler = &parse_dji_mag }, { .msgId = DJI_ID_MAG, .handler = &parse_dji_mag },
{ .msgID = DJI_ID_VER, .handler = &parse_dji_ver }, { .msgId = DJI_ID_VER, .handler = &parse_dji_ver },
}; };
#define DJI_HANDLER_TABLE_SIZE NELEMENTS(dji_handler_table) #define DJI_HANDLER_TABLE_SIZE NELEMENTS(djiHandlerTable)
static bool useMag = false;
// detected hw version // detected hw version
uint32_t djiHwVersion = -1; uint32_t djiHwVersion = -1;
uint32_t djiSwVersion = -1; uint32_t djiSwVersion = -1;
// parse incoming character stream for messages in DJI binary format // parse incoming character stream for messages in DJI binary format
int parse_dji_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) #define djiPacket ((struct DJIPacket *)parsedDjiStruct)
int parse_dji_stream(uint8_t *inputBuffer, uint16_t inputBufferLength, char *parsedDjiStruct, GPSPositionSensorData *gpsPosition, struct GPS_RX_STATS *gpsRxStats)
{ {
int ret = PARSER_INCOMPLETE; // message not (yet) complete enum ProtocolStates {
enum proto_states {
START, START,
DJI_SY2, DJI_SY2,
DJI_ID, DJI_ID,
@ -83,105 +83,104 @@ int parse_dji_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
DJI_CHK2, DJI_CHK2,
FINISHED FINISHED
}; };
enum restart_states { enum RestartStates {
RESTART_WITH_ERROR, RESTART_WITH_ERROR,
RESTART_NO_ERROR RESTART_NO_ERROR
}; };
uint8_t c; static uint16_t payloadCount = 0;
static enum proto_states proto_state = START; static enum ProtocolStates protocolState = START;
static uint16_t rx_count = 0; static bool previousPacketGood = true;
struct DJIPacket *dji = (struct DJIPacket *)gps_rx_buffer; int ret = PARSER_INCOMPLETE; // message not (yet) complete
uint16_t i = 0; uint16_t inputBufferIndex = 0;
uint16_t restart_index = 0; uint16_t restartIndex = 0; // input buffer location to restart from
enum restart_states restart_state; enum RestartStates restartState;
static bool previous_packet_good = true; uint8_t inputByte;
bool current_packet_good; bool currentPacketGood;
// switch continue is the normal condition and comes back to here for another byte // switch continue is the normal condition and comes back to here for another byte
// switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte // switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
while (i < len) { while (inputBufferIndex < inputBufferLength) {
c = rx[i++]; inputByte = inputBuffer[inputBufferIndex++];
switch (proto_state) { switch (protocolState) {
case START: // detect protocol case START: // detect protocol
if (c == DJI_SYNC1) { // first DJI sync char found if (inputByte == DJI_SYNC1) { // first DJI sync char found
proto_state = DJI_SY2; protocolState = DJI_SY2;
// restart here, at byte after SYNC1, if we fail to parse // restart here, at byte after SYNC1, if we fail to parse
restart_index = i; restartIndex = inputBufferIndex;
} }
continue; continue;
case DJI_SY2: case DJI_SY2:
if (c == DJI_SYNC2) { // second DJI sync char found if (inputByte == DJI_SYNC2) { // second DJI sync char found
proto_state = DJI_ID; protocolState = DJI_ID;
} else { } else {
restart_state = RESTART_NO_ERROR; restartState = RESTART_NO_ERROR;
break; break;
} }
continue; continue;
case DJI_ID: case DJI_ID:
dji->header.id = c; djiPacket->header.id = inputByte;
proto_state = DJI_LEN; protocolState = DJI_LEN;
continue; continue;
case DJI_LEN: case DJI_LEN:
if (c > sizeof(DJIPayload)) { if (inputByte > sizeof(DJIPayload)) {
gpsRxStats->gpsRxOverflow++; gpsRxStats->gpsRxOverflow++;
#if defined(PIOS_GPS_MINIMAL) #if defined(PIOS_GPS_MINIMAL)
restart_state = RESTART_NO_ERROR; restartState = RESTART_NO_ERROR;
break;
#else #else
restart_state = RESTART_WITH_ERROR; restartState = RESTART_WITH_ERROR;
break;
#endif #endif
break;
} else { } else {
dji->header.len = c; djiPacket->header.len = inputByte;
if (c == 0) { if (inputByte == 0) {
proto_state = DJI_CHK1; protocolState = DJI_CHK1;
} else { } else {
rx_count = 0; payloadCount = 0;
proto_state = DJI_PAYLOAD; protocolState = DJI_PAYLOAD;
} }
} }
continue; continue;
case DJI_PAYLOAD: case DJI_PAYLOAD:
if (rx_count < dji->header.len) { if (payloadCount < djiPacket->header.len) {
dji->payload.payload[rx_count] = c; djiPacket->payload.payload[payloadCount] = inputByte;
if (++rx_count == dji->header.len) { if (++payloadCount == djiPacket->header.len) {
proto_state = DJI_CHK1; protocolState = DJI_CHK1;
} }
} }
continue; continue;
case DJI_CHK1: case DJI_CHK1:
dji->header.ck_a = c; djiPacket->header.checksumA = inputByte;
proto_state = DJI_CHK2; protocolState = DJI_CHK2;
continue; continue;
case DJI_CHK2: case DJI_CHK2:
dji->header.ck_b = c; djiPacket->header.checksumB = inputByte;
// ignore checksum errors on correct mag packets that nonetheless have checksum errors // ignore checksum errors on correct mag packets that nonetheless have checksum errors
// these checksum errors happen very often on clone DJI GPS (never on real DJI GPS) // these checksum errors happen very often on clone DJI GPS (never on real DJI GPS)
// and are caused by a clone DJI GPS firmware error // and are caused by a clone DJI GPS firmware error
// the errors happen when it is time to send a non-mag packet (4 or 5 per second) // the errors happen when it is time to send a non-mag packet (4 or 5 per second)
// instead of a mag packet (30 per second) // instead of a mag packet (30 per second)
current_packet_good = checksum_dji_message(dji); currentPacketGood = checksum_dji_message(djiPacket);
// message complete and valid or (it's a mag packet and the previous "any" packet was good) // message complete and valid or (it's a mag packet and the previous "any" packet was good)
if (current_packet_good || (dji->header.id == DJI_ID_MAG && previous_packet_good)) { if (currentPacketGood || (djiPacket->header.id == DJI_ID_MAG && previousPacketGood)) {
parse_dji_message(dji, GpsData); parse_dji_message(djiPacket, gpsPosition);
gpsRxStats->gpsRxReceived++; gpsRxStats->gpsRxReceived++;
proto_state = START; protocolState = START;
// overwrite PARSER_INCOMPLETE with PARSER_COMPLETE // overwrite PARSER_INCOMPLETE with PARSER_COMPLETE
// but don't overwrite PARSER_ERROR with PARSER_COMPLETE // but don't overwrite PARSER_ERROR with PARSER_COMPLETE
// pass PARSER_ERROR to caller if it happens even once // pass PARSER_ERROR to caller if it happens even once
// only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data // only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data
// that allows the caller to know if we are parsing GPS data // that allows the caller to know if we are parsing GPS data
// or just other packets for some reason (DJI clone firmware bug that happens sometimes) // or just other packets for some reason (DJI clone firmware bug that happens sometimes)
if (dji->header.id == DJI_ID_GPS && ret == PARSER_INCOMPLETE) { if (djiPacket->header.id == DJI_ID_GPS && ret == PARSER_INCOMPLETE) {
ret = PARSER_COMPLETE; // message complete & processed ret = PARSER_COMPLETE; // message complete & processed
} }
} else { } else {
gpsRxStats->gpsRxChkSumError++; gpsRxStats->gpsRxChkSumError++;
restart_state = RESTART_WITH_ERROR; restartState = RESTART_WITH_ERROR;
previous_packet_good = false; previousPacketGood = false;
break; break;
} }
previous_packet_good = current_packet_good; previousPacketGood = currentPacketGood;
continue; continue;
default: default:
continue; continue;
@ -192,13 +191,13 @@ int parse_dji_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
// and it does the expected thing across calls // and it does the expected thing across calls
// if restarting due to error detected in 2nd call to this function (on split packet) // if restarting due to error detected in 2nd call to this function (on split packet)
// then we just restart at index 0, which is mid-packet, not the second byte // then we just restart at index 0, which is mid-packet, not the second byte
if (restart_state == RESTART_WITH_ERROR) { if (restartState == RESTART_WITH_ERROR) {
ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets) ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
} }
rx += restart_index; // restart parsing just past the most recent SYNC1 inputBuffer += restartIndex; // restart parsing just past the most recent SYNC1
len -= restart_index; inputBufferLength -= restartIndex;
i = 0; inputBufferIndex = 0;
proto_state = START; protocolState = START;
} }
return ret; return ret;
@ -208,21 +207,21 @@ int parse_dji_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
bool checksum_dji_message(struct DJIPacket *dji) bool checksum_dji_message(struct DJIPacket *dji)
{ {
int i; int i;
uint8_t ck_a, ck_b; uint8_t checksumA, checksumB;
ck_a = dji->header.id; checksumA = dji->header.id;
ck_b = ck_a; checksumB = checksumA;
ck_a += dji->header.len; checksumA += dji->header.len;
ck_b += ck_a; checksumB += checksumA;
for (i = 0; i < dji->header.len; i++) { for (i = 0; i < dji->header.len; i++) {
ck_a += dji->payload.payload[i]; checksumA += dji->payload.payload[i];
ck_b += ck_a; checksumB += checksumA;
} }
if (dji->header.ck_a == ck_a && if (dji->header.checksumA == checksumA &&
dji->header.ck_b == ck_b) { dji->header.checksumB == checksumB) {
return true; return true;
} else { } else {
return false; return false;
@ -230,53 +229,59 @@ bool checksum_dji_message(struct DJIPacket *dji)
} }
static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition) static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition)
{ {
static bool inited = false; GPSVelocitySensorData gpsVelocity;
struct DjiGps *djiGps = &dji->payload.gps;
if (!inited) {
inited = true;
// Is there a model calculation we can do to get a reasonable value for geoid separation?
}
GPSVelocitySensorData GpsVelocity;
struct DJI_GPS *gps = &dji->payload.gps;
// decode with xor mask // decode with xor mask
uint8_t mask = gps->unused5; uint8_t mask = djiGps->unused5;
// for (uint8_t i=0; i<dji->header->len; ++i) {
for (uint8_t i = 0; i < 56; ++i) { // some bytes at the end are not xored
// if (i!=48 && i!=49 && i<=55) { // some bytes in the middle are not xored
if (i != 48 && i != 49) { for (uint8_t i = 0; i < GPS_DECODED_LENGTH; ++i) {
if (i != GPS_NOT_XORED_BYTE_1 && i != GPS_NOT_XORED_BYTE_2) {
dji->payload.payload[i] ^= mask; dji->payload.payload[i] ^= mask;
} }
} }
GpsVelocity.North = (float)gps->velN * 0.01f; gpsVelocity.North = (float)djiGps->velN * 0.01f;
GpsVelocity.East = (float)gps->velE * 0.01f; gpsVelocity.East = (float)djiGps->velE * 0.01f;
GpsVelocity.Down = (float)gps->velD * 0.01f; gpsVelocity.Down = (float)djiGps->velD * 0.01f;
GPSVelocitySensorSet(&GpsVelocity); GPSVelocitySensorSet(&gpsVelocity);
GpsPosition->Groundspeed = sqrtf(GpsVelocity.North * GpsVelocity.North + GpsVelocity.East * GpsVelocity.East); gpsPosition->Groundspeed = sqrtf(gpsVelocity.North * gpsVelocity.North + gpsVelocity.East * gpsVelocity.East);
GpsPosition->Heading = RAD2DEG(atan2f(-GpsVelocity.East, -GpsVelocity.North)) + 180.0f; // don't allow a funny number like 4.87416e-06 to show up in Uavo Browser for Heading
GpsPosition->Altitude = (float)gps->hMSL * 0.001f; // smallest groundspeed is 0.01f from (int)1 * (float)0.01
// so this is saying if groundspeed is zero
if (gpsPosition->Groundspeed < 0.009f) {
gpsPosition->Heading = 0.0f;
} else {
gpsPosition->Heading = RAD2DEG(atan2f(-gpsVelocity.East, -gpsVelocity.North)) + 180.0f;
}
gpsPosition->Altitude = (float)djiGps->hMSL * 0.001f;
// there is no source of geoid separation data in the DJI protocol // there is no source of geoid separation data in the DJI protocol
GpsPosition->GeoidSeparation = 0.0f; // Is there a reasonable world model calculation we can do to get a value for geoid separation?
GpsPosition->Latitude = gps->lat; gpsPosition->GeoidSeparation = 0.0f;
GpsPosition->Longitude = gps->lon; gpsPosition->Latitude = djiGps->lat;
GpsPosition->Satellites = gps->numSV; gpsPosition->Longitude = djiGps->lon;
GpsPosition->PDOP = gps->pDOP * 0.01f; gpsPosition->Satellites = djiGps->numSV;
GpsPosition->HDOP = sqrtf((float)gps->nDOP * (float)gps->nDOP + (float)gps->eDOP * (float)gps->eDOP) * 0.01f; gpsPosition->PDOP = djiGps->pDOP * 0.01f;
GpsPosition->VDOP = gps->vDOP * 0.01f; gpsPosition->HDOP = sqrtf((float)djiGps->nDOP * (float)djiGps->nDOP + (float)djiGps->eDOP * (float)djiGps->eDOP) * 0.01f;
if (gps->flags & FLAGS_GPSFIX_OK) { if (gpsPosition->HDOP > 99.99f) {
GpsPosition->Status = gps->fixType == FIXTYPE_3D ? gpsPosition->HDOP = 99.99f;
}
gpsPosition->VDOP = djiGps->vDOP * 0.01f;
if (djiGps->flags & FLAGS_GPSFIX_OK) {
gpsPosition->Status = djiGps->fixType == FIXTYPE_3D ?
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
} else { } else {
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; gpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
} }
GpsPosition->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI; gpsPosition->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
GpsPosition->AutoConfigStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED; gpsPosition->AutoConfigStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED;
GPSPositionSensorSet(GpsPosition); // gpsPosition->BaudRate = GPSPOSITIONSENSOR_BAUDRATE_115200;
GPSPositionSensorSet(gpsPosition);
// Time is valid, set GpsTime // Time is valid, set GpsTime
GPSTimeData GpsTime; GPSTimeData GpsTime;
@ -287,22 +292,22 @@ static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosit
// and maybe make the assumption that most people will fly at 5pm, not 1am // and maybe make the assumption that most people will fly at 5pm, not 1am
// this is part of the DJI protocol // this is part of the DJI protocol
// see DJI.h for further info // see DJI.h for further info
GpsTime.Year = (int16_t)gps->year + 2000; GpsTime.Year = (int16_t)djiGps->year + 2000;
GpsTime.Month = gps->month; GpsTime.Month = djiGps->month;
GpsTime.Day = gps->day; GpsTime.Day = djiGps->day;
GpsTime.Hour = gps->hour; GpsTime.Hour = djiGps->hour;
GpsTime.Minute = gps->min; GpsTime.Minute = djiGps->min;
GpsTime.Second = gps->sec; GpsTime.Second = djiGps->sec;
GPSTimeSet(&GpsTime); GPSTimeSet(&GpsTime);
} }
static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition) static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition)
{ {
if (!useMag) { if (!useMag) {
return; return;
} }
struct DJI_MAG *mag = &dji->payload.mag; struct DjiMag *mag = &dji->payload.mag;
union { union {
struct { struct {
int8_t mask; int8_t mask;
@ -318,45 +323,49 @@ static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPosi
} }
static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition) static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition)
{ {
struct DJI_VER *ver = &dji->payload.ver; {
struct DjiVer *ver = &dji->payload.ver;
// decode with xor mask // decode with xor mask
uint8_t mask = (uint8_t)(ver->unused1); uint8_t mask = (uint8_t)(ver->unused1);
// for (uint8_t i=0; i<dji->header->len; ++i) { // first 4 bytes are unused and 0 before the encryption
for (uint8_t i = 4; i < 12; ++i) { // so any one of them can be used for the decrypting xor mask
for (uint8_t i = VER_FIRST_DECODED_BYTE; i < sizeof(struct DjiVer); ++i) {
dji->payload.payload[i] ^= mask; dji->payload.payload[i] ^= mask;
} }
djiHwVersion = ver->hwVersion; djiHwVersion = ver->hwVersion;
djiSwVersion = ver->swVersion; djiSwVersion = ver->swVersion;
}
{
GPSPositionSensorSensorTypeOptions sensorType;
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI; sensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
} }
}
// DJI message parser // DJI message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing // returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition)
uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition)
{ {
uint32_t id = 0;
static bool djiInitialized = false; static bool djiInitialized = false;
uint32_t id = 0;
if (!djiInitialized) { if (!djiInitialized) {
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0. // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
GpsPosition->HDOP = 99.99f; gpsPosition->HDOP = 99.99f;
GpsPosition->PDOP = 99.99f; gpsPosition->PDOP = 99.99f;
GpsPosition->VDOP = 99.99f; gpsPosition->VDOP = 99.99f;
djiInitialized = true; djiInitialized = true;
} }
for (uint8_t i = 0; i < DJI_HANDLER_TABLE_SIZE; i++) { for (uint8_t i = 0; i < DJI_HANDLER_TABLE_SIZE; i++) {
const dji_message_handler *handler = &dji_handler_table[i]; const djiMessageHandler *handler = &djiHandlerTable[i];
if (handler->msgID == dji->header.id) { if (handler->msgId == dji->header.id) {
handler->handler(dji, GpsPosition); handler->handler(dji, gpsPosition);
break; break;
} }
} }
@ -374,6 +383,7 @@ uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *GpsPosi
return id; return id;
} }
void dji_load_mag_settings() void dji_load_mag_settings()
{ {
if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_DJI) { if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_DJI) {

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

@ -574,6 +574,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
if (gpsPort && gpsEnabled) { if (gpsPort && gpsEnabled) {
// if we have ubx auto config then sometimes we don't set the baud rate // if we have ubx auto config then sometimes we don't set the baud rate
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// just for UBX, because it has autoconfig
// if in startup, or not configured to do ubx and ubx auto config // if in startup, or not configured to do ubx and ubx auto config
// //
// on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate // on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
@ -588,6 +589,7 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
// always set the baud rate // always set the baud rate
gps_set_fc_baud_from_settings(); gps_set_fc_baud_from_settings();
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
// just for UBX, because it has subtypes UBX(6), UBX7 and UBX8
// changing anything in HwSettings will make it re-verify the sensor type (including auto-baud if not completely disabled) // changing anything in HwSettings will make it re-verify the sensor type (including auto-baud if not completely disabled)
// for auto baud disabled, the user can just try some baud rates and when the baud rate is correct, the sensor type becomes valid // for auto baud disabled, the user can just try some baud rates and when the baud rate is correct, the sensor type becomes valid
gps_ubx_reset_sensor_type(); gps_ubx_reset_sensor_type();

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

@ -34,22 +34,25 @@
#include "pios_math.h" #include "pios_math.h"
#include <pios_helpers.h> #include <pios_helpers.h>
#include <pios_delay.h> #include <pios_delay.h>
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "inc/UBX.h" #include "inc/UBX.h"
#include "inc/GPS.h" #include "inc/GPS.h"
#include <string.h> #include <string.h>
#ifndef PIOS_GPS_MINIMAL #if !defined(PIOS_GPS_MINIMAL)
#include <auxmagsupport.h> #include <auxmagsupport.h>
static bool useMag = false; static bool useMag = false;
#endif #endif /* !defined(PIOS_GPS_MINIMAL) */
GPSPositionSensorSensorTypeOptions sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
// this is set and used by this low level ubx code
// it is also reset by the ubx configuration code (UBX6 vs. UBX7) in ubx_autoconfig.c
GPSPositionSensorSensorTypeOptions ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
static bool usePvt = false; static bool usePvt = false;
static uint32_t lastPvtTime = 0; static uint32_t lastPvtTime = 0;
// parse table item // parse table item
typedef struct { typedef struct {
uint8_t msgClass; uint8_t msgClass;
@ -58,31 +61,27 @@ typedef struct {
} ubx_message_handler; } ubx_message_handler;
// parsing functions, roughly ordered by reception rate (higher rate messages on top) // parsing functions, roughly ordered by reception rate (higher rate messages on top)
static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_posllh(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_dop(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
#ifndef PIOS_GPS_MINIMAL #if !defined(PIOS_GPS_MINIMAL)
static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_timeutc(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_op_sys(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_op_mag(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_ack_ack(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_ack_nak(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition); static void parse_ubx_mon_ver(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition);
#endif #endif /* !defined(PIOS_GPS_MINIMAL) */
const ubx_message_handler ubx_handler_table[] = { const ubx_message_handler ubx_handler_table[] = {
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .handler = &parse_ubx_nav_posllh }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_POSLLH, .handler = &parse_ubx_nav_posllh },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .handler = &parse_ubx_nav_velned }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_VELNED, .handler = &parse_ubx_nav_velned },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .handler = &parse_ubx_nav_sol }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SOL, .handler = &parse_ubx_nav_sol },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .handler = &parse_ubx_nav_dop }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_DOP, .handler = &parse_ubx_nav_dop },
#ifndef PIOS_GPS_MINIMAL #if !defined(PIOS_GPS_MINIMAL)
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .handler = &parse_ubx_nav_pvt }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_PVT, .handler = &parse_ubx_nav_pvt },
{ .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG, .handler = &parse_ubx_op_mag }, { .msgClass = UBX_CLASS_OP_CUST, .msgID = UBX_ID_OP_MAG, .handler = &parse_ubx_op_mag },
{ .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .handler = &parse_ubx_nav_svinfo }, { .msgClass = UBX_CLASS_NAV, .msgID = UBX_ID_NAV_SVINFO, .handler = &parse_ubx_nav_svinfo },
@ -93,7 +92,7 @@ const ubx_message_handler ubx_handler_table[] = {
{ .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_NAK, .handler = &parse_ubx_ack_nak }, { .msgClass = UBX_CLASS_ACK, .msgID = UBX_ID_ACK_NAK, .handler = &parse_ubx_ack_nak },
{ .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_VER, .handler = &parse_ubx_mon_ver }, { .msgClass = UBX_CLASS_MON, .msgID = UBX_ID_MON_VER, .handler = &parse_ubx_mon_ver },
#endif #endif /* !defined(PIOS_GPS_MINIMAL) */
}; };
#define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table) #define UBX_HANDLER_TABLE_SIZE NELEMENTS(ubx_handler_table)
@ -106,11 +105,10 @@ struct UBX_ACK_NAK ubxLastNak;
// If a PVT sentence is received in the last UBX_PVT_TIMEOUT (ms) timeframe it disables VELNED/POSLLH/SOL/TIMEUTC // 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
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats) int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{ {
int ret = PARSER_INCOMPLETE; // message not (yet) complete
enum proto_states { enum proto_states {
START, START,
UBX_SY2, UBX_SY2,
@ -127,13 +125,14 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
RESTART_WITH_ERROR, RESTART_WITH_ERROR,
RESTART_NO_ERROR RESTART_NO_ERROR
}; };
uint8_t c;
static enum proto_states proto_state = START;
static uint16_t rx_count = 0; static uint16_t rx_count = 0;
static enum proto_states proto_state = START;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
int ret = PARSER_INCOMPLETE; // message not (yet) complete
uint16_t i = 0; uint16_t i = 0;
uint16_t restart_index = 0; uint16_t restart_index = 0;
enum restart_states restart_state; enum restart_states restart_state;
uint8_t c;
// switch continue is the normal condition and comes back to here for another byte // switch continue is the normal condition and comes back to here for another byte
// switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte // switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
@ -173,11 +172,10 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
gpsRxStats->gpsRxOverflow++; gpsRxStats->gpsRxOverflow++;
#if defined(PIOS_GPS_MINIMAL) #if defined(PIOS_GPS_MINIMAL)
restart_state = RESTART_NO_ERROR; restart_state = RESTART_NO_ERROR;
break;
#else #else
restart_state = RESTART_WITH_ERROR; restart_state = RESTART_WITH_ERROR;
break;
#endif #endif
break;
} else { } else {
if (ubx->header.len == 0) { if (ubx->header.len == 0) {
proto_state = UBX_CHK1; proto_state = UBX_CHK1;
@ -247,10 +245,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
return ret; return ret;
} }
// Keep track of various GPS messages needed to make up a single UAVO update // Keep track of various GPS messages needed to make up a single UAVO update
// time-of-week timestamp is used to correlate matching messages // time-of-week timestamp is used to correlate matching messages
#define POSLLH_RECEIVED (1 << 0) #define POSLLH_RECEIVED (1 << 0)
#define STATUS_RECEIVED (1 << 1) #define STATUS_RECEIVED (1 << 1)
#define DOP_RECEIVED (1 << 2) #define DOP_RECEIVED (1 << 2)
@ -380,6 +376,7 @@ static void parse_ubx_nav_velned(struct UBXPacket *ubx, GPSPositionSensorData *G
} }
} }
} }
#if !defined(PIOS_GPS_MINIMAL) #if !defined(PIOS_GPS_MINIMAL)
static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{ {
@ -510,11 +507,11 @@ static void parse_ubx_mon_ver(struct UBXPacket *ubx, __attribute__((unused)) GPS
struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver; struct UBX_MON_VER *mon_ver = &ubx->payload.mon_ver;
ubxHwVersion = atoi(mon_ver->hwVersion); ubxHwVersion = atoi(mon_ver->hwVersion);
sensorType = (ubxHwVersion >= 80000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 : ubxSensorType = (ubxHwVersion >= UBX_HW_VERSION_8) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX8 :
((ubxHwVersion >= 70000) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX); ((ubxHwVersion >= UBX_HW_VERSION_7) ? GPSPOSITIONSENSOR_SENSORTYPE_UBX7 : GPSPOSITIONSENSOR_SENSORTYPE_UBX);
// send sensor type right now because on UBX NEMA we don't get a full set of messages // send sensor type right now because on UBX NEMA we don't get a full set of messages
// and we want to be able to see sensor type even on UBX NEMA GPS's // and we want to be able to see sensor type even on UBX NEMA GPS's
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); GPSPositionSensorSensorTypeSet((uint8_t *)&ubxSensorType);
} }
static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition) static void parse_ubx_op_sys(struct UBXPacket *ubx, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
@ -543,10 +540,8 @@ static void parse_ubx_op_mag(struct UBXPacket *ubx, __attribute__((unused)) GPSP
} }
#endif /* if !defined(PIOS_GPS_MINIMAL) */ #endif /* if !defined(PIOS_GPS_MINIMAL) */
// UBX message parser // UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing // returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition) uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosition)
{ {
uint32_t id = 0; uint32_t id = 0;
@ -569,7 +564,7 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
} }
} }
GpsPosition->SensorType = sensorType; GpsPosition->SensorType = ubxSensorType;
if (msgtracker.msg_received == ALL_RECEIVED) { if (msgtracker.msg_received == ALL_RECEIVED) {
// leave BaudRate field alone! // leave BaudRate field alone!
@ -602,5 +597,5 @@ void op_gpsv9_load_mag_settings()
useMag = false; useMag = false;
} }
} }
#endif #endif // !defined(PIOS_GPS_MINIMAL)
#endif // PIOS_INCLUDE_GPS_UBX_PARSER #endif // defined(PIOS_INCLUDE_GPS_UBX_PARSER)

42
flight/modules/GPS/inc/DJI.h
View File

@ -169,7 +169,7 @@ typedef enum {
*/ */
// DJI GPS packet // DJI GPS packet
struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct offset struct DjiGps { // byte offset from beginning of packet, subtract 5 for struct offset
struct { // YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS struct { // YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS
uint32_t sec : 6; uint32_t sec : 6;
uint32_t min : 6; uint32_t min : 6;
@ -200,7 +200,7 @@ struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct
uint16_t unused4; // BYTE 58-59: ??? (seems to be always 0) uint16_t unused4; // BYTE 58-59: ??? (seems to be always 0)
uint8_t unused5; // BYTE 60 (XM): not sure yet, but I use it as the XOR mask uint8_t unused5; // BYTE 60 (XM): not sure yet, but I use it as the XOR mask
uint16_t seqNo; // BYTE 61-62 (SN): sequence number (not XORed), once there is a lock uint16_t seqNo; // BYTE 61-62 (SN): sequence number (not XORed), once there is a lock
// increases with every message. When the lock is lost later LSB and MSB are swapped with every message. // increases with every message. When the lock is lost later LSB and MSB are swapped (in all messages where lock is lost).
} __attribute__((packed)); } __attribute__((packed));
#define FLAGS_GPSFIX_OK (1 << 0) #define FLAGS_GPSFIX_OK (1 << 0)
@ -208,12 +208,16 @@ struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct
#define FLAGS_WKNSET (1 << 2) #define FLAGS_WKNSET (1 << 2)
#define FLAGS_TOWSET (1 << 3) #define FLAGS_TOWSET (1 << 3)
#define FIXTYPE_NO_FIX 0 #define FIXTYPE_NO_FIX 0x00 /* No Fix */
#define FIXTYPE_DEAD_RECKON 0x01 // Dead Reckoning only #define FIXTYPE_DEAD_RECKON 0x01 /* Dead Reckoning only */
#define FIXTYPE_2D 0x02 // 2D-Fix #define FIXTYPE_2D 0x02 /* 2D-Fix */
#define FIXTYPE_3D 0x03 // 3D-Fix #define FIXTYPE_3D 0x03 /* 3D-Fix */
#define FIXTYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined #define FIXTYPE_GNSS_DEAD_RECKON 0x04 /* GNSS + dead reckoning combined */
#define FIXTYPE_TIME_ONLY 0x05 // Time only fix #define FIXTYPE_TIME_ONLY 0x05 /* Time only fix */
#define GPS_DECODED_LENGTH offsetof(struct DjiGps, seqNo)
#define GPS_NOT_XORED_BYTE_1 offsetof(struct DjiGps, numSV)
#define GPS_NOT_XORED_BYTE_2 offsetof(struct DjiGps, unused2)
/* /*
@ -260,7 +264,7 @@ struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct
y any a (y and x?) values, convert radians to degrees and add 360 if the result is negative. y any a (y and x?) values, convert radians to degrees and add 360 if the result is negative.
*/ */
struct DJI_MAG { // byte offset from beginning of packet, subtract 5 for struct offset struct DjiMag { // byte offset from beginning of packet, subtract 5 for struct offset
int16_t x; // BYTE 5-6 (CX): compass X axis data (signed) - see comments below int16_t x; // BYTE 5-6 (CX): compass X axis data (signed) - see comments below
int16_t y; // BYTE 7-8 (CY): compass Y axis data (signed) - see comments below int16_t y; // BYTE 7-8 (CY): compass Y axis data (signed) - see comments below
int16_t z; // BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below int16_t z; // BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below
@ -294,26 +298,27 @@ struct DJI_MAG { // byte offset from beginning of packet, subtract 5 for struct
BYTE 17-18 (CS): checksum, calculated the same way as for uBlox binary messages BYTE 17-18 (CS): checksum, calculated the same way as for uBlox binary messages
*/ */
struct DJI_VER { // byte offset from beginning of packet, subtract 5 for struct offset struct DjiVer { // byte offset from beginning of packet, subtract 5 for struct offset
uint32_t unused1; // BYTE 5-8" ??? (seems to be always 0) uint32_t unused1; // BYTE 5-8" ??? (seems to be always 0)
uint32_t swVersion; // BYTE 9-12 (FW): firmware version uint32_t swVersion; // BYTE 9-12 (FW): firmware version
uint32_t hwVersion; // BYTE 13-16 (HW): hardware id uint32_t hwVersion; // BYTE 13-16 (HW): hardware id
} __attribute__((packed)); } __attribute__((packed));
#define VER_FIRST_DECODED_BYTE offsetof(struct DjiVer, swVersion)
typedef union { typedef union {
uint8_t payload[0]; uint8_t payload[0];
// Nav Class // Nav Class
struct DJI_GPS gps; struct DjiGps gps;
struct DJI_MAG mag; struct DjiMag mag;
struct DJI_VER ver; struct DjiVer ver;
} DJIPayload; } DJIPayload;
struct DJIHeader { struct DJIHeader {
uint8_t id; uint8_t id;
uint8_t len; uint8_t len;
uint8_t ck_a; // these are not part of the dji header, they are actually in the trailer uint8_t checksumA; // these are not part of the dji header, they are actually in the trailer
uint8_t ck_b; // but they are kept here for parsing ease uint8_t checksumB; // but they are kept here for parsing ease
} __attribute__((packed)); } __attribute__((packed));
struct DJIPacket { struct DJIPacket {
@ -321,12 +326,7 @@ struct DJIPacket {
DJIPayload payload; DJIPayload payload;
} __attribute__((packed)); } __attribute__((packed));
extern GPSPositionSensorSensorTypeOptions sensorType; int parse_dji_stream(uint8_t *inputBuffer, uint16_t inputBufferLength, char *parsedDjiStruct, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *GpsRxStats);
bool checksum_dji_message(struct DJIPacket *);
uint32_t parse_dji_message(struct DJIPacket *, GPSPositionSensorData *);
int parse_dji_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
void dji_load_mag_settings(); void dji_load_mag_settings();
#endif /* DJI_H */ #endif /* DJI_H */

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

@ -613,7 +613,7 @@ union UBXSENTPACKET {
// Used by AutoConfig code // Used by AutoConfig code
extern int32_t ubxHwVersion; extern int32_t ubxHwVersion;
extern GPSPositionSensorSensorTypeOptions sensorType; extern GPSPositionSensorSensorTypeOptions ubxSensorType;
extern struct UBX_ACK_ACK ubxLastAck; extern struct UBX_ACK_ACK ubxLastAck;
extern struct UBX_ACK_NAK ubxLastNak; extern struct UBX_ACK_NAK ubxLastNak;

4
flight/modules/GPS/ubx_autoconfig.c
View File

@ -241,8 +241,8 @@ void gps_ubx_reset_sensor_type()
if (__sync_fetch_and_add(&mutex, 1) == 0) { if (__sync_fetch_and_add(&mutex, 1) == 0) {
ubxHwVersion = -1; ubxHwVersion = -1;
baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful baud_to_try_index -= 1; // undo postincrement and start with the one that was most recently successful
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN; ubxSensorType = GPSPOSITIONSENSOR_SENSORTYPE_UNKNOWN;
GPSPositionSensorSensorTypeSet(&sensorType); GPSPositionSensorSensorTypeSet(&ubxSensorType);
// make the sensor type / autobaud code time out immediately to send the request immediately // make the sensor type / autobaud code time out immediately to send the request immediately
status->lastStepTimestampRaw += 0x8000000UL; status->lastStepTimestampRaw += 0x8000000UL;
} }

8
flight/modules/Sensors/sensors.c
View File

@ -195,7 +195,7 @@ static uint8_t baro_temp_calibration_count = 0;
#if defined(PIOS_INCLUDE_HMC5X83) #if defined(PIOS_INCLUDE_HMC5X83)
// Allow AuxMag to be disabled without reboot // Allow AuxMag to be disabled without reboot
// because the other mags are that way // because the other mags are that way
static bool useMag = false; static bool useAuxMag = false;
#endif #endif
/** /**
@ -497,7 +497,7 @@ static void handleMag(float *samples, float temperature)
#if defined(PIOS_INCLUDE_HMC5X83) #if defined(PIOS_INCLUDE_HMC5X83)
static void handleAuxMag(float *samples) static void handleAuxMag(float *samples)
{ {
if (useMag) { if (useAuxMag) {
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK); auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
} }
} }
@ -639,9 +639,9 @@ void aux_hmc5x83_load_mag_settings()
uint8_t magType = auxmagsupport_get_type(); uint8_t magType = auxmagsupport_get_type();
if (magType == AUXMAGSETTINGS_TYPE_I2C || magType == AUXMAGSETTINGS_TYPE_FLEXI) { if (magType == AUXMAGSETTINGS_TYPE_I2C || magType == AUXMAGSETTINGS_TYPE_FLEXI) {
useMag = true; useAuxMag = true;
} else { } else {
useMag = false; useAuxMag = false;
} }
} }
#endif #endif

1
flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
View File

@ -6,6 +6,7 @@
* @{ * @{
* @file pios_config.h * @file pios_config.h
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
* @brief PiOS configuration header, the compile time config file for the PIOS. * @brief PiOS configuration header, the compile time config file for the PIOS.
* Defines which PiOS libraries and features are included in the firmware. * Defines which PiOS libraries and features are included in the firmware.
* @see The GNU Public License (GPL) Version 3 * @see The GNU Public License (GPL) Version 3