MultiWii

Code: Select all

  #define MTK_SBAS_INTEGRITYMODE"$PMTK319,1*24\r\n"    // TEST:ACK 
  #define MTK_NAVTHRES_OFF      "$PMTK397,0*23\r\n"    // TEST:ACK Threshold was 1.00m before (PMTK447 returns 0.0 NOW)
  #define MTK_SBAS_ON      "$PMTK313,1*2E\r\n"    // Enable search SBAS sat // TEST:ACK
  #define MTK_WAAS_ON           "$PMTK301,2*2E\r\n"    // DGPS MODE WAAS // TEST:ACK

Code: Select all

  #define MTK__DT_NAVTHRES_OFF  "$PMTK527,0.00*00\r\n" // Turn off Navthreshold // TEST: ERROR UNSUPPORTED COMMAND
  #define MTK__DT_NAVTHRES_OFF  "$PMTK527,0*2E\r\n" // Turn off Navthreshold // TEST: ERROR UNSUPPORTED COMMAND
  #define MTK_DT_SBAS_ENABLED   "$PMTK513,1*28\r\n"  // Enable Search for SBAS Sats // TEST: ERROR UNSUPPORTED COMMAND
  #define MTK_DT_DGPS_WAAS      "$PMTK501,2*28\r\n"  // DGPS data source mode: WAAS // TEST: ERROR UNSUPPORTED COMMAND

Code: Select all

#define MINSATPH 7                     // 5 is default minimal number of sats for gps PosHold, i found valid data with 7 sats. Since PH is luxury we want more sats..
#define MINSATRTH 5                    // 5 is default minimal number of sats for gps Return to Home since this is more an emergency function a 20m error doesn't matter

Code: Select all

// *********************************** GENERAL GPS TYPE
//#define NMEA
//#define UBLOX
#define MTK                            // Defined: Binary, 57K Baud, 5Hz, SBAS, WAAS, Navthreshold: OFF

// *********************************** GENERAL FILTER VALUES
#define SPIKEFILTER                    // Turn on experimental spikefilter
#define AFALWAYSOFF                    // Always turn off the moving average filter("AF"), doesn't matter whats in mwii config.h defined

// *********************************** GENERAL GPS VALUES
#define MINSAT 7                       // 5 is default minimal number of sats for gps functions, i found valid data with 7 sats

// *********************************** MTK BINARY SPECIAL (FW:AXN1.51_2722_3329_384.1151100.5.bin)
//#define MTK10HZ                      // According to this post: http://www.multiwii.com/forum/viewtopic.php?f=6&t=1881&start=10#p19325
                                       // when enabeling 10HZ in mtk binary mode, you will loose SBAS functionality!! That will just work at 5Hz (default).

Code: Select all

