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_config.h"
  26 #include "debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30 #include "common/utils.h"
  31
  32 #include "drivers/system.h"
  33 #include "drivers/serial.h"
  34 #include "drivers/serial_uart.h"
  35
  36 #include "io/serial.h"
  37 #include "io/gps.h"
  38 #include "io/gps_private.h"
  39
  40 #include "flight/gps_conversion.h"
  41
  42 #include "config/config.h"
  43 #include "config/runtime_config.h"
  44
  45 #if defined(GPS) && defined(GPS_PROTO_NAZA)
  46
  47 #define NAZA_MAX_PAYLOAD_SIZE   256
  48
  49 typedef struct {
  50     uint8_t res[4]; // 0
  51     uint8_t fw[4]; // 4
  52     uint8_t hw[4]; // 8
  53 } naza_ver;
  54
  55 typedef struct {
  56     uint16_t x; // 0
  57     uint16_t y; // 2
  58     uint16_t z; // 4
  59 } naza_mag;
  60
  61 typedef struct {
  62     uint32_t time; // GPS msToW 0
  63     int32_t longitude; // 4
  64     int32_t latitude; // 8
  65     int32_t altitude_msl; // 12
  66     int32_t h_acc; // 16
  67     int32_t v_acc; // 20
  68     int32_t reserved;
  69     int32_t ned_north; // 28
  70     int32_t ned_east; // 32
  71     int32_t ned_down;  // 36
  72     uint16_t pdop;  // 40
  73     uint16_t vdop;  // 42
  74     uint16_t ndop; // 44
  75     uint16_t edop;      // 46
  76     uint8_t satellites; // 48
  77     uint8_t reserved3; //
  78     uint8_t fix_type; // 50
  79     uint8_t reserved4; //
  80     uint8_t fix_status; // 52
  81     uint8_t reserved5;
  82     uint8_t reserved6;
  83     uint8_t mask;       // 55
  84 } naza_nav;
  85
  86 enum {
  87     HEADER1 = 0x55,
  88     HEADER2 = 0xAA,
  89     ID_NAV = 0x10,
  90     ID_MAG = 0x20,
  91     ID_VER = 0x30,
  92     LEN_NAV = 0x3A,
  93     LEN_MAG = 0x06,
  94 } naza_protocol_bytes;
  95
  96 typedef enum {
  97     NO_FIX = 0,
  98     FIX_2D = 2,
  99     FIX_3D = 3,
 100     FIX_DGPS = 4
 101 } fixType_t;
 102
 103 // Receive buffer
 104 static union {
 105     naza_mag mag;
 106     naza_nav nav;
 107     naza_ver ver;
 108     uint8_t bytes[NAZA_MAX_PAYLOAD_SIZE];
 109 } _buffernaza;
 110
 111 // Packet checksum accumulators
 112 static uint8_t _ck_a;
 113 static uint8_t _ck_b;
 114
 115 // State machine state
 116 static bool _skip_packet;
 117 static uint8_t _step;
 118 static uint8_t _msg_id;
 119 static uint16_t _payload_length;
 120 static uint16_t _payload_counter;
 121
 122 // do we have new position information?
 123 static bool _new_position;
 124
 125 // do we have new speed information?
 126 static bool _new_speed;
 127
 128 int32_t decodeLong(uint32_t idx, uint8_t mask)
 129 {
 130     union { uint32_t l; uint8_t b[4]; } val;
 131     val.l=idx;
 132     for(int i = 0; i < 4; i++) val.b[i] ^= mask;
 133     return val.l;
 134 }
 135
 136 int16_t decodeShort(uint16_t idx, uint8_t mask)
 137 {
 138     union { uint16_t s; uint8_t b[2]; } val;
 139     val.s=idx;
 140     for(int i = 0; i < 2; i++) val.b[i] ^= mask;
 141     return val.s;
 142 }
 143
 144 static bool NAZA_parse_gps(void)
 145 {
 146     uint8_t mask;
 147     uint8_t mask_mag;
 148
 149     switch (_msg_id) {
 150     case ID_NAV:
 151         mask = _buffernaza.nav.mask;
 152
 153         //uint32_t time = decodeLong(_buffernaza.nav.time, mask);
 154         //uint32_t second = time & 0b00111111; time >>= 6;
 155         //uint32_t minute = time & 0b00111111; time >>= 6;
 156         //uint32_t hour = time & 0b00001111; time >>= 4;
 157         //uint32_t day = time & 0b00011111; time >>= 5;
 158         //uint32_t month = time & 0b00001111; time >>= 4;
 159         //uint32_t year = time & 0b01111111;
 160
 161         gpsSol.llh.lon = decodeLong(_buffernaza.nav.longitude, mask);
 162         gpsSol.llh.lat = decodeLong(_buffernaza.nav.latitude, mask);
 163         gpsSol.llh.alt = decodeLong(_buffernaza.nav.altitude_msl, mask) / 10.0f;  //alt in cm
 164
 165         uint8_t fixType = _buffernaza.nav.fix_type ^ mask;
 166         //uint8_t fixFlags = _buffernaza.nav.fix_status ^ mask;
 167
 168         //uint8_t r3 = _buffernaza.nav.reserved3 ^ mask;
 169         //uint8_t r4 = _buffernaza.nav.reserved4 ^ mask;
 170         //uint8_t r5 = _buffernaza.nav.reserved5 ^ mask;
 171         //uint8_t r6 = _buffernaza.nav.reserved6 ^ mask;
 172
 173         gpsSol.flags.fix3D = (fixType == FIX_3D) ? 1 : 0;
 174
 175         uint32_t h_acc = decodeLong(_buffernaza.nav.h_acc, mask); // mm
 176         uint32_t v_acc = decodeLong(_buffernaza.nav.v_acc, mask); // mm
 177         //uint32_t test = decodeLong(_buffernaza.nav.reserved, mask);
 178
 179         gpsSol.velNED[0] = decodeLong(_buffernaza.nav.ned_north, mask);  // cm/s
 180         gpsSol.velNED[1] = decodeLong(_buffernaza.nav.ned_east, mask);   // cm/s
 181         gpsSol.velNED[2] = decodeLong(_buffernaza.nav.ned_down, mask);   // cm/s
 182
 183
 184         //uint16_t pdop = decodeShort(_buffernaza.nav.pdop, mask); // pdop
 185         //uint16_t vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop
 186         //uint16_t ndop = decodeShort(_buffernaza.nav.ndop, mask);
 187         //uint16_t edop = decodeShort(_buffernaza.nav.edop, mask);
 188         //gpsSol.hdop = sqrtf(powf(ndop,2)+powf(edop,2));
 189         //gpsSol.vdop = decodeShort(_buffernaza.nav.vdop, mask); // vdop
 190
 191         gpsSol.eph = gpsConstrainEPE(h_acc / 10);   // hAcc in cm
 192         gpsSol.epv = gpsConstrainEPE(v_acc / 10);   // vAcc in cm
 193         gpsSol.numSat = _buffernaza.nav.satellites;
 194         gpsSol.groundSpeed = sqrtf(powf(gpsSol.velNED[0], 2)+powf(gpsSol.velNED[1], 2)); //cm/s
 195
 196         // calculate gps heading from VELNE
 197         gpsSol.groundCourse = (uint16_t) (fmodf(RADIANS_TO_DECIDEGREES(atan2_approx(gpsSol.velNED[1], gpsSol.velNED[0]))+3600.0f,3600.0f));
 198
 199         gpsSol.flags.validVelNE = 1;
 200         gpsSol.flags.validVelD = 1;
 201         gpsSol.flags.validEPE = 1;
 202
 203         _new_position = true;
 204         _new_speed = true;
 205         break;
 206     case ID_MAG:
 207         mask_mag = (_buffernaza.mag.z)&0xFF;
 208         mask_mag = (((mask_mag ^ (mask_mag >> 4)) & 0x0F) | ((mask_mag << 3) & 0xF0)) ^ (((mask_mag & 0x01) << 3) | ((mask_mag & 0x01) << 7));
 209
 210         gpsSol.magData[0] = decodeShort(_buffernaza.mag.x, mask_mag);
 211         gpsSol.magData[1] = decodeShort(_buffernaza.mag.y, mask_mag);
 212         gpsSol.magData[2] = (_buffernaza.mag.z ^ (mask_mag<<8));
 213
 214         break;
 215     case ID_VER:
 216         break;
 217     default:
 218         return false;
 219     }
 220
 221     // we only return true when we get new position and speed data
 222     // this ensures we don't use stale data
 223     if (_new_position && _new_speed) {
 224         _new_speed = _new_position = false;
 225         return true;
 226     }
 227     return false;
 228 }
 229
 230 static bool gpsNewFrameNAZA(uint8_t data)
 231 {
 232     bool parsed = false;
 233
 234     switch (_step) {
 235         case 0: // Sync char 1 (0x55)
 236             if (HEADER1 == data) {
 237                 _skip_packet = false;
 238                 _step++;
 239             }
 240             break;
 241         case 1: // Sync char 2 (0xAA)
 242             if (HEADER2 != data) {
 243                 _step = 0;
 244                 break;
 245             }
 246             _step++;
 247             break;
 248         case 2: // Id
 249             _step++;
 250             _ck_b = _ck_a = data;   // reset the checksum accumulators
 251             _msg_id = data;
 252             break;
 253         case 3: // Payload length
 254             _step++;
 255             _ck_b += (_ck_a += data);       // checksum byte
 256             _payload_length = data; // payload length low byte
 257             if (_payload_length > NAZA_MAX_PAYLOAD_SIZE) {
 258                 // we can't receive the whole packet, just log the error and start searching for the next packet.
 259                 gpsStats.errors++;
 260                 _step = 0;
 261                 break;
 262             }
 263             // prepare to receive payload
 264             _payload_counter = 0;
 265             if (_payload_length == 0) {
 266                 _step = 6;
 267             }
 268             break;
 269         case 4:
 270             _ck_b += (_ck_a += data);       // checksum byte
 271             if (_payload_counter < NAZA_MAX_PAYLOAD_SIZE) {
 272                 _buffernaza.bytes[_payload_counter] = data;
 273             }
 274             if (++_payload_counter >= _payload_length) {
 275                 _step++;
 276             }
 277             break;
 278         case 5:
 279             _step++;
 280             if (_ck_a != data) {
 281                 _skip_packet = true;          // bad checksum
 282                 gpsStats.errors++;
 283             }
 284             break;
 285         case 6:
 286             _step = 0;
 287             if (_ck_b != data) {
 288                 gpsStats.errors++;
 289                 break;              // bad checksum
 290             }
 291
 292             gpsStats.packetCount++;
 293
 294             if (_skip_packet) {
 295                 break;
 296             }
 297
 298             if (NAZA_parse_gps()) {
 299                 parsed = true;
 300             }
 301     }
 302     return parsed;
 303 }
 304
 305 static bool gpsReceiveData(void)
 306 {
 307     bool hasNewData = false;
 308
 309     if (gpsState.gpsPort) {
 310         while (serialRxBytesWaiting(gpsState.gpsPort)) {
 311             uint8_t newChar = serialRead(gpsState.gpsPort);
 312             if (gpsNewFrameNAZA(newChar)) {
 313                 gpsSol.flags.gpsHeartbeat = !gpsSol.flags.gpsHeartbeat;
 314                 hasNewData = true;
 315             }
 316         }
 317     }
 318
 319     return hasNewData;
 320 }
 321
 322 static bool gpsInitialize(void)
 323 {
 324     gpsSetState(GPS_CHANGE_BAUD);
 325     return false;
 326 }
 327
 328 static bool gpsChangeBaud(void)
 329 {
 330     gpsFinalizeChangeBaud();
 331     return false;
 332 }
 333
 334 bool gpsHandleNAZA(void)
 335 {
 336     // Receive data
 337     bool hasNewData = gpsReceiveData();
 338
 339     // Process state
 340     switch(gpsState.state) {
 341     default:
 342         return false;
 343
 344     case GPS_INITIALIZING:
 345         return gpsInitialize();
 346
 347     case GPS_CHANGE_BAUD:
 348         return gpsChangeBaud();
 349
 350     case GPS_CHECK_VERSION:
 351     case GPS_CONFIGURE:
 352         // No autoconfig, switch straight to receiving data
 353         gpsSetState(GPS_RECEIVING_DATA);
 354         return false;
 355
 356     case GPS_RECEIVING_DATA:
 357         return hasNewData;
 358     }
 359 }
 360
 361 #endif