From 2ae1f9c63ddf21b44bfe039275611cb26fe552d4 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 9 Feb 2016 14:43:44 -0500 Subject: [PATCH] LP-212 DJI GPSMag support --- flight/libraries/auxmagsupport.c | 7 +- flight/modules/GPS/DJI.c | 390 ++++++++++++++++++ flight/modules/GPS/GPS.c | 171 ++++++-- flight/modules/GPS/NMEA.c | 3 +- flight/modules/GPS/UBX.c | 25 +- flight/modules/GPS/inc/DJI.h | 332 +++++++++++++++ flight/modules/GPS/inc/UBX.h | 4 +- flight/modules/GPS/ubx_autoconfig.c | 19 +- flight/modules/Sensors/sensors.c | 49 ++- .../coptercontrol/firmware/inc/pios_config.h | 1 + .../boards/discoveryf4bare/firmware/Makefile | 4 +- .../firmware/inc/pios_config.h | 3 +- .../boards/osd/firmware/inc/pios_config.h | 1 + .../revolution/firmware/inc/pios_config.h | 3 +- .../revonano/firmware/inc/pios_config.h | 3 +- .../revoproto/firmware/inc/pios_config.h | 4 +- shared/uavobjectdefinition/auxmagsettings.xml | 2 +- .../uavobjectdefinition/gpspositionsensor.xml | 2 +- shared/uavobjectdefinition/gpssettings.xml | 2 +- 19 files changed, 947 insertions(+), 78 deletions(-) create mode 100644 flight/modules/GPS/DJI.c create mode 100644 flight/modules/GPS/inc/DJI.h diff --git a/flight/libraries/auxmagsupport.c b/flight/libraries/auxmagsupport.c index f87c1a50b..68c620e6f 100644 --- a/flight/libraries/auxmagsupport.c +++ b/flight/libraries/auxmagsupport.c @@ -2,7 +2,8 @@ ****************************************************************************** * * @file auxmagsupport.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Functions to handle aux mag data and calibration. * -- * @see The GNU Public License (GPL) Version 3 @@ -57,6 +58,10 @@ void auxmagsupport_reload_settings() // GPSV9, Ext (unused), and Flexi AuxMagSettingsTypeGet(&option); + + const uint8_t status = AUXMAGSENSOR_STATUS_NONE; + // next sample from other external mags will provide the right status if present + AuxMagSensorStatusSet((uint8_t *)&status); } void auxmagsupport_publish_samples(float mags[3], uint8_t status) diff --git a/flight/modules/GPS/DJI.c b/flight/modules/GPS/DJI.c new file mode 100644 index 000000000..ee1428fdf --- /dev/null +++ b/flight/modules/GPS/DJI.c @@ -0,0 +1,390 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information (DJI-Naza binary format) + * @{ + * + * @file DJI.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief GPS module, handles DJI stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "pios.h" +#include "pios_math.h" +#include +#include +// dji parser is required for sensorType +#if (defined(PIOS_INCLUDE_GPS_DJI_PARSER) && defined(PIOS_INCLUDE_GPS_DJI_PARSER)) +#include "inc/DJI.h" +#include "inc/GPS.h" +#include + +#include + +bool useMag = false; + +// this is defined in DJI.c +extern GPSPositionSensorSensorTypeOptions sensorType; + +// 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_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition); +static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition); + +// parse table item +typedef struct { + uint8_t msgID; + void (*handler)(struct DJIPacket *, GPSPositionSensorData *GpsPosition); +} dji_message_handler; + +const dji_message_handler dji_handler_table[] = { + { .msgID = DJI_ID_GPS, .handler = &parse_dji_gps }, + { .msgID = DJI_ID_MAG, .handler = &parse_dji_mag }, + { .msgID = DJI_ID_VER, .handler = &parse_dji_ver }, +}; +#define DJI_HANDLER_TABLE_SIZE NELEMENTS(dji_handler_table) + + +// detected hw version +uint32_t djiHwVersion = -1; +uint32_t djiSwVersion = -1; + +// 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) +{ + int ret = PARSER_INCOMPLETE; // message not (yet) complete + enum proto_states { + START, + DJI_SY2, + DJI_ID, + DJI_LEN, + DJI_PAYLOAD, + DJI_CHK1, + DJI_CHK2, + FINISHED + }; + enum restart_states { + RESTART_WITH_ERROR, + RESTART_NO_ERROR + }; + uint8_t c; + static enum proto_states proto_state = START; + static uint16_t rx_count = 0; + struct DJIPacket *dji = (struct DJIPacket *)gps_rx_buffer; + uint16_t i = 0; + uint16_t restart_index = 0; + enum restart_states restart_state; + static bool previous_packet_good = true; + bool current_packet_good; + + // 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 + while (i < len) { + c = rx[i++]; + switch (proto_state) { + case START: // detect protocol + if (c == DJI_SYNC1) { // first DJI sync char found + proto_state = DJI_SY2; + // restart here, at byte after SYNC1, if we fail to parse + restart_index = i; } + continue; + case DJI_SY2: + if (c == DJI_SYNC2) { // second DJI sync char found + proto_state = DJI_ID; + } else { + restart_state = RESTART_NO_ERROR; + break; + } + continue; + case DJI_ID: + dji->header.id = c; + proto_state = DJI_LEN; + continue; + case DJI_LEN: + if (c > sizeof(DJIPayload)) { + gpsRxStats->gpsRxOverflow++; +#if defined(PIOS_GPS_MINIMAL) + restart_state = RESTART_NO_ERROR; + break; +#else + restart_state = RESTART_WITH_ERROR; + break; +#endif + } else { + dji->header.len = c; + if (c == 0) { + proto_state = DJI_CHK1; + } else { + rx_count = 0; + proto_state = DJI_PAYLOAD; + } + } + continue; + case DJI_PAYLOAD: + if (rx_count < dji->header.len) { + dji->payload.payload[rx_count] = c; + if (++rx_count == dji->header.len) { + proto_state = DJI_CHK1; + } + } + continue; + case DJI_CHK1: + dji->header.ck_a = c; + proto_state = DJI_CHK2; + continue; + case DJI_CHK2: + dji->header.ck_b = c; + // 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) + // 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) + // instead of a mag packet (30 per second) + current_packet_good = checksum_dji_message(dji); + // 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)) { + parse_dji_message(dji, GpsData); + gpsRxStats->gpsRxReceived++; + proto_state = START; + // overwrite PARSER_INCOMPLETE with PARSER_COMPLETE + // but don't overwrite PARSER_ERROR with PARSER_COMPLETE + // 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 + // 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) + if (dji->header.id==DJI_ID_GPS && ret==PARSER_INCOMPLETE) { + ret = PARSER_COMPLETE; // message complete & processed + } + } else { + gpsRxStats->gpsRxChkSumError++; + restart_state = RESTART_WITH_ERROR; + previous_packet_good = false; + break; + } + previous_packet_good = current_packet_good; + continue; + default: + continue; + } + + // this simple restart doesn't work across calls + // but it does work within a single call + // and it does the expected thing across calls + // 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 + if (restart_state == RESTART_WITH_ERROR) { + 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 + len -= restart_index; + i = 0; + proto_state = START; + } + + return ret; +} + + +bool checksum_dji_message(struct DJIPacket *dji) +{ + int i; + uint8_t ck_a, ck_b; + + ck_a = dji->header.id; + ck_b = ck_a; + + ck_a += dji->header.len; + ck_b += ck_a; + + for (i = 0; i < dji->header.len; i++) { + ck_a += dji->payload.payload[i]; + ck_b += ck_a; + } + + if (dji->header.ck_a == ck_a && + dji->header.ck_b == ck_b) { + return true; + } else { + return false; + } +} + + +static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition) +{ + static bool inited=false; + 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 + uint8_t mask = gps->unused5; + //for (uint8_t i=0; iheader->len; ++i) { + for (uint8_t i=0; i<56; ++i) { + //if (i!=48 && i!=49 && i<=55) { + if (i!=48 && i!=49) { + dji->payload.payload[i]^=mask; + } + } + + GpsVelocity.North = (float)gps->velN * 0.01f; + GpsVelocity.East = (float)gps->velE * 0.01f; + GpsVelocity.Down = (float)gps->velD * 0.01f; + GPSVelocitySensorSet(&GpsVelocity); + + GpsPosition->Groundspeed = sqrtf(GpsVelocity.North*GpsVelocity.North + GpsVelocity.East*GpsVelocity.East); + GpsPosition->Heading = RAD2DEG(atan2f(-GpsVelocity.East, -GpsVelocity.North)) + 180.0f; + GpsPosition->Altitude = (float)gps->hMSL * 0.001f; + // there is no source of geoid separation data in the DJI protocol + GpsPosition->GeoidSeparation = 0.0f; + GpsPosition->Latitude = gps->lat; + GpsPosition->Longitude = gps->lon; + GpsPosition->Satellites = gps->numSV; + GpsPosition->PDOP = gps->pDOP * 0.01f; + // cliffg FIXME + // might get away with just using a max function here... + // hmmm pdop=sqrt(hdop*hdop+hdop*hdop+vdop*vdop) + // pdop*pdop=hdop*hdop+hdop*hdop+vdop*vdop + // pdop*pdop-vdop*vdop=hdop*hdop+hdop*hdop + // (pdop*pdop-vdop*vdop)/2=hdop*hdop + // srtq(pdop*pdop-vdop*vdop)/2=hdop + GpsPosition->HDOP = sqrtf((float)gps->nDOP*(float)gps->nDOP + (float)gps->eDOP*(float)gps->eDOP) * 0.01f; + GpsPosition->VDOP = gps->vDOP * 0.01f; + if (gps->flags & FLAGS_GPSFIX_OK) { + GpsPosition->Status = gps->fixType == FIXTYPE_3D ? + GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D; + } else { + GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX; + } + GpsPosition->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI; + GpsPosition->AutoConfigStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED; + //GpsPosition->BaudRate = GPSPOSITIONSENSOR_BAUDRATE_115200; + GPSPositionSensorSet(GpsPosition); + + // Time is valid, set GpsTime + GPSTimeData GpsTime; + // cliffg FIXME + // the lowest bit of day and the highest bit of hour overlap (xored? no stranger than that) + // this causes strange day/hour changes + // we could track it here and even if we guess wrong initially + // we can massage the data so that time doesn't jump + // and maybe make the assumption that most people will fly at 5pm, not 1am + // so if it looks if we have to make a choice between 5pm on the 10th and + GpsTime.Year = (int16_t)gps->year + 2000; + GpsTime.Month = gps->month; + GpsTime.Day = gps->day; + GpsTime.Hour = gps->hour; + GpsTime.Minute = gps->min; + GpsTime.Second = gps->sec; + GPSTimeSet(&GpsTime); +} + + +static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition) +{ + if (!useMag) { + return; + } + struct DJI_MAG *mag = &dji->payload.mag; + union { + struct { + int8_t mask; + int8_t mask2; + }; + int16_t maskmask; + } u; + u.mask = (int8_t) (dji->payload.payload[4]); + u.mask = u.mask2 = (((u.mask ^ (u.mask >> 4)) & 0x0F) | ((u.mask << 3) & 0xF0)) ^ (((u.mask & 0x01) << 3) | ((u.mask & 0x01) << 7)); + // yes, z is only xored by mask<<8, not maskmask + float mags[3] = { mag->x^u.maskmask, mag->y^u.maskmask, mag->z^((int16_t)u.mask<<8) }; + auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK); +} + + +static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition) +{ + struct DJI_VER *ver = &dji->payload.ver; + + // decode with xor mask + uint8_t mask = (uint8_t)(ver->unused1); + //for (uint8_t i=0; iheader->len; ++i) { + for (uint8_t i=4; i<12; ++i) { + dji->payload.payload[i]^=mask; + } + + djiHwVersion = ver->hwVersion; + djiSwVersion = ver->swVersion; + sensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI; + GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); +} + + +// DJI message parser +// returns UAVObjectID if a UAVObject structure is ready for further processing + +uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition) +{ + uint32_t id = 0; + static bool djiInitialized = false; + + if (!djiInitialized) { + // 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->PDOP = 99.99f; + GpsPosition->VDOP = 99.99f; + djiInitialized = true; + } + + for (uint8_t i = 0; i < DJI_HANDLER_TABLE_SIZE; i++) { + const dji_message_handler *handler = &dji_handler_table[i]; + if (handler->msgID == dji->header.id) { + handler->handler(dji, GpsPosition); + break; + } + } + + { + uint8_t status; + GPSPositionSensorStatusGet(&status); + if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) { + // Some dji thing has been received so GPS is there + status = GPSPOSITIONSENSOR_STATUS_NOFIX; + GPSPositionSensorStatusSet(&status); + } + } + + return id; +} + +void dji_load_mag_settings() +{ + if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_DJI) { + useMag = true; + } else { + useMag = false; + } +} +#endif // PIOS_INCLUDE_GPS_DJI_PARSER diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index fea271921..969dce205 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -7,7 +7,8 @@ * @{ * * @file GPS.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief GPS module, handles GPS and NMEA stream * @see The GNU Public License (GPL) Version 3 * @@ -48,9 +49,11 @@ #include "GPS.h" #include "NMEA.h" #include "UBX.h" -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) +#include "DJI.h" +#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #include "inc/ubx_autoconfig.h" #endif +#include #include PERF_DEFINE_COUNTER(counterBytesIn); @@ -67,7 +70,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData); static float GravityAccel(float latitude, float longitude, float altitude); #endif -#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); #ifndef PIOS_GPS_MINIMAL void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev); @@ -124,7 +127,7 @@ static char *gps_rx_buffer; static uint32_t timeOfLastUpdateMs; -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) static struct GPS_RX_STATS gpsRxStats; #endif @@ -178,7 +181,7 @@ int32_t GPSInitialize(void) GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); -#ifdef PIOS_INCLUDE_GPS_UBX_PARSER +#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) AuxMagSensorInitialize(); AuxMagSettingsInitialize(); GPSExtendedStatusInitialize(); @@ -212,19 +215,34 @@ int32_t GPSInitialize(void) #endif /* if defined(REVOLUTION) */ if (gpsEnabled) { -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER) - gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH); -#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER) +#if defined(PIOS_GPS_MINIMAL) +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); -#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER) - gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH); #else gps_rx_buffer = NULL; #endif -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) +#else /* defined(PIOS_GPS_MINIMAL) */ +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + size_t bufSize = NMEA_MAX_PACKET_LENGTH; +#else + size_t bufSize = 0; +#endif +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + if (bufSize < sizeof(struct UBXPacket)) bufSize = sizeof(struct UBXPacket); +#endif +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) + if (bufSize < sizeof(struct DJIPacket)) bufSize = sizeof(struct DJIPacket); +#endif + gps_rx_buffer = pios_malloc(bufSize); +#else /* defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) */ + gps_rx_buffer = NULL; +#endif /* defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) */ +#endif /* defined(PIOS_GPS_MINIMAL) */ +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) PIOS_Assert(gps_rx_buffer); #endif -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup GPSSettingsConnectCallback(updateGpsSettings); #endif @@ -251,17 +269,15 @@ static void gpsTask(__attribute__((unused)) void *parameters) // 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible portTickType xDelay = 5 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - #ifdef PIOS_GPS_SETS_HOMELOCATION portTickType homelocationSetDelay = 0; #endif GPSPositionSensorData gpspositionsensor; timeOfLastUpdateMs = timeNowMs; - GPSPositionSensorGet(&gpspositionsensor); -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - // this should be done in the task because it calls out to actually start the GPS serial reads +#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) + // this should be done in the task because it calls out to actually start the ubx GPS serial reads updateGpsSettings(0); #endif @@ -321,7 +337,7 @@ static void gpsTask(__attribute__((unused)) void *parameters) PERF_TRACK_VALUE(counterBytesIn, cnt); PERF_MEASURE_PERIOD(counterRate); switch (gpsSettings.DataProtocol) { -#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && !defined(PIOS_GPS_MINIMAL) case GPSSETTINGS_DATAPROTOCOL_NMEA: res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; @@ -330,6 +346,11 @@ static void gpsTask(__attribute__((unused)) void *parameters) case GPSSETTINGS_DATAPROTOCOL_UBX: res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); break; +#endif +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) + case GPSSETTINGS_DATAPROTOCOL_DJI: + res = parse_dji_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); + break; #endif default: res = NO_PARSER; // this should not happen @@ -343,8 +364,13 @@ static void gpsTask(__attribute__((unused)) void *parameters) } } - // if there is any error at all, set status to NOGPS - // Check for GPS timeout + // if there is a protocol error or communication error, or timeout error, + // generally, if there is an error that is due to configuration or bad hardware, set status to NOGPS + // poor GPS signal gets a different error/alarm (below) + // + // should this be expanded to include aux mag status as well? currently the aux mag + // attached to a GPS protocol (OPV9 and DJI) still says OK after the GPS/mag goes down + // (data cable unplugged or flight battery removed with USB still powering the FC) timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; if ((res == PARSER_ERROR) || (timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS || @@ -356,10 +382,15 @@ static void gpsTask(__attribute__((unused)) void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); } // if we parsed at least one packet successfully + // we aren't offline, but we still may not have a good enough fix to fly + // or we might not even be receiving all necessary GPS packets (NMEA) else if (res == PARSER_COMPLETE) { - // we appear to be receiving GPS sentences OK, we've had an update + // we appear to be receiving packets OK // criteria for GPS-OK taken from this post... // http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 + // NMEA doesn't verify that all necessary packet types for an update have been received + // + // if (the fix is good) { if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) && (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { @@ -380,9 +411,11 @@ static void gpsTask(__attribute__((unused)) void *parameters) homelocationSetDelay = 0; } #endif + // else if (we are at least getting what might be usable GPS data to finish a flight with) { } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) && (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + // else data is probably not good enough to fly } else { AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } @@ -498,60 +531,95 @@ void gps_set_fc_baud_from_arg(uint8_t baud) } +// set the FC port baud rate +// from HwSettings or override with 115200 if DJI +static void gps_set_fc_baud_from_settings() +{ + uint8_t speed; + // Retrieve settings +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { + speed = HWSETTINGS_GPSSPEED_115200; + } else { +#endif + HwSettingsGPSSpeedGet(&speed); +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) + } +#endif + // set fc baud + gps_set_fc_baud_from_arg(speed); +} + + /** - * Update the GPS settings, called on startup. - * FIXME: This should be in the GPSSettings object. But objects have - * too much overhead yet. Also the GPS has no any specific settings - * like protocol, etc. Thus the HwSettings object which contains the - * GPS port speed is used for now. + * HwSettings callback + * Generally speaking, set the FC baud rate + * and for UBX, if it is safe, start the auto config process + * this allows the user to change the baud rate and see if it works without rebooting */ // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) { -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport #endif // if GPS (UBX or NMEA) is enabled at all if (gpsPort && gpsEnabled) { +// 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 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 // after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled // always we must set the Revo port baud rate if not using UBX -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) if (ev == NULL || previousGpsPort != gpsPort || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) #endif { - uint8_t speed; - // Retrieve settings - HwSettingsGPSSpeedGet(&speed); - // set fc baud - gps_set_fc_baud_from_arg(speed); + // always set the baud rate + gps_set_fc_baud_from_settings(); #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) - // even changing the baud rate will make it re-verify the sensor type - // that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct + // 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 gps_ubx_reset_sensor_type(); #endif } #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) else { - // it will never do this during startup because ev == NULL + // else: + // - we are past startup + // - we are running uxb protocol + // - and some form of ubx auto config is enabled + // + // it will never do this during startup because ev == NULL during startup + // + // this is here so that runtime (not startup) user changes to HwSettings will restart auto-config gps_ubx_autoconfig_set(NULL); } #endif } -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) previousGpsPort = gpsPort; #endif } -#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) +#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { + auxmagsupport_reload_settings(); +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) op_gpsv9_load_mag_settings(); +#endif +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) + dji_load_mag_settings(); +#endif +#if defined(PIOS_INCLUDE_HMC5X83) + aux_hmc5x83_load_mag_settings(); +#endif } @@ -561,9 +629,17 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) GPSSettingsGet(&gpsSettings); - // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NMEA +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) + // each time there is a protocol change, set the baud rate + // so that DJI can be forced to 115200, but changing to another protocol will change the baud rate to the user specified value + // note that changes to HwSettings GPS baud rate are detected in the HwSettings callback, + // but changing to/from DJI is effectively a baud rate change because DJI is forced to be 115200 + gps_set_fc_baud_from_settings(); // knows to force 115200 for DJI +#endif + + // it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running another protocol // because ubx auto config never gets called - // setting it up completely means that if we switch from initial NMEA to UBX or disabled to enabled, that it will start normally + // setting it up completely means that if we switch from the other protocol to UBX or disabled to enabled, that it will start normally newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig; newconfig.navRate = gpsSettings.UbxRate; newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE : @@ -650,9 +726,22 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) break; } - gps_ubx_autoconfig_set(&newconfig); +// handle protocol changes and devices that have just been powered up +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { + // do whatever the user has configured for power on startup + // even autoconfig disabled still gets the ubx version + gps_ubx_autoconfig_set(&newconfig); + } +#endif +#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) + if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) { + // clear out satellite data because DJI doesn't provide it + GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0); + } +#endif } -#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ +#endif /* (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) */ /** * @} * @} diff --git a/flight/modules/GPS/NMEA.c b/flight/modules/GPS/NMEA.c index 13ca0555f..825164bf3 100644 --- a/flight/modules/GPS/NMEA.c +++ b/flight/modules/GPS/NMEA.c @@ -7,7 +7,8 @@ * @{ * * @file NMEA.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief GPS module, handles GPS and NMEA stream * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/modules/GPS/UBX.c b/flight/modules/GPS/UBX.c index 262c72ab6..d6f77518f 100644 --- a/flight/modules/GPS/UBX.c +++ b/flight/modules/GPS/UBX.c @@ -7,7 +7,8 @@ * @{ * * @file UBX.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief GPS module, handles GPS and NMEA stream * @see The GNU Public License (GPL) Version 3 * @@ -130,7 +131,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition static enum proto_states proto_state = START; static uint16_t rx_count = 0; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; - uint16_t i = 0; + uint16_t i = 0; uint16_t restart_index = 0; enum restart_states restart_state; @@ -204,15 +205,20 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition // this has been proven by running it without autoconfig and testing: // data coming from OPV9 "GPS+MCU" port the checksum errors happen roughly every 5 to 30 seconds // same data coming from OPV9 "GPS Only" port the checksums are always good - // this also ocassionally causes parse_ubx_message() to issue alarms because not all the messages were received + // this also occasionally causes parse_ubx_message() to issue alarms because not all the messages were received // see OP GPSV9 comment in parse_ubx_message() for further information - if (checksum_ubx_message(ubx)) { // message complete and valid - parse_ubx_message(ubx, GpsData); + if (checksum_ubx_message(ubx)) { gpsRxStats->gpsRxReceived++; proto_state = START; - // pass PARSER_ERROR to be to caller if it happens even once - if (ret == PARSER_INCOMPLETE) { - ret = PARSER_COMPLETE; // message complete & processed + // overwrite PARSER_INCOMPLETE with PARSER_COMPLETE + // but don't overwrite PARSER_ERROR with PARSER_COMPLETE + // 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 + // that allows the caller to know if we are parsing GPS data + // or just other packets for some reason (mis-configuration) + if (parse_ubx_message(ubx, GpsData)==GPSPOSITIONSENSOR_OBJID + && ret == PARSER_INCOMPLETE) { + ret = PARSER_COMPLETE; } } else { gpsRxStats->gpsRxChkSumError++; @@ -230,9 +236,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition // 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 if (restart_state == 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) } - // else restart with no error rx += restart_index; // restart parsing just past the most recent SYNC1 len -= restart_index; i = 0; diff --git a/flight/modules/GPS/inc/DJI.h b/flight/modules/GPS/inc/DJI.h new file mode 100644 index 000000000..7f6adc7c8 --- /dev/null +++ b/flight/modules/GPS/inc/DJI.h @@ -0,0 +1,332 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file DJI.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * @brief GPS module, handles DJI stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef DJI_H +#define DJI_H +#include "openpilot.h" +#include "gpspositionsensor.h" +#include "gpsextendedstatus.h" +#ifndef PIOS_GPS_MINIMAL +#include "auxmagsensor.h" +#endif + +#include "GPS.h" + +#define DJI_SYNC1 0x55 // DJI protocol synchronization characters +#define DJI_SYNC2 0xaa + +// Message IDs +typedef enum { + DJI_ID_GPS = 0x10, + DJI_ID_MAG = 0x20, + DJI_ID_VER = 0x30, +} dji_msg_id; + +// private structures + +/* +GPS protocol info from +http://www.rcgroups.com/forums/showpost.php?p=26210591&postcount=15 + +The 0x10 message contains GPS data. Here is the structure of the message, +fields marked with XX I'm not sure about yet. The others will be described below. + +55 AA 10 3A DT DT DT DT LO LO LO LO LA LA LA LA AL AL AL AL HA HA HA HA VA VA VA VA XX XX XX XX +NV NV NV NV EV EV EV EV DV DV DV DV PD PD VD VD ND ND ED ED NS XX FT XX SF XX XX XM SN SN CS CS + +The payload is XORed with a mask that changes over time (see below for more details). + +Values in the message are stored in little endian. + +HEADER +------------- +BYTE 1-2: message header - always 55 AA +BYTE 3: message id (0x10 for GPS message) +BYTE 4: lenght of the payload (0x3A or 58 decimal for 0x10 message) + +PAYLOAD +-------------- +BYTE 5-8 (DT) : date and time, see details below +BYTE 9-12 (LO) : longitude (x10^7, degree decimal) +BYTE 13-16 (LA): latitude (x10^7, degree decimal) +BYTE 17-20 (AL): altitude (in milimeters) +BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details) +BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details) +BYTE 29-32 : ??? (seems to be always 0) +BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details) +BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details) +BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details) +BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details) +BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details) +BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details) +BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details) +BYTE 53 (NS) : number of satellites (not XORed) +BYTE 54 : ??? (not XORed, seems to be always 0) +BYTE 55 (FT) : fix type (0 - no lock, 2 - 2D lock, 3 - 3D lock, not sure if other values can be expected + : see uBlox NAV-SOL message for details) +BYTE 56 : ??? (seems to be always 0) +BYTE 57 (SF) : fix status flags (see uBlox NAV-SOL message for details) +BYTE 58-59 : ??? (seems to be always 0) +BYTE 60 (XM) : not sure yet, but I use it as the XOR mask +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. + +CHECKSUM +----------------- +BYTE 63-64 (CS): checksum, calculated the same way as for uBlox binary messages + + +XOR mask +--------------- +All bytes of the payload except 53rd (NS), 54th, 61st (SN LSB) and 62nd (SN MSB) are XORed with a mask. +Mask is calculated based on the value of byte 53rd (NS) and 61st (SN LSB). + +If we index bits from LSB to MSB as 0-7 we have: +mask[0] = 53rdByte[0] xor 61stByte[4] +mask[1] = 53rdByte[1] xor 61stByte[5] +mask[2] = 53rdByte[2] xor 61stByte[6] +mask[3] = 53rdByte[3] xor 61stByte[7] xor 53rdByte[0]; +mask[4] = 53rdByte[1]; +mask[5] = 53rdByte[2]; +mask[6] = 53rdByte[3]; +mask[7] = 53rdByte[0] xor 61stByte[4]; + +To simplify calculations any of the unknown bytes that when XORer seem to be always 0 (29-32, 56, 58-60) +can be used as XOR mask (based on the fact that 0 XOR mask == mask). In the library I use byte 60. + +Date and time format +---------------------------- +Date (Year, Month, Day) and time (Hour, Minute, Second) are stored as little endian 32bit unsigned integer, +the meaning of particular bits is as follows: + +YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS + +NOTE 1: to get the day value correct you must add 1 when hour is > 7 +NOTE 2: for the time between 16:00 and 23:59 the hour will be returned as 0-7 + and there seems to be no way to differentiate between 00:00 - 07:59 and 16:00 - 23:59. +From further discussion in the thread, it sounds like the day is written into the buffer +(buffer initially zero bits) and then the hour is xored into the buffer +with the bottom bit of day and top bit of hour mapped to the same buffer bit? +Is that even correct? Or could we have a correct hour and the day is just wrong? + +http://www.rcgroups.com/forums/showpost.php?p=28158918&postcount=180 +Midnight between 13th and 14th of March +0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0 +identical with +4PM, 14th of March +0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0 + +Midnight between 14th and 15th of March +0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16 +identical with +4PM, 15th of March +0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16 + +So as you can see even if we take 5 bits the hour is not correct either +Are they are xored? If we knew the date from a different source we would know the time. +Maybe the xor mask itself contains the bit. Does the mask change? and how? across the transitions. + +http://www.rcgroups.com/forums/showpost.php?p=28168741&postcount=182 +Originally Posted by gwouite View Post + Question, are you sure that at 4PM, you're day value doesn't increase of 1 ? +It does, but it also does decrease by 1 at 8am so you have: +00:00 - 07:59 => day = X, hour = 0 - 7 +08:00 - 15:59 => day = X - 1, hour = 8 - 15 +16:00 - 23:59 => day = X, hour = 0 - 7 + +http://www.rcgroups.com/forums/showpost.php?p=28782603&postcount=218 +Here is the SBAS config from the Naza GPS +CFG-SBAS - 06 16 08 00 01 03 03 00 51 62 06 00 +If I read it correctly EGNOS (PRN 124/124/126), MSAS (PRN 129/137) and WAAS (PRN 133/134/135/138) are enabled. +*/ + +// DJI GPS packet +struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct offset + struct { // YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS + uint32_t sec:6; + uint32_t min:6; + uint32_t hour:4; + uint32_t day:5; + uint32_t month:4; + uint32_t year:7; + }; // BYTE 5-8 (DT): date and time, see details above + int32_t lon; // BYTE 9-12 (LO): longitude (x10^7, degree decimal) + int32_t lat; // BYTE 13-16 (LA): latitude (x10^7, degree decimal) + int32_t hMSL; // BYTE 17-20 (AL): altitude (in millimeters) (is this MSL or geoid?) + uint32_t hAcc; // BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details) + uint32_t vAcc; // BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details) + uint32_t unused1; // BYTE 29-32: ??? (seems to be always 0) + int32_t velN; // BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details) + int32_t velE; // BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details) + int32_t velD; // BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details) + uint16_t pDOP; // BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details) + uint16_t vDOP; // BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details) + uint16_t nDOP; // BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details) + uint16_t eDOP; // BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details) + uint8_t numSV; // BYTE 53 (NS): number of satellites (not XORed) + uint8_t unused2; // BYTE 54: ??? (not XORed, seems to be always 0) + uint8_t fixType; // BYTE 55 (FT): fix type (0 - no lock, 2 - 2D lock, 3 - 3D lock, not sure if other values can be expected + // see uBlox NAV-SOL message for details) + uint8_t unused3; // BYTE 56: ??? (seems to be always 0) + uint8_t flags; // BYTE 57 (SF): fix status flags (see uBlox NAV-SOL message for details) + 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 + 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. +} __attribute__((packed)); + +#define FLAGS_GPSFIX_OK (1 << 0) +#define FLAGS_DIFFSOLN (1 << 1) +#define FLAGS_WKNSET (1 << 2) +#define FLAGS_TOWSET (1 << 3) + +#define FIXTYPE_NO_FIX 0 +#define FIXTYPE_DEAD_RECKON 0x01 // Dead Reckoning only +#define FIXTYPE_2D 0x02 // 2D-Fix +#define FIXTYPE_3D 0x03 // 3D-Fix +#define FIXTYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined +#define FIXTYPE_TIME_ONLY 0x05 // Time only fix + + +/* +mag protocol info from +http://www.rcgroups.com/forums/showpost.php?p=26248426&postcount=62 + +The 0x20 message contains compass data. Here is the structure of the message, +fields marked with XX I'm not sure about yet. The others will be described below. + +55 AA 20 06 CX CX CY CY CZ CZ CS CS + +Values in the message are stored in little endian. + +HEADER +------------- +BYTE 1-2: message header - always 55 AA +BYTE 3: message id (0x20 for compass message) +BYTE 4: length of the payload (0x06 or 6 decimal for 0x20 message) + +PAYLOAD +-------------- +BYTE 5-6 (CX): compass X axis data (signed) - see comments below +BYTE 7-8 (CY): compass Y axis data (signed) - see comments below +BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below + +CHECKSUM +----------------- +BYTE 11-12 (CS): checksum, calculated the same way as for uBlox binary messages + +All the bytes of the payload except 9th are XORed with a mask. +Mask is calculated based on the value of the 9th byte. + +If we index bits from LSB to MSB as 0-7 we have: +mask[0] = 9thByte[0] xor 9thByte[4] +mask[1] = 9thByte[1] xor 9thByte[5] +mask[2] = 9thByte[2] xor 9thByte[6] +mask[3] = 9thByte[3] xor 9thByte[7] xor 9thByte[0]; +mask[4] = 9thByte[1]; +mask[5] = 9thByte[2]; +mask[6] = 9thByte[3]; +mask[7] = 9thByte[4] xor 9thByte[0]; + +To calculate the heading (not tilt compensated) you need to do atan2 on the resulting +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 + 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 z; // BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below +} __attribute__((packed)); + + +/* +version info from +http://www.rcgroups.com/forums/showpost.php?p=27058649&postcount=120 + +This is still to be confirmed but I believe the 0x30 message carries the GPS module hardware id and firmware version. + +55 AA 30 0C XX XX XX XX FW FW FW FW HW HW HW HW CS CS + +Note that you need to read version numbers backwards (02 01 00 06 means v6.0.1.2) + +HEADER +------------- +BYTE 1-2: message header - always 55 AA +BYTE 3: message id (0x30 for GPS module versions message) +BYTE 4: length of the payload (0x0C or 12 decimal for 0x30 message) + +PAYLOAD +-------------- +BYTE 5-8" ??? (seems to be always 0) +BYTE 9-12 (FW): firmware version +BYTE 13-16 (HW): hardware id + +CHECKSUM +----------------- +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 + uint32_t unused1; // BYTE 5-8" ??? (seems to be always 0) + uint32_t swVersion; // BYTE 9-12 (FW): firmware version + uint32_t hwVersion; // BYTE 13-16 (HW): hardware id +} __attribute__((packed)); + + + +typedef union { + uint8_t payload[0]; + // Nav Class + struct DJI_GPS gps; + struct DJI_MAG mag; + struct DJI_VER ver; +} DJIPayload; + +struct DJIHeader { + uint8_t id; + uint8_t len; + uint8_t ck_a; // 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 +} __attribute__((packed)); + +struct DJIPacket { + struct DJIHeader header; + DJIPayload payload; +} __attribute__((packed)); + +extern GPSPositionSensorSensorTypeOptions sensorType; + +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(); + +#endif /* DJI_H */ diff --git a/flight/modules/GPS/inc/UBX.h b/flight/modules/GPS/inc/UBX.h index 8263da2ca..96ae75ab8 100644 --- a/flight/modules/GPS/inc/UBX.h +++ b/flight/modules/GPS/inc/UBX.h @@ -7,7 +7,8 @@ * @{ * * @file UBX.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief GPS module, handles GPS and NMEA stream * @see The GNU Public License (GPL) Version 3 * @@ -621,5 +622,6 @@ uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *); int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *); void op_gpsv9_load_mag_settings(); +void aux_hmc5x83_load_mag_settings(); #endif /* UBX_H */ diff --git a/flight/modules/GPS/ubx_autoconfig.c b/flight/modules/GPS/ubx_autoconfig.c index 3e0adf012..0fb8fc732 100644 --- a/flight/modules/GPS/ubx_autoconfig.c +++ b/flight/modules/GPS/ubx_autoconfig.c @@ -7,7 +7,8 @@ * @{ * * @file ubx_autoconfig.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief Support code for UBX AutoConfig * @see The GNU Public License (GPL) Version 3 * @@ -849,6 +850,8 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send) } +// (re)init the autoconfig stuff so that it will run if called +// // this can be called from a different thread // so everything it touches must be declared volatile void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config) @@ -883,17 +886,17 @@ void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config) status->currentStep = new_step; status->currentStepSave = new_step; + // this forces the sensor type detection to occur outside the FSM + // and _can_ also engage the autobaud detection that is outside the FSM + // don't do it if FSM is enabled as FSM can change the baud itself + // (don't do it because the baud rates are already in sync) + gps_ubx_reset_sensor_type(); + if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) { // enabled refers to autoconfigure // note that sensor type (gps type) detection happens even if completely disabled - // also note that AutoBaud is less than Configure + // also note that AutoBaud is less than AutoBaudAndConfigure enabled = true; - } else { - // this forces the sensor type detection to occur outside the FSM - // and _can_ also engage the autobaud detection that is outside the FSM - // don't do it if FSM is enabled as FSM can change the baud itself - // (don't do it because the baud rates are already in sync) - gps_ubx_reset_sensor_type(); } } diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index d91a7c7d8..24af7b648 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -7,7 +7,8 @@ * @{ * * @file sensors.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. * @brief Module to handle fetch and preprocessing of sensor data * * @see The GNU Public License (GPL) Version 3 @@ -62,6 +63,7 @@ #include #include #include +#include #include #include @@ -128,6 +130,10 @@ PERF_DEFINE_COUNTER(counterBaroPeriod); PERF_DEFINE_COUNTER(counterSensorPeriod); PERF_DEFINE_COUNTER(counterSensorResets); +#if defined(PIOS_INCLUDE_HMC5X83) +void aux_hmc5x83_load_settings(); +#endif + // Private functions static void SensorsTask(void *parameters); static void settingsUpdatedCb(UAVObjEvent *objEv); @@ -141,7 +147,9 @@ static void clearContext(sensor_fetch_context *sensor_context); static void handleAccel(float *samples, float temperature); static void handleGyro(float *samples, float temperature); static void handleMag(float *samples, float temperature); +#if defined(PIOS_INCLUDE_HMC5X83) static void handleAuxMag(float *samples); +#endif static void handleBaro(float sample, float temperature); static void updateAccelTempBias(float temperature); @@ -184,6 +192,12 @@ static float baro_temp_bias = 0; static float baro_temperature = NAN; static uint8_t baro_temp_calibration_count = 0; +#if defined(PIOS_INCLUDE_HMC5X83) +// Allow AuxMag to be disabled without reboot +// because the other mags are that way +static bool useMag = false; +#endif + /** * Initialise the module. Called before the start function * \returns 0 on success or -1 if initialisation failed @@ -200,9 +214,11 @@ int32_t SensorsInitialize(void) AttitudeSettingsInitialize(); AccelGyroSettingsInitialize(); +#if defined(PIOS_INCLUDE_HMC5X83) // for auxmagsupport.c helpers AuxMagSettingsInitialize(); AuxMagSensorInitialize(); +#endif RevoSettingsConnectCallback(&settingsUpdatedCb); RevoCalibrationConnectCallback(&settingsUpdatedCb); @@ -366,9 +382,12 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE); float inv_count = 1.0f / (float)sensor_context->count; - if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) || - (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) || - (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)) { + if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) + || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) +#if defined(PIOS_INCLUDE_HMC5X83) + || (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG) +#endif + ) { float t = inv_count * scales[0]; samples[0] = ((float)sensor_context->accum[0].x * t); samples[1] = ((float)sensor_context->accum[0].y * t); @@ -379,12 +398,12 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE handleMag(samples, temperature); PERF_MEASURE_PERIOD(counterMagPeriod); return; - +#if defined(PIOS_INCLUDE_HMC5X83) case PIOS_SENSORS_TYPE_3AXIS_AUXMAG: handleAuxMag(samples); PERF_MEASURE_PERIOD(counterMagPeriod); return; - +#endif default: PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count); PERF_MEASURE_PERIOD(counterAccelPeriod); @@ -473,10 +492,14 @@ static void handleMag(float *samples, float temperature) MagSensorSet(&mag); } +#if defined(PIOS_INCLUDE_HMC5X83) static void handleAuxMag(float *samples) { - auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK); + if (useMag) { + auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK); + } } +#endif static void handleBaro(float sample, float temperature) { @@ -607,6 +630,18 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) fabsf(baroCorrection.c) > 1e-9f || fabsf(baroCorrection.d) > 1e-9f)); } + +#if defined(PIOS_INCLUDE_HMC5X83) +void aux_hmc5x83_load_mag_settings() { + uint8_t magType = auxmagsupport_get_type(); + + if (magType == AUXMAGSETTINGS_TYPE_I2C || magType == AUXMAGSETTINGS_TYPE_FLEXI) { + useMag = true; + } else { + useMag = false; + } +} +#endif /** * @} * @} diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 8bf8f3634..b207bd0c2 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -148,6 +148,7 @@ #define PIOS_GPS_MINIMAL /* #define PIOS_INCLUDE_GPS_NMEA_PARSER */ #define PIOS_INCLUDE_GPS_UBX_PARSER +/* #define PIOS_INCLUDE_GPS_DJI_PARSER */ /* #define PIOS_GPS_SETS_HOMELOCATION */ /* Stabilization options */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index d82ff41d6..8accd89be 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -1,5 +1,5 @@ # -# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org +# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org # Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org # Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot # @@ -32,7 +32,7 @@ USE_CXX = YES USE_DSP_LIB ?= NO # List of mandatory modules to include -#MODULES += Sensors +MODULES += Sensors #MODULES += Attitude/revolution #MODULES += StateEstimation # use instead of Attitude #MODULES += Altitude/revolution diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index a551f858f..5138b10d9 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -5,7 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 @@ -149,6 +149,7 @@ /* #define PIOS_GPS_MINIMAL */ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index a835f6a99..75ef3286a 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -141,6 +141,7 @@ /* #define PIOS_GPS_MINIMAL */ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 4328266bf..3a068a05a 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -5,7 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @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. * Defines which PiOS libraries and features are included in the firmware. @@ -153,6 +153,7 @@ /* #define PIOS_GPS_MINIMAL */ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index 7cfe391db..984b77959 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -5,7 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * @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. * Defines which PiOS libraries and features are included in the firmware. @@ -157,6 +157,7 @@ /* #define PIOS_GPS_MINIMAL */ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 270c9507a..468264855 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -5,7 +5,8 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * @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. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 @@ -141,6 +142,7 @@ /* #define PIOS_GPS_MINIMAL */ #define PIOS_INCLUDE_GPS_NMEA_PARSER #define PIOS_INCLUDE_GPS_UBX_PARSER +#define PIOS_INCLUDE_GPS_DJI_PARSER #define PIOS_GPS_SETS_HOMELOCATION /* Stabilization options */ diff --git a/shared/uavobjectdefinition/auxmagsettings.xml b/shared/uavobjectdefinition/auxmagsettings.xml index f4a4a882d..99ba5cea3 100644 --- a/shared/uavobjectdefinition/auxmagsettings.xml +++ b/shared/uavobjectdefinition/auxmagsettings.xml @@ -6,7 +6,7 @@ defaultvalue="1,0,0,0,1,0,0,0,1"/> - + diff --git a/shared/uavobjectdefinition/gpspositionsensor.xml b/shared/uavobjectdefinition/gpspositionsensor.xml index 1e53c3e0d..566678a4b 100644 --- a/shared/uavobjectdefinition/gpspositionsensor.xml +++ b/shared/uavobjectdefinition/gpspositionsensor.xml @@ -12,7 +12,7 @@ - + diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml index 57f02f492..ac20c56fd 100644 --- a/shared/uavobjectdefinition/gpssettings.xml +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -1,7 +1,7 @@ GPS Module specific settings - +