flight/modules/GPS/inc/DJI.h

   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.h
  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 #ifndef DJI_H
  32 #define DJI_H
  33 #include "openpilot.h"
  34 #include "gpspositionsensor.h"
  35 #include "gpsextendedstatus.h"
  36 #ifndef PIOS_GPS_MINIMAL
  37 #include "auxmagsensor.h"
  38 #endif
  39
  40 #include "GPS.h"
  41
  42 #define DJI_SYNC1 0x55 // DJI protocol synchronization characters
  43 #define DJI_SYNC2 0xaa
  44
  45 // Message IDs
  46 typedef enum {
  47     DJI_ID_GPS = 0x10,
  48     DJI_ID_MAG = 0x20,
  49     DJI_ID_VER = 0x30,
  50 } dji_msg_id;
  51
  52 // private structures
  53
  54 /*
  55    GPS protocol info from
  56    http://www.rcgroups.com/forums/showpost.php?p=26210591&postcount=15
  57
  58    The 0x10 message contains GPS data. Here is the structure of the message,
  59    fields marked with XX I'm not sure about yet. The others will be described below.
  60
  61    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
  62    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
  63
  64    The payload is XORed with a mask that changes over time (see below for more details).
  65
  66    Values in the message are stored in little endian.
  67
  68    HEADER
  69    -------------
  70    BYTE 1-2: message header - always 55 AA
  71    BYTE 3: message id (0x10 for GPS message)
  72    BYTE 4: lenght of the payload (0x3A or 58 decimal for 0x10 message)
  73
  74    PAYLOAD
  75    --------------
  76    BYTE 5-8 (DT)  : date and time, see details below
  77    BYTE 9-12 (LO) : longitude (x10^7, degree decimal)
  78    BYTE 13-16 (LA): latitude (x10^7, degree decimal)
  79    BYTE 17-20 (AL): altitude (in milimeters)
  80    BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details)
  81    BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details)
  82    BYTE 29-32     : ??? (seems to be always 0)
  83    BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details)
  84    BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details)
  85    BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details)
  86    BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details)
  87    BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details)
  88    BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details)
  89    BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details)
  90    BYTE 53 (NS)   : number of satellites (not XORed)
  91    BYTE 54        : ??? (not XORed, seems to be always 0)
  92    BYTE 55 (FT)   : fix type (0 - no lock, 2 - 2D lock, 3 - 3D lock, not sure if other values can be expected
  93                : see uBlox NAV-SOL message for details)
  94    BYTE 56        : ??? (seems to be always 0)
  95    BYTE 57 (SF)   : fix status flags (see uBlox NAV-SOL message for details)
  96    BYTE 58-59     : ??? (seems to be always 0)
  97    BYTE 60 (XM)   : not sure yet, but I use it as the XOR mask
  98    BYTE 61-62 (SN): sequence number (not XORed), once there is a lock - increases with every message.
  99                  When the lock is lost later LSB and MSB are swapped with every message.
 100
 101    CHECKSUM
 102    -----------------
 103    BYTE 63-64 (CS): checksum, calculated the same way as for uBlox binary messages
 104
 105
 106    XOR mask
 107    ---------------
 108    All bytes of the payload except 53rd (NS), 54th, 61st (SN LSB) and 62nd (SN MSB) are XORed with a mask.
 109    Mask is calculated based on the value of byte 53rd (NS) and 61st (SN LSB).
 110
 111    If we index bits from LSB to MSB as 0-7 we have:
 112    mask[0] = 53rdByte[0] xor 61stByte[4]
 113    mask[1] = 53rdByte[1] xor 61stByte[5]
 114    mask[2] = 53rdByte[2] xor 61stByte[6]
 115    mask[3] = 53rdByte[3] xor 61stByte[7] xor 53rdByte[0];
 116    mask[4] = 53rdByte[1];
 117    mask[5] = 53rdByte[2];
 118    mask[6] = 53rdByte[3];
 119    mask[7] = 53rdByte[0] xor 61stByte[4];
 120
 121    To simplify calculations any of the unknown bytes that when XORer seem to be always 0 (29-32, 56, 58-60)
 122    can be used as XOR mask (based on the fact that 0 XOR mask == mask). In the library I use byte 60.
 123
 124    Date and time format
 125    ----------------------------
 126    Date (Year, Month, Day) and time (Hour, Minute, Second) are stored as little endian 32bit unsigned integer,
 127    the meaning of particular bits is as follows:
 128
 129    YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS
 130
 131    NOTE 1: to get the day value correct you must add 1 when hour is > 7
 132    NOTE 2: for the time between 16:00 and 23:59 the hour will be returned as 0-7
 133         and there seems to be no way to differentiate between 00:00 - 07:59 and 16:00 - 23:59.
 134
 135    From further discussion in the thread, it sounds like the day is written into the buffer
 136    (buffer initially zero bits) and then the hour is xored into the buffer
 137    with the bottom bit of day and top bit of hour mapped to the same buffer bit?
 138    Is that even correct?  Or could we have a correct hour and the day is just wrong?
 139
 140    http://www.rcgroups.com/forums/showpost.php?p=28158918&postcount=180
 141    Midnight between 13th and 14th of March
 142    0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0
 143    identical with
 144    4PM, 14th of March
 145    0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0
 146
 147    Midnight between 14th and 15th of March
 148    0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16
 149    identical with
 150    4PM, 15th of March
 151    0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16
 152
 153    So as you can see even if we take 5 bits the hour is not correct either
 154    Are they are xored?  If we knew the date from a different source we would know the time.
 155    Maybe the xor mask itself contains the bit.  Does the mask change?  and how?  across the transitions.
 156
 157    http://www.rcgroups.com/forums/showpost.php?p=28168741&postcount=182
 158    Originally Posted by gwouite View Post
 159    Question, are you sure that at 4PM, you're day value doesn't increase of 1 ?
 160    It does, but it also does decrease by 1 at 8am so you have:
 161    00:00 - 07:59 => day = X, hour = 0 - 7
 162    08:00 - 15:59 => day = X - 1, hour = 8 - 15
 163    16:00 - 23:59 => day = X, hour = 0 - 7
 164
 165    http://www.rcgroups.com/forums/showpost.php?p=28782603&postcount=218
 166    Here is the SBAS config from the Naza GPS
 167    CFG-SBAS - 06 16 08 00 01 03 03 00 51 62 06 00
 168    If I read it correctly EGNOS (PRN 124/124/126), MSAS (PRN 129/137) and WAAS (PRN 133/134/135/138) are enabled.
 169  */
 170
 171 // DJI GPS packet
 172 struct DjiGps { // byte offset from beginning of packet, subtract 5 for struct offset
 173     struct { // YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS
 174         uint32_t sec : 6;
 175         uint32_t min : 6;
 176         uint32_t hour : 4;
 177         uint32_t day : 5;
 178         uint32_t month : 4;
 179         uint32_t year : 7;
 180     }; // BYTE 5-8 (DT): date and time, see details above
 181     int32_t  lon;     // BYTE 9-12 (LO): longitude (x10^7, degree decimal)
 182     int32_t  lat;     // BYTE 13-16 (LA): latitude (x10^7, degree decimal)
 183     int32_t  hMSL;    // BYTE 17-20 (AL): altitude (in millimeters) (is this MSL or geoid?)
 184     uint32_t hAcc; // BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details)
 185     uint32_t vAcc; // BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details)
 186     uint32_t unused1; // BYTE 29-32: ??? (seems to be always 0)
 187     int32_t  velN;    // BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details)
 188     int32_t  velE;    // BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details)
 189     int32_t  velD;    // BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details)
 190     uint16_t pDOP; // BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details)
 191     uint16_t vDOP; // BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details)
 192     uint16_t nDOP; // BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details)
 193     uint16_t eDOP; // BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details)
 194     uint8_t  numSV;   // BYTE 53 (NS): number of satellites (not XORed)
 195     uint8_t  unused2; // BYTE 54: ??? (not XORed, seems to be always 0)
 196     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
 197                       // see uBlox NAV-SOL message for details)
 198     uint8_t  unused3; // BYTE 56: ??? (seems to be always 0)
 199     uint8_t  flags;   // BYTE 57 (SF): fix status flags (see uBlox NAV-SOL message for details)
 200     uint16_t unused4; // BYTE 58-59: ??? (seems to be always 0)
 201     uint8_t  unused5; // BYTE 60 (XM): not sure yet, but I use it as the XOR mask
 202     uint16_t seqNo; // BYTE 61-62 (SN): sequence number (not XORed), once there is a lock
 203                     // increases with every message. When the lock is lost later LSB and MSB are swapped (in all messages where lock is lost).
 204 } __attribute__((packed));
 205
 206 #define FLAGS_GPSFIX_OK          (1 << 0)
 207 #define FLAGS_DIFFSOLN           (1 << 1)
 208 #define FLAGS_WKNSET             (1 << 2)
 209 #define FLAGS_TOWSET             (1 << 3)
 210
 211 #define FIXTYPE_NO_FIX           0x00 /* No Fix */
 212 #define FIXTYPE_DEAD_RECKON      0x01 /* Dead Reckoning only */
 213 #define FIXTYPE_2D               0x02 /* 2D-Fix */
 214 #define FIXTYPE_3D               0x03 /* 3D-Fix */
 215 #define FIXTYPE_GNSS_DEAD_RECKON 0x04 /* GNSS + dead reckoning combined */
 216 #define FIXTYPE_TIME_ONLY        0x05 /* Time only fix */
 217
 218 #define GPS_DECODED_LENGTH       offsetof(struct DjiGps, seqNo)
 219 #define GPS_NOT_XORED_BYTE_1     offsetof(struct DjiGps, numSV)
 220 #define GPS_NOT_XORED_BYTE_2     offsetof(struct DjiGps, unused2)
 221
 222
 223 /*
 224    mag protocol info from
 225    http://www.rcgroups.com/forums/showpost.php?p=26248426&postcount=62
 226
 227    The 0x20 message contains compass data. Here is the structure of the message,
 228    fields marked with XX I'm not sure about yet. The others will be described below.
 229
 230    55 AA 20 06 CX CX CY CY CZ CZ CS CS
 231
 232    Values in the message are stored in little endian.
 233
 234    HEADER
 235    -------------
 236    BYTE 1-2: message header - always 55 AA
 237    BYTE 3: message id (0x20 for compass message)
 238    BYTE 4: length of the payload (0x06 or 6 decimal for 0x20 message)
 239
 240    PAYLOAD
 241    --------------
 242    BYTE 5-6 (CX): compass X axis data (signed) - see comments below
 243    BYTE 7-8 (CY): compass Y axis data (signed) - see comments below
 244    BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below
 245
 246    CHECKSUM
 247    -----------------
 248    BYTE 11-12 (CS): checksum, calculated the same way as for uBlox binary messages
 249
 250    All the bytes of the payload except 9th are XORed with a mask.
 251    Mask is calculated based on the value of the 9th byte.
 252
 253    If we index bits from LSB to MSB as 0-7 we have:
 254    mask[0] = 9thByte[0] xor 9thByte[4]
 255    mask[1] = 9thByte[1] xor 9thByte[5]
 256    mask[2] = 9thByte[2] xor 9thByte[6]
 257    mask[3] = 9thByte[3] xor 9thByte[7] xor 9thByte[0];
 258    mask[4] = 9thByte[1];
 259    mask[5] = 9thByte[2];
 260    mask[6] = 9thByte[3];
 261    mask[7] = 9thByte[4] xor 9thByte[0];
 262
 263    To calculate the heading (not tilt compensated) you need to do atan2 on the resulting
 264    y any a (y and x?) values, convert radians to degrees and add 360 if the result is negative.
 265  */
 266
 267 struct DjiMag { // byte offset from beginning of packet, subtract 5 for struct offset
 268     int16_t x; // BYTE 5-6 (CX): compass X axis data (signed) - see comments below
 269     int16_t y; // BYTE 7-8 (CY): compass Y axis data (signed) - see comments below
 270     int16_t z; // BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below
 271 } __attribute__((packed));
 272
 273
 274 /*
 275    version info from
 276    http://www.rcgroups.com/forums/showpost.php?p=27058649&postcount=120
 277
 278    This is still to be confirmed but I believe the 0x30 message carries the GPS module hardware id and firmware version.
 279
 280    55 AA 30 0C XX XX XX XX FW FW FW FW HW HW HW HW CS CS
 281
 282    Note that you need to read version numbers backwards (02 01 00 06 means v6.0.1.2)
 283
 284    HEADER
 285    -------------
 286    BYTE 1-2: message header - always 55 AA
 287    BYTE 3: message id (0x30 for GPS module versions message)
 288    BYTE 4: length of the payload (0x0C or 12 decimal for 0x30 message)
 289
 290    PAYLOAD
 291    --------------
 292    BYTE 5-8" ??? (seems to be always 0)
 293    BYTE 9-12 (FW): firmware version
 294    BYTE 13-16 (HW): hardware id
 295
 296    CHECKSUM
 297    -----------------
 298    BYTE 17-18 (CS): checksum, calculated the same way as for uBlox binary messages
 299  */
 300
 301 struct DjiVer { // byte offset from beginning of packet, subtract 5 for struct offset
 302     uint32_t unused1; // BYTE 5-8" ??? (seems to be always 0)
 303     uint32_t swVersion; // BYTE 9-12 (FW): firmware version
 304     uint32_t hwVersion; // BYTE 13-16 (HW): hardware id
 305 } __attribute__((packed));
 306 #define VER_FIRST_DECODED_BYTE offsetof(struct DjiVer, swVersion)
 307
 308
 309 typedef union {
 310     uint8_t payload[0];
 311     // Nav Class
 312     struct DjiGps gps;
 313     struct DjiMag mag;
 314     struct DjiVer ver;
 315 } DJIPayload;
 316
 317 struct DJIHeader {
 318     uint8_t id;
 319     uint8_t len;
 320     uint8_t checksumA; // these are not part of the dji header, they are actually in the trailer
 321     uint8_t checksumB; // but they are kept here for parsing ease
 322 } __attribute__((packed));
 323
 324 struct DJIPacket {
 325     struct DJIHeader header;
 326     DJIPayload payload;
 327 } __attribute__((packed));
 328
 329 int parse_dji_stream(uint8_t *inputBuffer, uint16_t inputBufferLength, char *parsedDjiStruct, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *GpsRxStats);
 330 void dji_load_mag_settings();
 331
 332 #endif /* DJI_H */