You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
315 lines
10 KiB
315 lines
10 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
// Novatel/Tersus/ComNav GPS driver for ArduPilot. |
|
// Code by Michael Oborne |
|
// Derived from http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf |
|
|
|
#include "AP_GPS.h" |
|
#include "AP_GPS_NOVA.h" |
|
#include <AP_Logger/AP_Logger.h> |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
#define NOVA_DEBUGGING 0 |
|
|
|
#if NOVA_DEBUGGING |
|
#include <cstdio> |
|
# define Debug(fmt, args ...) \ |
|
do { \ |
|
printf("%s:%d: " fmt "\n", \ |
|
__FUNCTION__, __LINE__, \ |
|
## args); \ |
|
hal.scheduler->delay(1); \ |
|
} while(0) |
|
#else |
|
# define Debug(fmt, args ...) |
|
#endif |
|
|
|
AP_GPS_NOVA::AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, |
|
AP_HAL::UARTDriver *_port) : |
|
AP_GPS_Backend(_gps, _state, _port) |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
|
|
const char *init_str = _initialisation_blob[0]; |
|
const char *init_str1 = _initialisation_blob[1]; |
|
|
|
port->write((const uint8_t*)init_str, strlen(init_str)); |
|
port->write((const uint8_t*)init_str1, strlen(init_str1)); |
|
} |
|
|
|
const char* const AP_GPS_NOVA::_initialisation_blob[6] { |
|
"\r\n\r\nunlogall\r\n", // cleanup enviroment |
|
"log bestposb ontime 0.2 0 nohold\r\n", // get bestpos |
|
"log bestvelb ontime 0.2 0 nohold\r\n", // get bestvel |
|
"log psrdopb onchanged\r\n", // tersus |
|
"log psrdopb ontime 0.2\r\n", // comnav |
|
"log psrdopb\r\n" // poll message, as dop only changes when a sat is dropped/added to the visible list |
|
}; |
|
|
|
// Process all bytes available from the stream |
|
// |
|
bool |
|
AP_GPS_NOVA::read(void) |
|
{ |
|
uint32_t now = AP_HAL::millis(); |
|
|
|
if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) { |
|
const char *init_str = _initialisation_blob[_init_blob_index]; |
|
|
|
if (now > _init_blob_time) { |
|
port->write((const uint8_t*)init_str, strlen(init_str)); |
|
_init_blob_time = now + 200; |
|
_init_blob_index++; |
|
} |
|
} |
|
|
|
bool ret = false; |
|
while (port->available() > 0) { |
|
uint8_t temp = port->read(); |
|
ret |= parse(temp); |
|
} |
|
|
|
return ret; |
|
} |
|
|
|
bool |
|
AP_GPS_NOVA::parse(uint8_t temp) |
|
{ |
|
switch (nova_msg.nova_state) |
|
{ |
|
default: |
|
case nova_msg_parser::PREAMBLE1: |
|
if (temp == NOVA_PREAMBLE1) |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE2; |
|
nova_msg.read = 0; |
|
break; |
|
case nova_msg_parser::PREAMBLE2: |
|
if (temp == NOVA_PREAMBLE2) |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE3; |
|
} |
|
else |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
} |
|
break; |
|
case nova_msg_parser::PREAMBLE3: |
|
if (temp == NOVA_PREAMBLE3) |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::HEADERLENGTH; |
|
} |
|
else |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
} |
|
break; |
|
case nova_msg_parser::HEADERLENGTH: |
|
Debug("NOVA HEADERLENGTH\n"); |
|
nova_msg.header.data[0] = NOVA_PREAMBLE1; |
|
nova_msg.header.data[1] = NOVA_PREAMBLE2; |
|
nova_msg.header.data[2] = NOVA_PREAMBLE3; |
|
nova_msg.header.data[3] = temp; |
|
nova_msg.header.nova_headeru.headerlength = temp; |
|
nova_msg.nova_state = nova_msg_parser::HEADERDATA; |
|
nova_msg.read = 4; |
|
break; |
|
case nova_msg_parser::HEADERDATA: |
|
if (nova_msg.read >= sizeof(nova_msg.header.data)) { |
|
Debug("parse header overflow length=%u\n", (unsigned)nova_msg.read); |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
break; |
|
} |
|
nova_msg.header.data[nova_msg.read] = temp; |
|
nova_msg.read++; |
|
if (nova_msg.read >= nova_msg.header.nova_headeru.headerlength) |
|
{ |
|
nova_msg.nova_state = nova_msg_parser::DATA; |
|
} |
|
break; |
|
case nova_msg_parser::DATA: |
|
if (nova_msg.read >= sizeof(nova_msg.data)) { |
|
Debug("parse data overflow length=%u msglength=%u\n", (unsigned)nova_msg.read,nova_msg.header.nova_headeru.messagelength); |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
break; |
|
} |
|
nova_msg.data.bytes[nova_msg.read - nova_msg.header.nova_headeru.headerlength] = temp; |
|
nova_msg.read++; |
|
if (nova_msg.read >= (nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength)) |
|
{ |
|
Debug("NOVA DATA exit\n"); |
|
nova_msg.nova_state = nova_msg_parser::CRC1; |
|
} |
|
break; |
|
case nova_msg_parser::CRC1: |
|
nova_msg.crc = (uint32_t) (temp << 0); |
|
nova_msg.nova_state = nova_msg_parser::CRC2; |
|
break; |
|
case nova_msg_parser::CRC2: |
|
nova_msg.crc += (uint32_t) (temp << 8); |
|
nova_msg.nova_state = nova_msg_parser::CRC3; |
|
break; |
|
case nova_msg_parser::CRC3: |
|
nova_msg.crc += (uint32_t) (temp << 16); |
|
nova_msg.nova_state = nova_msg_parser::CRC4; |
|
break; |
|
case nova_msg_parser::CRC4: |
|
nova_msg.crc += (uint32_t) (temp << 24); |
|
nova_msg.nova_state = nova_msg_parser::PREAMBLE1; |
|
|
|
uint32_t crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.headerlength, (uint8_t *)&nova_msg.header.data, (uint32_t)0); |
|
crc = CalculateBlockCRC32((uint32_t)nova_msg.header.nova_headeru.messagelength, (uint8_t *)&nova_msg.data, crc); |
|
|
|
if (nova_msg.crc == crc) |
|
{ |
|
return process_message(); |
|
} |
|
else |
|
{ |
|
Debug("crc failed"); |
|
crc_error_counter++; |
|
} |
|
break; |
|
} |
|
|
|
return false; |
|
} |
|
|
|
bool |
|
AP_GPS_NOVA::process_message(void) |
|
{ |
|
uint16_t messageid = nova_msg.header.nova_headeru.messageid; |
|
|
|
Debug("NOVA process_message messid=%u\n",messageid); |
|
|
|
check_new_itow(nova_msg.header.nova_headeru.tow, nova_msg.header.nova_headeru.messagelength + nova_msg.header.nova_headeru.headerlength); |
|
|
|
if (messageid == 42) // bestpos |
|
{ |
|
const bestpos &bestposu = nova_msg.data.bestposu; |
|
|
|
state.time_week = nova_msg.header.nova_headeru.week; |
|
state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow; |
|
state.last_gps_time_ms = AP_HAL::millis(); |
|
|
|
state.location.lat = (int32_t) (bestposu.lat * (double)1e7); |
|
state.location.lng = (int32_t) (bestposu.lng * (double)1e7); |
|
state.location.alt = (int32_t) (bestposu.hgt * 100); |
|
|
|
state.num_sats = bestposu.svsused; |
|
|
|
state.horizontal_accuracy = (float) ((bestposu.latsdev + bestposu.lngsdev)/2); |
|
state.vertical_accuracy = (float) bestposu.hgtsdev; |
|
state.have_horizontal_accuracy = true; |
|
state.have_vertical_accuracy = true; |
|
state.rtk_age_ms = bestposu.diffage * 1000; |
|
state.rtk_num_sats = bestposu.svsused; |
|
|
|
if (bestposu.solstat == 0) // have a solution |
|
{ |
|
switch (bestposu.postype) |
|
{ |
|
case 16: |
|
state.status = AP_GPS::GPS_OK_FIX_3D; |
|
break; |
|
case 17: // psrdiff |
|
case 18: // waas |
|
case 20: // omnistar |
|
case 68: // ppp_converg |
|
case 69: // ppp |
|
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; |
|
break; |
|
case 32: // l1 float |
|
case 33: // iono float |
|
case 34: // narrow float |
|
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT; |
|
break; |
|
case 48: // l1 int |
|
case 50: // narrow int |
|
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; |
|
break; |
|
case 0: // NONE |
|
case 1: // FIXEDPOS |
|
case 2: // FIXEDHEIGHT |
|
default: |
|
state.status = AP_GPS::NO_FIX; |
|
break; |
|
} |
|
} |
|
else |
|
{ |
|
state.status = AP_GPS::NO_FIX; |
|
} |
|
|
|
_new_position = true; |
|
} |
|
|
|
if (messageid == 99) // bestvel |
|
{ |
|
const bestvel &bestvelu = nova_msg.data.bestvelu; |
|
|
|
state.ground_speed = (float) bestvelu.horspd; |
|
state.ground_course = (float) bestvelu.trkgnd; |
|
fill_3d_velocity(); |
|
state.velocity.z = -(float) bestvelu.vertspd; |
|
state.have_vertical_velocity = true; |
|
|
|
_last_vel_time = (uint32_t) nova_msg.header.nova_headeru.tow; |
|
_new_speed = true; |
|
} |
|
|
|
if (messageid == 174) // psrdop |
|
{ |
|
const psrdop &psrdopu = nova_msg.data.psrdopu; |
|
|
|
state.hdop = (uint16_t) (psrdopu.hdop*100); |
|
state.vdop = (uint16_t) (psrdopu.htdop*100); |
|
return false; |
|
} |
|
|
|
// ensure out position and velocity stay insync |
|
if (_new_position && _new_speed && _last_vel_time == state.time_week_ms) { |
|
_new_speed = _new_position = false; |
|
|
|
return true; |
|
} |
|
|
|
return false; |
|
} |
|
|
|
#define CRC32_POLYNOMIAL 0xEDB88320L |
|
uint32_t AP_GPS_NOVA::CRC32Value(uint32_t icrc) |
|
{ |
|
int i; |
|
uint32_t crc = icrc; |
|
for ( i = 8 ; i > 0; i-- ) |
|
{ |
|
if ( crc & 1 ) |
|
crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; |
|
else |
|
crc >>= 1; |
|
} |
|
return crc; |
|
} |
|
|
|
uint32_t AP_GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) |
|
{ |
|
while ( length-- != 0 ) |
|
{ |
|
crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); |
|
} |
|
return( crc ); |
|
}
|
|
|