src/main/io/gps_naza.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <ctype.h>
  21 #include <string.h>
  22 #include <math.h>
  23
  24 #include "platform.h"
  25 #include "build/build_config.h"
  26
  27
  28 #if defined(USE_GPS) && defined(USE_GPS_PROTO_NAZA)
  29
  30 #include "build/debug.h"
  31
  32 #include "common/axis.h"
  33 #include "common/gps_conversion.h"
  34 #include "common/maths.h"
  35 #include "common/utils.h"
  36
  37 #include "drivers/serial.h"
  38 #include "drivers/time.h"
  39
  40 #include "fc/config.h"
  41 #include "fc/runtime_config.h"
  42
  43 #include "io/gps.h"
  44 #include "io/gps_private.h"
  45 #include "io/serial.h"
  46
  47 #include "scheduler/protothreads.h"
  48
  49 #define NAZA_MAX_PAYLOAD_SIZE   256
  50
  51 typedef struct {
  52     uint8_t res[4]; // 0
  53     uint8_t fw[4]; // 4
  54     uint8_t hw[4]; // 8
  55 } naza_ver;
  56
  57 typedef struct {
  58     uint16_t x; // 0
  59     uint16_t y; // 2
  60     uint16_t z; // 4
  61 } naza_mag;
  62
  63 typedef struct {
  64     uint32_t time; // GPS msToW 0
  65     int32_t longitude; // 4
  66     int32_t latitude; // 8
  67     int32_t altitude_msl; // 12
  68     int32_t h_acc; // 16
  69     int32_t v_acc; // 20
  70     int32_t reserved;
  71     int32_t ned_north; // 28
  72     int32_t ned_east; // 32
  73     int32_t ned_down;  // 36
  74     uint16_t pdop;  // 40
  75     uint16_t vdop;  // 42
  76     uint16_t ndop; // 44
  77     uint16_t edop;  // 46
  78     uint8_t satellites; // 48
  79     uint8_t reserved3; //
  80     uint8_t fix_type; // 50
  81     uint8_t reserved4; //
  82     uint8_t fix_status; // 52
  83     uint8_t reserved5;
  84     uint8_t reserved6;
  85     uint8_t mask;   // 55
  86 } naza_nav;
  87
  88 enum {
  89     HEADER1 = 0x55,
  90     HEADER2 = 0xAA,
  91     ID_NAV = 0x10,
  92     ID_MAG = 0x20,
  93     ID_VER = 0x30,
  94     LEN_NAV = 0x3A,
  95     LEN_MAG = 0x06,
  96 } naza_protocol_bytes;
  97
  98 typedef enum {
  99     NO_FIX = 0,
 100     FIX_2D = 2,
 101     FIX_3D = 3,
 102     FIX_DGPS = 4
 103 } fixType_t;
 104
 105 // Receive buffer
 106 static union {
 107     naza_mag mag;
 108     naza_nav nav;
 109     naza_ver ver;
 110     uint8_t bytes[NAZA_MAX_PAYLOAD_SIZE];
 111 } _buffernaza;
 112
 113 // Packet checksum accumulators
 114 static uint8_t _ck_a;
 115 static uint8_t _ck_b;
 116
 117 // State machine state
 118 static bool _skip_packet;
 119 static uint8_t _step;
 120 static uint8_t _msg_id;
 121 static uint16_t _payload_length;
 122 static uint16_t _payload_counter;
 123
 124 // do we have new position information?
 125 static bool _new_position;
 126
 127 // do we have new speed information?
 128 static bool _new_speed;
 129
 130 int32_t decodeLong(uint32_t idx, uint8_t mask)
 131 {
 132     union { uint32_t l; uint8_t b[4]; } val;
 133     val.l=idx;
 134     for (int i = 0; i < 4; i++) val.b[i] ^= mask;
 135     return val.l;
 136 }
 137
 138 int16_t decodeShort(uint16_t idx, uint8_t mask)
 139 {
 140     union { uint16_t s; uint8_t b[2]; } val;
 141     val.s=idx;
 142     for (int i = 0; i < 2; i++) val.b[i] ^= mask;
 143     return val.s;
 144 }
 145
 146 static bool NAZA_parse_gps(void)
 147 {
 148     uint8_t mask;
 149     uint8_t mask_mag;
 150
 151     switch (_msg_id) {
 152     case ID_NAV:
 153         mask = _buffernaza.nav.mask;
 154
 155         uint32_t time = decodeLong(_buffernaza.nav.time, mask);
 156         gpsSol.time.seconds = time & 0b00111111; time >>= 6;
 157         gpsSol.time.minutes = time & 0b00111111; time >>= 6;
 158         gpsSol.time.hours = time & 0b00001111; time >>= 4;
 159         gpsSol.time.day = gpsSol.time.hours > 7?(time & 0b00011111) + 1:(time & 0b00011111); time >>= 5;
 160         gpsSol.time.month = time & 0b00001111; time >>= 4;
 161         gpsSol.time.year = (time & 0b01111111) + 2000;
 162
 163         gpsSol.llh.lon = decodeLong(_buffernaza.nav.longitude, mask);
 164         gpsSol.llh.lat = decodeLong(_buffernaza.nav.latitude, mask);
 165         gpsSol.llh.alt = decodeLong(_buffernaza.nav.altitude_msl, mask) / 10.0f;  //alt in cm
 166
 167         uint8_t fixType = _buffernaza.nav.fix_type ^ mask;
 168         //uint8_t fixFlags = _buffernaza.nav.fix_status ^ mask;
 169
 170         //uint8_t r3 = _buffernaza.nav.reserved3 ^ mask;
 171         //uint8_t r4 = _buffernaza.nav.reserved4 ^ mask;
 172         //uint8_t r5 = _buffernaza.nav.reserved5 ^ mask;
 173         //uint8_t r6 = _buffernaza.nav.reserved6 ^ mask;
 174
 175         if (fixType == FIX_2D)
 176             gpsSol.fixType = GPS_FIX_2D;
 177         else if (fixType == FIX_3D)
 178             gpsSol.fixType = GPS_FIX_3D;
 179         else
 180             gpsSol.fixType = GPS_NO_FIX;
 181
 182         uint32_t h_acc = decodeLong(_buffernaza.nav.h_acc, mask); // mm
 183         uint32_t v_acc = decodeLong(_buffernaza.nav.v_acc, mask); // mm
 184         //uint32_t test = decodeLong(_buffernaza.nav.reserved, mask);
 185
 186         gpsSol.velNED[0] = decodeLong(_buffernaza.nav.ned_north, mask);  // cm/s
 187         gpsSol.velNED[1] = decodeLong(_buffernaza.nav.ned_east, mask);   // cm/s
 188         gpsSol.velNED[2] = decodeLong(_buffernaza.nav.ned_down, mask);   // cm/s
 189
 190
 191         uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop
 192         //uint16_t vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop
 193         //uint16_t ndop = decodeShort(_buffernaza.nav.ndop, mask);
 194         //uint16_t edop = decodeShort(_buffernaza.nav.edop, mask);
 195         //gpsSol.hdop = sqrtf(powf(ndop,2)+powf(edop,2));
 196         //gpsSol.vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop
 197
 198         gpsSol.hdop = gpsConstrainEPE(pdop);        // PDOP
 199         gpsSol.eph = gpsConstrainEPE(h_acc / 10);   // hAcc in cm
 200         gpsSol.epv = gpsConstrainEPE(v_acc / 10);   // vAcc in cm
 201         gpsSol.numSat = _buffernaza.nav.satellites;
 202         gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
 203
 204         // calculate gps heading from VELNE
 205         gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f));
 206
 207         gpsSol.flags.validVelNE = 1;
 208         gpsSol.flags.validVelD = 1;
 209         gpsSol.flags.validEPE = 1;
 210         gpsSol.flags.validTime = 1;
 211
 212         _new_position = true;
 213         _new_speed = true;
 214         break;
 215     case ID_MAG:
 216         mask_mag = (_buffernaza.mag.z)&0xFF;
 217         mask_mag = (((mask_mag ^ (mask_mag >> 4)) & 0x0F) | ((mask_mag << 3) & 0xF0)) ^ (((mask_mag & 0x01) << 3) | ((mask_mag & 0x01) << 7));
 218
 219         gpsSol.magData[0] = decodeShort(_buffernaza.mag.x, mask_mag);
 220         gpsSol.magData[1] = decodeShort(_buffernaza.mag.y, mask_mag);
 221         gpsSol.magData[2] = (_buffernaza.mag.z ^ (mask_mag<<8));
 222
 223         gpsSol.flags.validMag = 1;
 224         break;
 225     case ID_VER:
 226         break;
 227     default:
 228         return false;
 229     }
 230
 231     // we only return true when we get new position and speed data
 232     // this ensures we don't use stale data
 233     if (_new_position && _new_speed) {
 234         _new_speed = _new_position = false;
 235         return true;
 236     }
 237     return false;
 238 }
 239
 240 static bool gpsNewFrameNAZA(uint8_t data)
 241 {
 242     bool parsed = false;
 243
 244     switch (_step) {
 245         case 0: // Sync char 1 (0x55)
 246             if (HEADER1 == data) {
 247                 _skip_packet = false;
 248                 _step++;
 249             }
 250             break;
 251         case 1: // Sync char 2 (0xAA)
 252             if (HEADER2 != data) {
 253                 _step = 0;
 254                 break;
 255             }
 256             _step++;
 257             break;
 258         case 2: // Id
 259             _step++;
 260             _ck_b = _ck_a = data;   // reset the checksum accumulators
 261             _msg_id = data;
 262             break;
 263         case 3: // Payload length
 264             _step++;
 265             _ck_b += (_ck_a += data);       // checksum byte
 266             _payload_length = data; // payload length low byte
 267             if (_payload_length > NAZA_MAX_PAYLOAD_SIZE) {
 268                 // we can't receive the whole packet, just log the error and start searching for the next packet.
 269                 gpsStats.errors++;
 270                 _step = 0;
 271                 break;
 272             }
 273             // prepare to receive payload
 274             _payload_counter = 0;
 275             if (_payload_length == 0) {
 276                 _step = 6;
 277             }
 278             break;
 279         case 4:
 280             _ck_b += (_ck_a += data);       // checksum byte
 281             if (_payload_counter < NAZA_MAX_PAYLOAD_SIZE) {
 282                 _buffernaza.bytes[_payload_counter] = data;
 283             }
 284             if (++_payload_counter >= _payload_length) {
 285                 _step++;
 286             }
 287             break;
 288         case 5:
 289             _step++;
 290             if (_ck_a != data) {
 291                 _skip_packet = true;          // bad checksum
 292                 gpsStats.errors++;
 293             }
 294             break;
 295         case 6:
 296             _step = 0;
 297             if (_ck_b != data) {
 298                 gpsStats.errors++;
 299                 break;              // bad checksum
 300             }
 301
 302             gpsStats.packetCount++;
 303
 304             if (_skip_packet) {
 305                 break;
 306             }
 307
 308             if (NAZA_parse_gps()) {
 309                 parsed = true;
 310             }
 311     }
 312     return parsed;
 313 }
 314
 315 static ptSemaphore_t semNewDataReady;
 316
 317 STATIC_PROTOTHREAD(gpsProtocolReceiverThread)
 318 {
 319     ptBegin(gpsProtocolReceiverThread);
 320
 321     while (1) {
 322         // Wait until there are bytes to consume
 323         ptWait(serialRxBytesWaiting(gpsState.gpsPort));
 324
 325         // Consume bytes until buffer empty of until we have full message received
 326         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 327             uint8_t newChar = serialRead(gpsState.gpsPort);
 328             if (gpsNewFrameNAZA(newChar)) {
 329                 ptSemaphoreSignal(semNewDataReady);
 330                 break;
 331             }
 332         }
 333     }
 334
 335     ptEnd(0);
 336 }
 337
 338 STATIC_PROTOTHREAD(gpsProtocolStateThread)
 339 {
 340     ptBegin(gpsProtocolStateThread);
 341
 342     // Change baud rate
 343     ptWait(isSerialTransmitBufferEmpty(gpsState.gpsPort));
 344     if (gpsState.gpsConfig->autoBaud != GPS_AUTOBAUD_OFF) {
 345         // Cycle through available baud rates and hope that we will match GPS
 346         serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
 347         gpsState.autoBaudrateIndex = (gpsState.autoBaudrateIndex + 1) % GPS_BAUDRATE_COUNT;
 348         ptDelayMs(GPS_BAUD_CHANGE_DELAY);
 349     }
 350     else {
 351         // Set baud rate
 352         serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.baudrateIndex]]);
 353     }
 354
 355     // No configuration is done for NAZA GPS
 356
 357     // GPS setup done, reset timeout
 358     gpsSetProtocolTimeout(GPS_TIMEOUT);
 359
 360     // GPS is ready - execute the gpsProcessNewSolutionData() based on gpsProtocolReceiverThread semaphore
 361     while (1) {
 362         ptSemaphoreWait(semNewDataReady);
 363         gpsProcessNewSolutionData();
 364     }
 365
 366     ptEnd(0);
 367 }
 368
 369 void gpsRestartNAZA(void)
 370 {
 371     ptSemaphoreInit(semNewDataReady);
 372     ptRestart(ptGetHandle(gpsProtocolReceiverThread));
 373     ptRestart(ptGetHandle(gpsProtocolStateThread));
 374 }
 375
 376 void gpsHandleNAZA(void)
 377 {
 378     // Run the protocol threads
 379     gpsProtocolReceiverThread();
 380     gpsProtocolStateThread();
 381
 382     // If thread stopped - signal communication loss and restart
 383     if (ptIsStopped(ptGetHandle(gpsProtocolReceiverThread)) || ptIsStopped(ptGetHandle(gpsProtocolStateThread))) {
 384         gpsSetState(GPS_LOST_COMMUNICATION);
 385     }
 386 }
 387
 388 #endif