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             : //  Novatel/Tersus/ComNav GPS driver for ArduPilot.
      17             : //  Code by Michael Oborne
      18             : //  Derived from https://hexagondownloads.blob.core.windows.net/public/Novatel/assets/Documents/Manuals/om-20000129/om-20000129.pdf
      19             : 
      20             : #include "AP_GPS.h"
      21             : #include "AP_GPS_NOVA.h"
      22             : #include <AP_Logger/AP_Logger.h>
      23             : 
      24             : #if AP_GPS_NOVA_ENABLED
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : #define NOVA_DEBUGGING 0
      29             : 
      30             : #if NOVA_DEBUGGING
      31             : #include <cstdio>
      32             :  # define Debug(fmt, args ...)                  \
      33             : do {                                            \
      34             :     printf("%s:%d: " fmt "\n",     \
      35             :                         __FUNCTION__, __LINE__, \
      36             :                         ## args);               \
      37             :     hal.scheduler->delay(1);                    \
      38             : } while(0)
      39             : #else
      40             :  # define Debug(fmt, args ...)
      41             : #endif
      42             : 
      43           4 : AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps,
      44             :                          AP_GPS::Params &_params,
      45             :                          AP_GPS::GPS_State &_state,
      46           4 :                          AP_HAL::UARTDriver *_port) :
      47           4 :     AP_GPS_Backend(_gps, _params, _state, _port)
      48             : {
      49           4 :     nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
      50             : 
      51           4 :     nova_msg.header.data[0] = NOVA_PREAMBLE1;
      52           4 :     nova_msg.header.data[1] = NOVA_PREAMBLE2;
      53           4 :     nova_msg.header.data[2] = NOVA_PREAMBLE3;
      54             : 
      55           4 :     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
      56           4 :         const char *init_str = _initialisation_blob[0];
      57           4 :         const char *init_str1 = _initialisation_blob[1];
      58             : 
      59           4 :         port->write((const uint8_t*)init_str, strlen(init_str));
      60           4 :         port->write((const uint8_t*)init_str1, strlen(init_str1));
      61             :     }
      62           4 : }
      63             : 
      64             : const char* const AP_GPS_NOVA::_initialisation_blob[4] {
      65             :     "\r\n\r\nunlogall\r\n", // cleanup enviroment
      66             :     "log bestposb ontime 0.2 0 nohold\r\n",
      67             :     "log bestvelb ontime 0.2 0 nohold\r\n",
      68             :     "log psrdopb ontime 0.2 0 nohold\r\n",
      69             : };
      70             : 
      71             : // Process all bytes available from the stream
      72             : //
      73             : bool
      74        1190 : AP_GPS_NOVA::read(void)
      75             : {
      76        1190 :     if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
      77        1190 :         const uint32_t now = AP_HAL::millis();
      78             : 
      79        1190 :         if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {
      80         136 :             const char *init_str = _initialisation_blob[_init_blob_index];
      81             : 
      82         136 :             if (now > _init_blob_time) {
      83          16 :                 port->write((const uint8_t*)init_str, strlen(init_str));
      84          16 :                 _init_blob_time = now + 200;
      85          16 :                 _init_blob_index++;
      86             :             }
      87             :         }
      88             :     }
      89             : 
      90        1190 :     bool ret = false;
      91       14671 :     for (uint16_t i=0; i<8192; i++) {
      92             :         uint8_t temp;
      93       14671 :         if (!port->read(temp)) {
      94        1190 :             break;
      95             :         }
      96             : #if AP_GPS_DEBUG_LOGGING_ENABLED
      97             :         log_data(&temp, 1);
      98             : #endif
      99       13481 :         ret |= parse(temp);
     100             :     }
     101             :     
     102        1190 :     return ret;
     103             : }
     104             : 
     105             : bool
     106       13481 : AP_GPS_NOVA::parse(uint8_t temp)
     107             : {
     108       13481 :     switch (nova_msg.nova_state)
     109             :     {
     110        1157 :         default:
     111             :         case nova_msg_parser::PREAMBLE1:
     112        1157 :             if (temp == NOVA_PREAMBLE1) {
     113         156 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE2;
     114             :             }
     115        1157 :             nova_msg.read = 0;
     116        1157 :             break;
     117         156 :         case nova_msg_parser::PREAMBLE2:
     118         156 :             if (temp == NOVA_PREAMBLE2) {
     119         156 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE3;
     120             :             } else {
     121           0 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
     122             :             }
     123         156 :             break;
     124         156 :         case nova_msg_parser::PREAMBLE3:
     125         156 :             if (temp == NOVA_PREAMBLE3) {
     126         156 :                 nova_msg.nova_state = nova_msg_parser::HEADERLENGTH;
     127             :             } else {
     128           0 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
     129             :             }
     130         156 :             break;
     131         156 :         case nova_msg_parser::HEADERLENGTH:
     132             :             Debug("NOVA HEADERLENGTH\n");
     133         156 :             nova_msg.header.data[3] = temp;
     134         156 :             nova_msg.header.nova_headeru.headerlength = temp;
     135         156 :             nova_msg.nova_state = nova_msg_parser::HEADERDATA;
     136         156 :             nova_msg.read = 4;
     137         156 :             break;
     138        3744 :         case nova_msg_parser::HEADERDATA:
     139        3744 :             if (nova_msg.read >= sizeof(nova_msg.header.data)) {
     140             :                 Debug("parse header overflow length=%u\n", (unsigned)nova_msg.read);
     141           0 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
     142           0 :                 break;
     143             :             }
     144        3744 :             nova_msg.header.data[nova_msg.read] = temp;
     145        3744 :             nova_msg.read++;
     146        3744 :             if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength) {
     147         156 :                 nova_msg.nova_state = nova_msg_parser::DATA;
     148             :             }
     149        3744 :             break;
     150        7488 :         case nova_msg_parser::DATA:
     151        7488 :             if (nova_msg.read >= sizeof(nova_msg.data)) {
     152             :                 Debug("parse data overflow length=%u msglength=%u\n", (unsigned)nova_msg.read,nova_msg.header.nova_headeru.messagelength);
     153           0 :                 nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
     154           0 :                 break;
     155             :             }
     156        7488 :             nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp;
     157        7488 :             nova_msg.read++;
     158        7488 :             if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)) {
     159             :                 Debug("NOVA DATA exit\n");
     160         156 :                 nova_msg.nova_state = nova_msg_parser::CRC1;
     161             :             }
     162        7488 :             break;
     163         156 :         case nova_msg_parser::CRC1:
     164         156 :             nova_msg.crc = (uint32_t) (temp << 0);
     165         156 :             nova_msg.nova_state = nova_msg_parser::CRC2;
     166         156 :             break;
     167         156 :         case nova_msg_parser::CRC2:
     168         156 :             nova_msg.crc += (uint32_t) (temp << 8);
     169         156 :             nova_msg.nova_state = nova_msg_parser::CRC3;
     170         156 :             break;
     171         156 :         case nova_msg_parser::CRC3:
     172         156 :             nova_msg.crc += (uint32_t) (temp << 16);
     173         156 :             nova_msg.nova_state = nova_msg_parser::CRC4;
     174         156 :             break;
     175         156 :         case nova_msg_parser::CRC4:
     176         156 :             nova_msg.crc += (uint32_t) (temp << 24);
     177         156 :             nova_msg.nova_state = nova_msg_parser::PREAMBLE1;
     178             : 
     179         156 :             uint32_t crc = crc_crc32((uint32_t)0, (uint8_t *)&nova_msg.header.data, (uint32_t)nova_msg.header.nova_headeru.headerlength);
     180         156 :             crc = crc_crc32(crc, (uint8_t *)&nova_msg.data, (uint32_t)nova_msg.header.nova_headeru.messagelength);
     181             : 
     182         156 :             if (nova_msg.crc == crc) {
     183         156 :                 return process_message();
     184             :             } else {
     185             :                 Debug("crc failed");
     186           0 :                 crc_error_counter++;
     187             :             }
     188           0 :             break;
     189             :     }
     190             : 
     191       13325 :     return false;
     192             : }
     193             : 
     194             : bool
     195         156 : AP_GPS_NOVA::process_message(void)
     196             : {
     197         156 :     const uint16_t messageid = nova_msg.header.nova_headeru.messageid;
     198             : 
     199             :     Debug("NOVA process_message messid=%u\n",messageid);
     200             : 
     201         156 :     check_new_itow(nova_msg.header.nova_headeru.tow, nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength);
     202             :  
     203         156 :     switch (messageid) {
     204          52 :     case NOVA_BESTPOS: {  // bestpos
     205          52 :         const bestpos &bestposu = nova_msg.data.bestposu;
     206             : 
     207          52 :         state.time_week = nova_msg.header.nova_headeru.week;
     208          52 :         state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow;
     209          52 :         state.last_gps_time_ms = AP_HAL::millis();
     210             : 
     211          52 :         state.location.lat = (int32_t) (bestposu.lat * (double)1e7);
     212          52 :         state.location.lng = (int32_t) (bestposu.lng * (double)1e7);
     213          52 :         state.have_undulation = true;
     214          52 :         state.undulation = -bestposu.undulation;
     215          52 :         set_alt_amsl_cm(state, bestposu.hgt * 100);
     216             : 
     217          52 :         state.num_sats = bestposu.svsused;
     218             : 
     219          52 :         state.horizontal_accuracy =  norm(bestposu.latsdev, bestposu.lngsdev);
     220          52 :         state.vertical_accuracy = (float) bestposu.hgtsdev;
     221          52 :         state.have_horizontal_accuracy = true;
     222          52 :         state.have_vertical_accuracy = true;
     223          52 :         state.rtk_age_ms = bestposu.diffage * 1000;
     224          52 :         state.rtk_num_sats = bestposu.svsused;
     225             : 
     226          52 :         if (bestposu.solstat == 0) // have a solution
     227             :         {
     228          52 :             switch (bestposu.postype)
     229             :             {
     230           0 :                 case 16:
     231           0 :                     state.status = AP_GPS_FixType::FIX_3D;
     232           0 :                     break;
     233           0 :                 case 17: // psrdiff
     234             :                 case 18: // waas
     235             :                 case 20: // omnistar
     236             :                 case 68: // ppp_converg
     237             :                 case 69: // ppp
     238           0 :                     state.status = AP_GPS_FixType::DGPS;
     239           0 :                     break;
     240          52 :                 case 32: // l1 float
     241             :                 case 33: // iono float
     242             :                 case 34: // narrow float
     243          52 :                     state.status = AP_GPS_FixType::RTK_FLOAT;
     244          52 :                     break;
     245           0 :                 case 48: // l1 int
     246             :                 case 50: // narrow int
     247           0 :                     state.status = AP_GPS_FixType::RTK_FIXED;
     248           0 :                     break;
     249           0 :                 case 0: // NONE
     250             :                 case 1: // FIXEDPOS
     251             :                 case 2: // FIXEDHEIGHT
     252             :                 default:
     253           0 :                     state.status = AP_GPS_FixType::NONE;
     254           0 :                     break;
     255             :             }
     256             :         } else {
     257           0 :             state.status = AP_GPS_FixType::NONE;
     258             :         }
     259             :         
     260          52 :         _new_position = true;
     261          52 :         break;
     262             :     }
     263          52 :     case NOVA_BESTVEL: {  // bestvel
     264          52 :         const bestvel &bestvelu = nova_msg.data.bestvelu;
     265             : 
     266          52 :         state.ground_speed = (float) bestvelu.horspd;
     267          52 :         state.ground_course = (float) bestvelu.trkgnd;
     268          52 :         fill_3d_velocity();
     269          52 :         state.velocity.z = -(float) bestvelu.vertspd;
     270          52 :         state.have_vertical_velocity = true;
     271             :         
     272          52 :         _last_vel_time = (uint32_t) nova_msg.header.nova_headeru.tow;
     273          52 :         _new_speed = true;
     274          52 :         break;
     275             :     }
     276          52 :     case NOVA_PSRDOP: {  // psrdop
     277          52 :         const psrdop &psrdopu = nova_msg.data.psrdopu;
     278             : 
     279          52 :         state.hdop = (uint16_t) (psrdopu.hdop*100);
     280          52 :         state.vdop = (uint16_t) (psrdopu.htdop*100);
     281          52 :         return false;
     282             :     }
     283           0 :     default:
     284           0 :         break;
     285             :     }
     286             : 
     287             :     // ensure out position and velocity stay insync
     288         104 :     if (_new_position && _new_speed && _last_vel_time == state.time_week_ms) {
     289          52 :         _new_speed = _new_position = false;
     290             :         
     291          52 :         return true;
     292             :     }
     293             :     
     294          52 :     return false;
     295             : }
     296             : 
     297             : #endif