Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             :  //
      17             :  //  Emlid Reach Binary (ERB) GPS driver for ArduPilot.
      18             :  //  ERB protocol: http://files.emlid.com/ERB.pdf
      19             : 
      20             : #include "AP_GPS.h"
      21             : #include "AP_GPS_ERB.h"
      22             : 
      23             : #if AP_GPS_ERB_ENABLED
      24             : 
      25             : #define ERB_DEBUGGING 0
      26             : 
      27             : #define STAT_FIX_VALID 0x01
      28             : 
      29             : #include <AP_HAL/AP_HAL.h>
      30             : 
      31             : extern const AP_HAL::HAL& hal;
      32             : 
      33             : #if ERB_DEBUGGING
      34             :  # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
      35             : #else
      36             :  # define Debug(fmt, args ...)
      37             : #endif
      38             : 
      39             : // Process bytes available from the stream
      40             : //
      41             : // The stream is assumed to contain only messages we recognise.  If it
      42             : // contains other messages, and those messages contain the preamble
      43             : // bytes, it is possible for this code to fail to synchronise to the
      44             : // stream immediately.  Without buffering the entire message and
      45             : // re-processing it from the top, this is unavoidable. The parser
      46             : // attempts to avoid this when possible.
      47             : //
      48             : bool
      49           0 : AP_GPS_ERB::read(void)
      50             : {
      51             :     uint8_t data;
      52             :     int16_t numc;
      53           0 :     bool parsed = false;
      54             : 
      55           0 :     numc = port->available();
      56           0 :     for (int16_t i = 0; i < numc; i++) {        // Process bytes received
      57             : 
      58             :         // read the next byte
      59           0 :         data = port->read();
      60             : #if AP_GPS_DEBUG_LOGGING_ENABLED
      61             :         log_data(&data, 1);
      62             : #endif
      63             : 
      64           0 :         reset:
      65           0 :         switch(_step) {
      66             : 
      67             :         // Message preamble detection
      68             :         //
      69           0 :         case 1:
      70           0 :             if (PREAMBLE2 == data) {
      71           0 :                 _step++;
      72           0 :                 break;
      73             :             }
      74           0 :             _step = 0;
      75             :             Debug("reset %u", __LINE__);
      76             :             FALLTHROUGH;
      77           0 :         case 0:
      78           0 :             if(PREAMBLE1 == data)
      79           0 :                 _step++;
      80           0 :             break;
      81             : 
      82             :         // Message header processing
      83             :         //
      84           0 :         case 2:
      85           0 :             _step++;
      86           0 :             _msg_id = data;
      87           0 :             _ck_b = _ck_a = data;                       // reset the checksum accumulators
      88           0 :             break;
      89           0 :         case 3:
      90           0 :             _step++;
      91           0 :             _ck_b += (_ck_a += data);                   // checksum byte
      92           0 :             _payload_length = data;                     // payload length low byte
      93           0 :             break;
      94           0 :         case 4:
      95           0 :             _step++;
      96           0 :             _ck_b += (_ck_a += data);                   // checksum byte
      97           0 :             _payload_length += (uint16_t)(data<<8);
      98           0 :             _payload_counter = 0;                       // prepare to receive payload
      99           0 :             break;
     100             : 
     101             :         // Receive message data
     102             :         //
     103           0 :         case 5:
     104           0 :             _ck_b += (_ck_a += data);                   // checksum byte
     105           0 :             if (_payload_counter < sizeof(_buffer)) {
     106           0 :                 _buffer[_payload_counter] = data;
     107             :             }
     108           0 :             if (++_payload_counter == _payload_length)
     109           0 :                 _step++;
     110           0 :             break;
     111             : 
     112             :         // Checksum and message processing
     113             :         //
     114           0 :         case 6:
     115           0 :             _step++;
     116           0 :             if (_ck_a != data) {
     117             :                 Debug("bad cka %x should be %x", data, _ck_a);
     118           0 :                 _step = 0;
     119           0 :                 goto reset;
     120             :             }
     121           0 :             break;
     122           0 :         case 7:
     123           0 :             _step = 0;
     124           0 :             if (_ck_b != data) {
     125             :                 Debug("bad ckb %x should be %x", data, _ck_b);
     126           0 :                 break;                                  // bad checksum
     127             :             }
     128             : 
     129           0 :             if (_parse_gps()) {
     130           0 :                 parsed = true;
     131             :             }
     132           0 :             break;
     133             :         }
     134             :     }
     135           0 :     return parsed;
     136             : }
     137             : 
     138             : bool
     139           0 : AP_GPS_ERB::_parse_gps(void)
     140             : {
     141           0 :     switch (_msg_id) {
     142           0 :     case MSG_VER:
     143             :         Debug("Version of ERB protocol %u.%u.%u",
     144             :               _buffer.ver.ver_high,
     145             :               _buffer.ver.ver_medium,
     146             :               _buffer.ver.ver_low);
     147           0 :         break;
     148           0 :     case MSG_POS:
     149             :         Debug("Message POS");
     150           0 :         _last_pos_time        = _buffer.pos.time;
     151           0 :         state.location.lng    = (int32_t)(_buffer.pos.longitude * (double)1e7);
     152           0 :         state.location.lat    = (int32_t)(_buffer.pos.latitude * (double)1e7);
     153           0 :         state.have_undulation = true;
     154           0 :         state.undulation = _buffer.pos.altitude_msl - _buffer.pos.altitude_ellipsoid;
     155           0 :         set_alt_amsl_cm(state, _buffer.pos.altitude_msl * 100);
     156           0 :         state.status          = next_fix;
     157           0 :         _new_position = true;
     158           0 :         state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;
     159           0 :         state.vertical_accuracy = _buffer.pos.vertical_accuracy * 1.0e-3f;
     160           0 :         state.have_horizontal_accuracy = true;
     161           0 :         state.have_vertical_accuracy = true;
     162           0 :         break;
     163           0 :     case MSG_STAT:
     164             :         Debug("Message STAT fix_status=%u fix_type=%u",
     165             :               _buffer.stat.fix_status,
     166             :               _buffer.stat.fix_type);
     167           0 :         if (_buffer.stat.fix_status & STAT_FIX_VALID) {
     168           0 :             if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FIX) {
     169           0 :                 next_fix = AP_GPS_FixType::RTK_FIXED;
     170           0 :             } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_FLOAT) {
     171           0 :                 next_fix = AP_GPS_FixType::RTK_FLOAT;
     172           0 :             } else if (_buffer.stat.fix_type == AP_GPS_ERB::FIX_SINGLE) {
     173           0 :                 next_fix = AP_GPS_FixType::FIX_3D;
     174             :             } else {
     175           0 :                 next_fix = AP_GPS_FixType::NONE;
     176           0 :                 state.status = AP_GPS_FixType::NONE;
     177             :             }
     178             :         } else {
     179           0 :             next_fix = AP_GPS_FixType::NONE;
     180           0 :             state.status = AP_GPS_FixType::NONE;
     181             :         }
     182           0 :         state.num_sats = _buffer.stat.satellites;
     183           0 :         if (next_fix >= AP_GPS_FixType::FIX_3D) {
     184             :             // use the uart receive time to make packet timestamps more accurate
     185           0 :             set_uart_timestamp(_payload_length + sizeof(erb_header) + 2);
     186           0 :             state.last_gps_time_ms = AP_HAL::millis();
     187           0 :             state.time_week_ms    = _buffer.stat.time;
     188           0 :             state.time_week       = _buffer.stat.week;
     189             :         }
     190           0 :         break;
     191           0 :     case MSG_DOPS:
     192             :         Debug("Message DOPS");
     193           0 :         state.hdop = _buffer.dops.hDOP;
     194           0 :         state.vdop = _buffer.dops.vDOP;
     195           0 :         break;
     196           0 :     case MSG_VEL:
     197             :         Debug("Message VEL");
     198           0 :         _last_vel_time         = _buffer.vel.time;
     199           0 :         state.ground_speed     = _buffer.vel.speed_2d * 0.01f;        // m/s
     200             :         // Heading 2D deg * 100000 rescaled to deg * 100
     201           0 :         state.ground_course = wrap_360(_buffer.vel.heading_2d * 1.0e-5f);
     202           0 :         state.have_vertical_velocity = true;
     203           0 :         state.velocity.x = _buffer.vel.vel_north * 0.01f;
     204           0 :         state.velocity.y = _buffer.vel.vel_east * 0.01f;
     205           0 :         state.velocity.z = _buffer.vel.vel_down * 0.01f;
     206           0 :         state.have_speed_accuracy = true;
     207           0 :         state.speed_accuracy = _buffer.vel.speed_accuracy * 0.01f;
     208           0 :         _new_speed = true;
     209           0 :         break;
     210           0 :     case MSG_RTK:
     211             :         Debug("Message RTK");
     212           0 :         state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED;
     213           0 :         state.rtk_num_sats      = _buffer.rtk.base_num_sats;
     214           0 :         if (_buffer.rtk.age_cs == 0xFFFF) {
     215           0 :             state.rtk_age_ms    = 0xFFFFFFFF;
     216             :         } else {
     217           0 :             state.rtk_age_ms    = _buffer.rtk.age_cs * 10;
     218             :         }
     219           0 :         state.rtk_baseline_x_mm = _buffer.rtk.baseline_N_mm;
     220           0 :         state.rtk_baseline_y_mm = _buffer.rtk.baseline_E_mm;
     221           0 :         state.rtk_baseline_z_mm = _buffer.rtk.baseline_D_mm;
     222           0 :         state.rtk_accuracy      = _buffer.rtk.ar_ratio;
     223             : 
     224           0 :         state.rtk_week_number   = _buffer.rtk.base_week_number;
     225           0 :         state.rtk_time_week_ms  = _buffer.rtk.base_time_week_ms;
     226           0 :         break;
     227           0 :     default:
     228             :         Debug("Unexpected message 0x%02x", (unsigned)_msg_id);
     229           0 :         return false;
     230             :     }
     231             :     // we only return true when we get new position and speed data
     232             :     // this ensures we don't use stale data
     233           0 :     if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
     234           0 :         _new_speed = _new_position = false;
     235           0 :         _fix_count++;
     236           0 :         return true;
     237             :     }
     238           0 :     return false;
     239             : }
     240             : 
     241             : /*
     242             :   detect a ERB GPS. Adds one byte, and returns true if the stream
     243             :   matches a ERB
     244             :  */
     245             : bool
     246    23038517 : AP_GPS_ERB::_detect(struct ERB_detect_state &state, uint8_t data)
     247             : {
     248    23038517 : reset:
     249    23038517 :     switch (state.step) {
     250        4533 :         case 1:
     251        4533 :             if (PREAMBLE2 == data) {
     252           1 :                 state.step++;
     253           1 :                 break;
     254             :             }
     255        4532 :             state.step = 0;
     256             :             FALLTHROUGH;
     257    23038414 :         case 0:
     258    23038414 :             if (PREAMBLE1 == data)
     259        4538 :                 state.step++;
     260    23038414 :             break;
     261           1 :         case 2:
     262           1 :             state.step++;
     263           1 :             state.ck_b = state.ck_a = data;
     264           1 :             break;
     265           1 :         case 3:
     266           1 :             state.step++;
     267           1 :             state.ck_b += (state.ck_a += data);
     268           1 :             state.payload_length = data;
     269           1 :             break;
     270           1 :         case 4:
     271           1 :             state.step++;
     272           1 :             state.ck_b += (state.ck_a += data);
     273           1 :             state.payload_counter = 0;
     274           1 :             break;
     275          98 :         case 5:
     276          98 :             state.ck_b += (state.ck_a += data);
     277          98 :             if (++state.payload_counter == state.payload_length)
     278           1 :                 state.step++;
     279          98 :             break;
     280           1 :         case 6:
     281           1 :             state.step++;
     282           1 :             if (state.ck_a != data) {
     283           1 :                 state.step = 0;
     284           1 :                 goto reset;
     285             :             }
     286           0 :             break;
     287           0 :         case 7:
     288           0 :             state.step = 0;
     289           0 :             if (state.ck_b == data) {
     290           0 :                 return true;
     291             :             } else {
     292           0 :                 goto reset;
     293             :             }
     294             :     }
     295    23038516 :     return false;
     296             : }
     297             : #endif