bool GPS_MTK_newFrame(uint8_t data)
{
   static uint8_t pstep;                              // Parse Step
   static uint8_t lastbyte;                           // Last Byte for Sync detection
   static uint8_t LSLshifter;                         // Bitshiftvalue
   static uint8_t chkA,count;

   static int32_t lat;                                // MTK Dataset
   static int32_t lon;                                // MTK Dataset
   static int32_t alt;                                // MTK Dataset
   static int32_t grspeed;                            // MTK Dataset
   static int32_t grcourse;                           // MTK Dataset
   static uint8_t satn,fixtype;                       // MTK Dataset
   int32_t tmp32;
   bool parsed;
   
   parsed = false;

   if (pstep == 0 && data == 0xdd && lastbyte == 0xd0) pstep = 100; // Detect Sync "0xD0,0xDD" Only search for Sync when not already decoding
   lastbyte = data;

   switch(pstep) {
    case 0:                                           // Special Case: Do Nothing
    break;
    case 100:                                         // Special Case: Prepare next decoding run
    pstep = 1;                                        // Jump into decoding on next run
    chkA = 0; count = 0;                              // Reset values
    break;

    case 1:                                           // Payload Byte is always $20! (This is the first Byte after sync preamble)
    if (data == 0x20) pstep++;                        // Since it is always $20 we take it as extended, undocumented syncword "preamble3"
     else pstep =0;                                   // Error! Wait for sync
    chkA = chkA + data;
    count ++;
    break;

    case 2:                                           // Read Dataset Latitude
    lat = data;
    LSLshifter = 0;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    case 3:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lat = lat | tmp32;
    if (LSLshifter == 24){lat = lat * 10; pstep++;}
    chkA = chkA + data;
    count ++;
    break;    

    case 4:                                           // Read Dataset Longitude
    lon = data;
    LSLshifter = 0;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    case 5:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lon = lon | tmp32;
    if (LSLshifter == 24){lon = lon * 10; pstep++;}
    chkA = chkA + data;
    count ++;
    break;

    case 6:                                           // Read Dataset MSL Altitude
    alt = data;
    LSLshifter = 0;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    case 7:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    alt = alt | tmp32;
    if (LSLshifter == 24){alt = alt/100; pstep++;}
    chkA = chkA + data;
    count ++;
    break;

    case 8:                                           // Read Dataset Ground Speed
    grspeed = data;
    LSLshifter = 0;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    case 9:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grspeed = grspeed | tmp32;
    if (LSLshifter == 24) pstep++;
    chkA = chkA + data;
    count ++;
    break;

    case 10:                                          // Read Dataset Heading
    grcourse = data;
    LSLshifter = 0;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    case 11:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grcourse = grcourse | tmp32;
    if (LSLshifter == 24) pstep++;
    chkA = chkA + data;
    count ++;
    break;

    case 12:                                          // Read number of satellites in view
    satn = data;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;
    
    case 13:                                          // Read Fix Type
    fixtype = data;
    pstep++;
    chkA = chkA + data;
    count ++;
    break;

    case 14:                                          // Wait for cheksum A
    if (count == 33){                                 // 33 = 0x21
      if (chkA == data) pstep++;                      // ChecksumA reached. Correct? than go on
       else pstep = 0;                                // Error?
    }else{
      chkA = chkA + data;
      count ++;}
    break;

    case 15:                                          // Dataset RDY !! Cheksum B omitted, ChkA was OK
    if (fixtype==1){                                  // No Fix
    i2c_dataset.status.gps2dfix = 0;
    i2c_dataset.status.gps3dfix = 0;}
    if (fixtype==2){                                  // GPS 2D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 0;}
    if (fixtype==3){                                  // GPS 3D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 1;}
    GPS_read[LAT] = lat;
    GPS_read[LON] = lon;
    i2c_dataset.altitude = alt;
    i2c_dataset.ground_speed = grspeed;
    i2c_dataset.ground_course = grcourse;
    i2c_dataset.status.numsats = satn;
    parsed = true;                                    // RDY
    pstep = 0;                                        // Do nothing
    break;
   }
 return parsed;
}

Code: Select all

bool GPS_MTK_newFrame(uint8_t data)
{
   static uint8_t pstep;                              // Parse Step
   static uint8_t lastbyte;                           // Last Byte for Sync detection
   static uint8_t LSLshifter;                         // Bitshiftvalue

   static int32_t lat;                                // MTK Dataset
   static int32_t lon;                                // MTK Dataset
   static int32_t alt;                                // MTK Dataset
   static int32_t grspeed;                            // MTK Dataset
   static int32_t grcourse;                           // MTK Dataset
   static uint8_t satn;                               // MTK Dataset
   int32_t tmp32;
   bool parsed;
   
   parsed = false;

   if(pstep == 0 || pstep>5){                         // Only search for Sync when idle or lat and lon already decoded.
    if (data == 0xdd && lastbyte == 0xd0) pstep = 100;// Detect Sync "0xD0,0xDD"
   }
   lastbyte = data;

   switch(pstep) {
    case 0:                                           // Special Case: Do Nothing
    break;
    case 100:                                         // Special Case: Jump into decoding on next run
    pstep = 1;
    break;

    case 1:                                           // Payload Byte is always $20! (This is the first Byte after sync preamble)
    if (data == 0x20) pstep++;                        // Since it is always $20 we take it as extended, undocumented syncword "preamble3"
     else pstep =0;
    break;

    case 2:                                           // Read Dataset Latitude
    lat = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 3:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lat = lat | tmp32;
    if (LSLshifter == 24){lat = lat * 10; pstep++;}
    break;    

    case 4:                                           // Read Dataset Longitude
    lon = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 5:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lon = lon | tmp32;
    if (LSLshifter == 24){lon = lon * 10; pstep++;}
    break;

    case 6:                                           // Read Dataset MSL Altitude
    alt = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 7:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    alt = alt | tmp32;
    if (LSLshifter == 24){alt = alt/100; pstep++;}
    break;

    case 8:                                           // Read Dataset Ground Speed
    grspeed = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 9:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grspeed = grspeed | tmp32;
    if (LSLshifter == 24) pstep++;
    break;

    case 10:                                          // Read Dataset Heading
    grcourse = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 11:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grcourse = grcourse | tmp32;
    if (LSLshifter == 24) pstep++;
    break;

    case 12:                                          // Read number of satellites in view
    satn = data;
    pstep++;
    break;
    
    case 13:                                          // Read Fix Type and finish readout, mwii does not need the rest
    if (data==1){                                     // No Fix
    i2c_dataset.status.gps2dfix = 0;
    i2c_dataset.status.gps3dfix = 0;}
    if (data==2){                                     // GPS 2D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 0;}
    if (data==3){                                     // GPS 3D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 1;}
    GPS_read[LAT] = lat;
    GPS_read[LON] = lon;
    i2c_dataset.altitude = alt;
    i2c_dataset.ground_speed = grspeed;
    i2c_dataset.ground_course = grcourse;
    i2c_dataset.status.numsats = satn;
    parsed = true;                                    // RDY
    pstep = 0;                                        // Do nothing with the next bytes
    break;
   }
 return parsed;
}

Code: Select all

bool GPS_MTK_newFrame(uint8_t data)
{
   static uint8_t pstep;                              // Parse Step
   static uint8_t lastbyte;                           // Last Byte for Sync detection
   static uint8_t LSLshifter;                         // Bitshiftvalue

   static int32_t lat;                                // MTK Dataset
   static int32_t lon;                                // MTK Dataset
   static int32_t alt;                                // MTK Dataset
   static int32_t grspeed;                            // MTK Dataset
   static int32_t grcourse;                           // MTK Dataset
   static uint8_t satn;                               // MTK Dataset
   int32_t tmp32;
   bool parsed;
   
   parsed = false;

   if (data == 0xdd && lastbyte == 0xd0) pstep = 100; // Detect Sync "0xD0,0xDD"
   lastbyte = data;

   switch(pstep) {
    case 0:                                           //  Special Case: Do Nothing
    break;
    case 100:                                         //  Special Case: Jump into decoding on next run
    pstep = 1;
    break;

    case 1:                                           //  Ignore Payload Byte (This is the first Byte after sync preamble)
    pstep++;
    break;

    case 2:                                           // Read Dataset Latitude
    lat = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 3:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lat = lat | tmp32;
    if (LSLshifter == 24){lat = lat * 10; pstep++;}
    break;    

    case 4:                                           // Read Dataset Longitude
    lon = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 5:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    lon = lon | tmp32;
    if (LSLshifter == 24){lon = lon * 10; pstep++;}
    break;

    case 6:                                           // Read Dataset MSL Altitude
    alt = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 7:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    alt = alt | tmp32;
    if (LSLshifter == 24){alt = alt/100; pstep++;}
    break;

    case 8:                                           // Read Dataset Ground Speed
    grspeed = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 9:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grspeed = grspeed | tmp32;
    if (LSLshifter == 24) pstep++;
    break;

    case 10:                                          // Read Dataset Heading
    grcourse = data;
    LSLshifter = 0;
    pstep++;
    break;
    case 11:
    LSLshifter = LSLshifter+8;
    tmp32 = data;
    tmp32 = tmp32<<LSLshifter;
    grcourse = grcourse | tmp32;
    if (LSLshifter == 24) pstep++;
    break;

    case 12:                                          // Read number of satellites in view
    satn = data;
    pstep++;
    break;
    
    case 13:                                          // Read Fix Type and finish readout, mwii does not need the rest
    if (data==1){                                     // No Fix
    i2c_dataset.status.gps2dfix = 0;
    i2c_dataset.status.gps3dfix = 0;}
    if (data==2){                                     // GPS 2D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 0;}
    if (data==3){                                     // GPS 3D Fix
    i2c_dataset.status.gps2dfix = 1;
    i2c_dataset.status.gps3dfix = 1;}
    GPS_read[LAT] = lat;
    GPS_read[LON] = lon;
    i2c_dataset.altitude = alt;
    i2c_dataset.ground_speed = grspeed;
    i2c_dataset.ground_course = grcourse;
    i2c_dataset.status.numsats = satn;
    parsed = true;                                    // RDY
    pstep = 0;                                        // Do nothing with the next bytes
    break;
   }
 return parsed;
}

Code: Select all

....
#if defined(MTK)
/* Using the AXN 1.51 firmware which defaults at 1Hz/38400 but supports binary protocol
* First connect to it with 38400, then set the speed to 115200
* and set the update rate to 10Hz
* and finally switch to Binary mode
*/

Serial.begin(38400);

Code: Select all

....
#if defined(MTK)
/* Using the AXN 1.51 firmware which defaults at 1Hz/38400 but supports binary protocol
* First connect to it with 38400, then set the speed to 115200
* and set the update rate to 10Hz
* and finally switch to Binary mode
*/

Serial.begin(115200);
...

Code: Select all

....
#if defined(MTK)
/* Using the AXN 1.51 firmware which defaults at 1Hz/38400 but supports binary protocol
 * First connect to it with 38400, then set the speed to 115200 
 * and set the update rate to 10Hz
 * and finally switch to Binary mode
 */

Serial.begin(38400);
delay(1500); //let it init
Serial.write("$PMTK251,115200*1F\r\n");
delay(300);
Serial.end();

Serial.begin(115200);
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");
delay(1000);
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");
delay(300);
Serial.write("$PMTK220,100*2F\r\n");
#else
  //open serial port at 115200
  Serial.begin(GPS_SERIAL_SPEED);
#endif
....

Code: Select all

....
#if defined(MTK)
/* Using the AXN 1.51 firmware which defaults at 1Hz/38400 but supports binary protocol
 * First connect to it with 38400, then set the speed to 57600
 * and set the update rate to 10Hz
 * and finally switch to Binary mode
 */

Serial.begin(38400);                          // Handshake
delay(1500); //let it init
Serial.write("$PMTK251,57600*2C\r\n");     // Go down to 57K for data integrity
delay(1000);
Serial.end();
delay(500);
Serial.begin(57600);
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");
delay(1000);
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");
delay(300);
Serial.write("$PMTK220,100*2F\r\n");
#else
  //open serial port at 115200
  Serial.begin(GPS_SERIAL_SPEED);
#endif
....

Code: Select all

{      // we does not have 3d fix or numsats less than 5 , stop navigation
                 nav_lat = 0;
                 nav_lon = 0;
                 GPSMode = GPSMODE_NONAV;
                 nav_mode = NAV_MODE_NONE;
                 wp_distance = 0;
                 i2c_dataset.distance_to_home = 0;
                 i2c_dataset.home_to_copter_bearing = 0;
                 GPS_update_i2c_dataset();
               }

Code: Select all

{      // we does not have 3d fix or numsats less than 5 , stop navigation
                 last_longitude = 0;               //Crashpilot Reset Velocity LON
                 last_latitude  = 0;               //Crashpilot Reset Velocity LAT
                 nav_lat = 0;
                 nav_lon = 0;
                 GPSMode = GPSMODE_NONAV;
                 nav_mode = NAV_MODE_NONE;
                 wp_distance = 0;
                 i2c_dataset.distance_to_home = 0;
                 i2c_dataset.home_to_copter_bearing = 0;
                 GPS_update_i2c_dataset();
               }

Code: Select all

 if (GPS_newFrame(SerialRead(GPS_SERIAL))) {

Code: Select all

 if (MTK_GPS_newFrame((uint8_t)SerialRead(GPS_SERIAL))) {

Code: Select all

  SerialOpen(GPS_SERIAL,38400);
    SerialPrint(GPS_SERIAL,"$PMTK220,200*2C\r\n");
    delay(400);
    SerialPrint(GPS_SERIAL,"$PMTK251,115200*1F\r\n")
    SerialPrint(GPS_SERIAL,"$PGCMD,16,0,0,0,0,0*6A\r\n");
    delay(500);
  SerialEnd(GPS_SERIAL);

Code: Select all

#if defined(MTK)
    struct diyd_mtk_msg {
        int32_t      latitude;
        int32_t      longitude;
        int32_t      altitude;
        int32_t      ground_speed;
        int32_t      ground_course;
        uint8_t      satellites;
        uint8_t      fix_type;
        uint32_t   utc_date;
        uint32_t   utc_time;
        uint16_t   hdop;
    };
// #pragma pack(pop)
    enum diyd_mtk_fix_type {
        FIX_NONE = 1,
        FIX_2D = 2,
        FIX_3D = 3
    };

    enum diyd_mtk_protocol_bytes {
        PREAMBLE1 = 0xd0,
        PREAMBLE2 = 0xdd,
    };

    // Packet checksum accumulators
    uint8_t    _ck_a;
    uint8_t    _ck_b;

    // State machine state
    uint8_t    _step;
    uint8_t      _payload_counter;

    // Time from UNIX Epoch offset
    long      _time_offset;
    bool      _offset_calculated;

    // Receive buffer
    union {
        diyd_mtk_msg   msg;
        uint8_t         bytes[];
    } _buffer;

inline long _swapl(const void *bytes)
{
    const uint8_t   *b = (const uint8_t *)bytes;
    union {
        long   v;
        uint8_t b[4];
    } u;

    u.b[0] = b[3];
    u.b[1] = b[2];
    u.b[2] = b[1];
    u.b[3] = b[0];

    return(u.v);
}

bool MTK_GPS_newFrame(uint8_t data)
{
       bool parsed = false;

restart:
        switch(_step) {

            // Message preamble, class, ID detection
            //
            // If we fail to match any of the expected bytes, we
            // reset the state machine and re-consider the failed
            // byte as the first byte of the preamble.  This
            // improves our chances of recovering from a mismatch
            // and makes it less likely that we will be fooled by
            // the preamble appearing as data in some other message.
            //
        case 0:
            if(PREAMBLE1 == data)
                _step++;
            break;
        case 1:
            if (PREAMBLE2 == data) {
                _step++;
                break;
            }
            _step = 0;
            goto restart;
        case 2:
            if (sizeof(_buffer) == data) {
                _step++;
                _ck_b = _ck_a = data;            // reset the checksum accumulators
                _payload_counter = 0;
            } else {
                _step = 0;                     // reset and wait for a message of the right class
                goto restart;
            }
            break;

            // Receive message data
            //
        case 3:
            _buffer.bytes[_payload_counter++] = data;
            _ck_b += (_ck_a += data);
            if (_payload_counter == sizeof(_buffer))
                _step++;
            break;

            // Checksum and message processing
            //
        case 4:
            _step++;
            if (_ck_a != data) {
                _step = 0;
            }
            break;
        case 5:
            _step = 0;
            if (_ck_b != data) {
                break;
            }

            f.GPS_FIX         = _buffer.msg.fix_type == FIX_3D;
            GPS_coord[LAT]      = _buffer.msg.latitude  * 10;   
            GPS_coord[LON]      = _buffer.msg.longitude * 10;   
            GPS_altitude      = _buffer.msg.altitude;
            GPS_speed                   = _buffer.msg.ground_speed;
            GPS_ground_course           = _buffer.msg.ground_course;
            GPS_numSat              = _buffer.msg.satellites;
            //GPS_hdop         = _buffer.msg.hdop;
            parsed = true;
            GPS_Present = 1;
        }
    return parsed;
}
#endif //MTK