flight/modules/GPS/DJI.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup GPSModule GPS Module
   6  * @brief Process GPS information (DJI-Naza binary format)
   7  * @{
   8  *
   9  * @file       DJI.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  * @brief      GPS module, handles DJI stream
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "openpilot.h"
  32 #include "pios.h"
  33 #include "pios_math.h"
  34 #include <pios_helpers.h>
  35 #include <pios_delay.h>
  36
  37 #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
  38
  39 #include "inc/DJI.h"
  40 #include "inc/GPS.h"
  41 #include <string.h>
  42 #include <auxmagsupport.h>
  43
  44 // parsing functions
  45 static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
  46 #if !defined(PIOS_GPS_MINIMAL)
  47 static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
  48 static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
  49 #endif /* !defined(PIOS_GPS_MINIMAL) */
  50
  51 static bool checksum_dji_message(struct DJIPacket *dji);
  52 static uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
  53
  54 // parse table item
  55 typedef struct {
  56     uint8_t msgId;
  57     void (*handler)(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
  58 } djiMessageHandler;
  59
  60 const djiMessageHandler djiHandlerTable[] = {
  61     { .msgId = DJI_ID_GPS, .handler = &parse_dji_gps },
  62 #if !defined(PIOS_GPS_MINIMAL)
  63     { .msgId = DJI_ID_MAG, .handler = &parse_dji_mag },
  64     { .msgId = DJI_ID_VER, .handler = &parse_dji_ver },
  65 #endif /* !defined(PIOS_GPS_MINIMAL) */
  66 };
  67 #define DJI_HANDLER_TABLE_SIZE NELEMENTS(djiHandlerTable)
  68
  69 #if !defined(PIOS_GPS_MINIMAL)
  70 static bool useMag    = false;
  71
  72 // detected hw version
  73 uint32_t djiHwVersion = -1;
  74 uint32_t djiSwVersion = -1;
  75 #endif /* !defined(PIOS_GPS_MINIMAL) */
  76
  77
  78 // parse incoming character stream for messages in DJI binary format
  79 #define djiPacket ((struct DJIPacket *)parsedDjiStruct)
  80 int parse_dji_stream(uint8_t *inputBuffer, uint16_t inputBufferLength, char *parsedDjiStruct, GPSPositionSensorData *gpsPosition, struct GPS_RX_STATS *gpsRxStats)
  81 {
  82     enum ProtocolStates {
  83         START,
  84         DJI_SY2,
  85         DJI_ID,
  86         DJI_LEN,
  87         DJI_PAYLOAD,
  88         DJI_CHK1,
  89         DJI_CHK2,
  90         FINISHED
  91     };
  92     enum RestartStates {
  93         RESTART_WITH_ERROR,
  94         RESTART_NO_ERROR
  95     };
  96     static uint16_t payloadCount   = 0;
  97     static enum ProtocolStates protocolState = START;
  98     static bool previousPacketGood = true;
  99     int ret = PARSER_INCOMPLETE; // message not (yet) complete
 100     uint16_t inputBufferIndex = 0;
 101     uint16_t restartIndex     = 0;  // input buffer location to restart from
 102     enum RestartStates restartState;
 103     uint8_t inputByte;
 104     bool currentPacketGood;
 105
 106     // switch continue is the normal condition and comes back to here for another byte
 107     // switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
 108     while (inputBufferIndex < inputBufferLength) {
 109         inputByte = inputBuffer[inputBufferIndex++];
 110         switch (protocolState) {
 111         case START: // detect protocol
 112             if (inputByte == DJI_SYNC1) { // first DJI sync char found
 113                 protocolState = DJI_SY2;
 114                 // restart here, at byte after SYNC1, if we fail to parse
 115                 restartIndex  = inputBufferIndex;
 116             }
 117             continue;
 118         case DJI_SY2:
 119             if (inputByte == DJI_SYNC2) { // second DJI sync char found
 120                 protocolState = DJI_ID;
 121             } else {
 122                 restartState = RESTART_NO_ERROR;
 123                 break;
 124             }
 125             continue;
 126         case DJI_ID:
 127             djiPacket->header.id = inputByte;
 128             protocolState = DJI_LEN;
 129             continue;
 130         case DJI_LEN:
 131             if (inputByte > sizeof(DJIPayload)) {
 132                 gpsRxStats->gpsRxOverflow++;
 133                 restartState = RESTART_WITH_ERROR;
 134                 break;
 135             } else {
 136                 djiPacket->header.len = inputByte;
 137                 if (inputByte == 0) {
 138                     protocolState = DJI_CHK1;
 139                 } else {
 140                     payloadCount  = 0;
 141                     protocolState = DJI_PAYLOAD;
 142                 }
 143             }
 144             continue;
 145         case DJI_PAYLOAD:
 146             if (payloadCount < djiPacket->header.len) {
 147                 djiPacket->payload.payload[payloadCount] = inputByte;
 148                 if (++payloadCount == djiPacket->header.len) {
 149                     protocolState = DJI_CHK1;
 150                 }
 151             }
 152             continue;
 153         case DJI_CHK1:
 154             djiPacket->header.checksumA = inputByte;
 155             protocolState = DJI_CHK2;
 156             continue;
 157         case DJI_CHK2:
 158             djiPacket->header.checksumB = inputByte;
 159             // ignore checksum errors on correct mag packets that nonetheless have checksum errors
 160             // these checksum errors happen very often on clone DJI GPS (never on real DJI GPS)
 161             // and are caused by a clone DJI GPS firmware error
 162             // the errors happen when it is time to send a non-mag packet (4 or 5 per second)
 163             // instead of a mag packet (30 per second)
 164             currentPacketGood = checksum_dji_message(djiPacket);
 165             // message complete and valid or (it's a mag packet and the previous "any" packet was good)
 166             if (currentPacketGood || (djiPacket->header.id == DJI_ID_MAG && previousPacketGood)) {
 167                 parse_dji_message(djiPacket, gpsPosition);
 168                 gpsRxStats->gpsRxReceived++;
 169                 protocolState = START;
 170                 // overwrite PARSER_INCOMPLETE with PARSER_COMPLETE
 171                 // but don't overwrite PARSER_ERROR with PARSER_COMPLETE
 172                 // pass PARSER_ERROR to caller if it happens even once
 173                 // only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data
 174                 // that allows the caller to know if we are parsing GPS data
 175                 // or just other packets for some reason (DJI clone firmware bug that happens sometimes)
 176                 if (djiPacket->header.id == DJI_ID_GPS && ret == PARSER_INCOMPLETE) {
 177                     ret = PARSER_COMPLETE; // message complete & processed
 178                 }
 179             } else {
 180                 gpsRxStats->gpsRxChkSumError++;
 181                 restartState = RESTART_WITH_ERROR;
 182                 previousPacketGood = false;
 183                 break;
 184             }
 185             previousPacketGood = currentPacketGood;
 186             continue;
 187         default:
 188             continue;
 189         }
 190
 191         // this simple restart doesn't work across calls
 192         // but it does work within a single call
 193         // and it does the expected thing across calls
 194         // if restarting due to error detected in 2nd call to this function (on split packet)
 195         // then we just restart at index 0, which is mid-packet, not the second byte
 196         if (restartState == RESTART_WITH_ERROR) {
 197             ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
 198         }
 199         inputBuffer       += restartIndex; // restart parsing just past the most recent SYNC1
 200         inputBufferLength -= restartIndex;
 201         inputBufferIndex   = 0;
 202         protocolState      = START;
 203     }
 204
 205     return ret;
 206 }
 207
 208
 209 bool checksum_dji_message(struct DJIPacket *dji)
 210 {
 211     int i;
 212     uint8_t checksumA, checksumB;
 213
 214     checksumA  = dji->header.id;
 215     checksumB  = checksumA;
 216
 217     checksumA += dji->header.len;
 218     checksumB += checksumA;
 219
 220     for (i = 0; i < dji->header.len; i++) {
 221         checksumA += dji->payload.payload[i];
 222         checksumB += checksumA;
 223     }
 224
 225     if (dji->header.checksumA == checksumA &&
 226         dji->header.checksumB == checksumB) {
 227         return true;
 228     } else {
 229         return false;
 230     }
 231 }
 232
 233
 234 static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition)
 235 {
 236     GPSVelocitySensorData gpsVelocity;
 237     struct DjiGps *djiGps = &dji->payload.gps;
 238
 239     // decode with xor mask
 240     uint8_t mask = djiGps->unused5;
 241
 242     // some bytes at the end are not xored
 243     // some bytes in the middle are not xored
 244     for (uint8_t i = 0; i < GPS_DECODED_LENGTH; ++i) {
 245         if (i != GPS_NOT_XORED_BYTE_1 && i != GPS_NOT_XORED_BYTE_2) {
 246             dji->payload.payload[i] ^= mask;
 247         }
 248     }
 249
 250     gpsVelocity.North = (float)djiGps->velN * 0.01f;
 251     gpsVelocity.East  = (float)djiGps->velE * 0.01f;
 252     gpsVelocity.Down  = (float)djiGps->velD * 0.01f;
 253     GPSVelocitySensorSet(&gpsVelocity);
 254
 255 #if !defined(PIOS_GPS_MINIMAL)
 256     gpsPosition->Groundspeed = sqrtf(gpsVelocity.North * gpsVelocity.North + gpsVelocity.East * gpsVelocity.East);
 257 #else
 258     gpsPosition->Groundspeed = fmaxf(gpsVelocity.North, gpsVelocity.East) * 1.2f; // within 20% or so
 259 #endif /* !defined(PIOS_GPS_MINIMAL) */
 260     // don't allow a funny number like 4.87416e-06 to show up in Uavo Browser for Heading
 261     // smallest groundspeed is 0.01f from (int)1 * (float)0.01
 262     // so this is saying if groundspeed is zero
 263     if (gpsPosition->Groundspeed < 0.009f) {
 264         gpsPosition->Heading = 0.0f;
 265     } else {
 266         gpsPosition->Heading = RAD2DEG(atan2f(-gpsVelocity.East, -gpsVelocity.North)) + 180.0f;
 267     }
 268     gpsPosition->Altitude = (float)djiGps->hMSL * 0.001f;
 269     // there is no source of geoid separation data in the DJI protocol
 270     // Is there a reasonable world model calculation we can do to get a value for geoid separation?
 271     gpsPosition->GeoidSeparation = 0.0f;
 272     gpsPosition->Latitude   = djiGps->lat;
 273     gpsPosition->Longitude  = djiGps->lon;
 274     gpsPosition->Satellites = djiGps->numSV;
 275     gpsPosition->PDOP = djiGps->pDOP * 0.01f;
 276 #if !defined(PIOS_GPS_MINIMAL)
 277     gpsPosition->HDOP = sqrtf((float)djiGps->nDOP * (float)djiGps->nDOP + (float)djiGps->eDOP * (float)djiGps->eDOP) * 0.01f;
 278     if (gpsPosition->HDOP > 99.99f) {
 279         gpsPosition->HDOP = 99.99f;
 280     }
 281 #else
 282     gpsPosition->HDOP = MAX(djiGps->nDOP, djiGps->eDOP) * 0.01f;
 283 #endif
 284     gpsPosition->VDOP = djiGps->vDOP * 0.01f;
 285     if (djiGps->flags & FLAGS_GPSFIX_OK) {
 286         gpsPosition->Status = djiGps->fixType == FIXTYPE_3D ?
 287                               GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
 288     } else {
 289         gpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 290     }
 291     gpsPosition->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
 292     gpsPosition->AutoConfigStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED;
 293     GPSPositionSensorSet(gpsPosition);
 294
 295 #if !defined(PIOS_GPS_MINIMAL)
 296     // Time is valid, set GpsTime
 297     GPSTimeData gpsTime;
 298     // the lowest bit of day and the highest bit of hour overlap (xored? no, stranger than that)
 299     // this causes strange day/hour changes
 300     // we could track it here and even if we guess wrong initially
 301     // we can massage the data so that time doesn't jump at least
 302     // and maybe make the assumption that most people will fly at 5pm, not 1am
 303     // this is part of the DJI protocol
 304     // see DJI.h for further info
 305     gpsTime.Year   = (int16_t)djiGps->year + 2000;
 306     gpsTime.Month  = djiGps->month;
 307     gpsTime.Day    = djiGps->day;
 308     gpsTime.Hour   = djiGps->hour;
 309     gpsTime.Minute = djiGps->min;
 310     gpsTime.Second = djiGps->sec;
 311     GPSTimeSet(&gpsTime);
 312 #endif /* !defined(PIOS_GPS_MINIMAL) */
 313 }
 314
 315
 316 #if !defined(PIOS_GPS_MINIMAL)
 317 void dji_load_mag_settings()
 318 {
 319     if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_DJI) {
 320         useMag = true;
 321     } else {
 322         useMag = false;
 323     }
 324 }
 325
 326
 327 static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition)
 328 {
 329     if (!useMag) {
 330         return;
 331     }
 332     struct DjiMag *mag = &dji->payload.mag;
 333     union {
 334         struct {
 335             int8_t mask;
 336             int8_t mask2;
 337         };
 338         int16_t maskmask;
 339     } u;
 340     u.mask = (int8_t)(dji->payload.payload[4]);
 341     u.mask = u.mask2 = (((u.mask ^ (u.mask >> 4)) & 0x0F) | ((u.mask << 3) & 0xF0)) ^ (((u.mask & 0x01) << 3) | ((u.mask & 0x01) << 7));
 342     // yes, z is only xored by mask<<8, not maskmask
 343     float mags[3] = { mag->x ^ u.maskmask, mag->y ^ u.maskmask, mag->z ^ ((int16_t)u.mask << 8) };
 344     auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
 345 }
 346
 347
 348 static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition)
 349 {
 350     {
 351         struct DjiVer *ver = &dji->payload.ver;
 352         // decode with xor mask
 353         uint8_t mask = (uint8_t)(ver->unused1);
 354
 355         // first 4 bytes are unused and 0 before the encryption
 356         // so any one of them can be used for the decrypting xor mask
 357         for (uint8_t i = VER_FIRST_DECODED_BYTE; i < sizeof(struct DjiVer); ++i) {
 358             dji->payload.payload[i] ^= mask;
 359         }
 360
 361         djiHwVersion = ver->hwVersion;
 362         djiSwVersion = ver->swVersion;
 363     }
 364     {
 365         GPSPositionSensorSensorTypeOptions sensorType;
 366         sensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
 367         GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
 368     }
 369 }
 370 #endif /* !defined(PIOS_GPS_MINIMAL) */
 371
 372
 373 // DJI message parser
 374 // returns UAVObjectID if a UAVObject structure is ready for further processing
 375 uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition)
 376 {
 377     static bool djiInitialized = false;
 378     uint32_t id = 0;
 379
 380     if (!djiInitialized) {
 381         // initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
 382         gpsPosition->HDOP = 99.99f;
 383         gpsPosition->PDOP = 99.99f;
 384         gpsPosition->VDOP = 99.99f;
 385         djiInitialized    = true;
 386     }
 387
 388     for (uint8_t i = 0; i < DJI_HANDLER_TABLE_SIZE; i++) {
 389         const djiMessageHandler *handler = &djiHandlerTable[i];
 390         if (handler->msgId == dji->header.id) {
 391             handler->handler(dji, gpsPosition);
 392             break;
 393         }
 394     }
 395
 396     {
 397         uint8_t status;
 398         GPSPositionSensorStatusGet(&status);
 399         if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
 400             // Some dji thing has been received so GPS is there
 401             status = GPSPOSITIONSENSOR_STATUS_NOFIX;
 402             GPSPositionSensorStatusSet(&status);
 403         }
 404     }
 405
 406     return id;
 407 }
 408 #endif // PIOS_INCLUDE_GPS_DJI_PARSER