diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index fa83efe1d5..d053933d8c 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -21,6 +21,7 @@ #include #include +#include "AP_GPS_NOVA.h" #include "AP_GPS_ERB.h" #include "AP_GPS_GSOF.h" #include "AP_GPS_MTK.h" @@ -42,7 +43,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE // @DisplayName: GPS type // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:PX4-UAVCAN,10:SBF,11:GSOF + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:PX4-UAVCAN,10:SBF,11:GSOF,12:QURT,13:ERB,14:MAV,15:NOVA // @RebootRequired: True AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1), @@ -244,6 +245,9 @@ AP_GPS::detect_instance(uint8_t instance) } else if ((_type[instance] == GPS_TYPE_GSOF)) { _broadcast_gps_type("GSOF", instance, -1); // baud rate isn't valid new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); + } else if ((_type[instance] == GPS_TYPE_NOVA)) { + _broadcast_gps_type("NOVA", instance, -1); // baud rate isn't valid + new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); } // record the time when we started detection. This is used to try diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 86aa839866..b21ccd3a2a 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -70,6 +70,7 @@ public: GPS_TYPE_QURT = 12, GPS_TYPE_ERB = 13, GPS_TYPE_MAV = 14, + GPS_TYPE_NOVA = 15, }; /// GPS status codes diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp new file mode 100644 index 0000000000..303ad56ff0 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -0,0 +1,314 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +// 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 + +extern const AP_HAL::HAL& hal; + +#define NOVA_DEBUGGING 0 + +#if NOVA_DEBUGGING +#include + # 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), + _new_position(0), + _new_speed(0) +{ + 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)); +} + +// 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); + + 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 = state.time_week_ms; + + state.location.lat = (int32_t) (bestposu.lat*1e7); + state.location.lng = (int32_t) (bestposu.lng*1e7); + state.location.alt = (int32_t) (bestposu.hgt*1e2); + + 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; + + 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 + case 48: // l1 int + case 50: // narrow int + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + 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.last_gps_time_ms) { + _new_speed = _new_position = false; + + return true; + } + + return false; +} + +void +AP_GPS_NOVA::inject_data(uint8_t *data, uint8_t len) +{ + if (port->txspace() > len) { + last_injected_data_ms = AP_HAL::millis(); + port->write(data, len); + } else { + Debug("NOVA: Not enough TXSPACE"); + } +} + +#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 ); +} diff --git a/libraries/AP_GPS/AP_GPS_NOVA.h b/libraries/AP_GPS/AP_GPS_NOVA.h new file mode 100644 index 0000000000..72054e1b70 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_NOVA.h @@ -0,0 +1,188 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 . + */ + +// Novatel/Tersus/ComNav GPS driver for ArduPilot. +// Code by Michael Oborne +// Derived from http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf + +#pragma once + +#include "AP_GPS.h" +#include "GPS_Backend.h" + +class AP_GPS_NOVA : public AP_GPS_Backend +{ +public: + AP_GPS_NOVA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; } + + // Methods + bool read(); + + void inject_data(uint8_t *data, uint8_t len); + +private: + + bool parse(uint8_t temp); + bool process_message(); + uint32_t CRC32Value(uint32_t icrc); + uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); + + static const uint8_t NOVA_PREAMBLE1 = 0xaa; + static const uint8_t NOVA_PREAMBLE2 = 0x44; + static const uint8_t NOVA_PREAMBLE3 = 0x12; + + // do we have new position information? + bool _new_position:1; + // do we have new speed information? + bool _new_speed:1; + + uint32_t _last_vel_time; + + uint8_t _init_blob_index = 0; + uint32_t _init_blob_time = 0; + const char* _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 + }; + + uint32_t last_hdop = 999; + uint32_t crc_error_counter = 0; + uint32_t last_injected_data_ms = 0; + bool validcommand = false; + + struct PACKED nova_header + { + // 0 + uint8_t preamble[3]; + // 3 + uint8_t headerlength; + // 4 + uint16_t messageid; + // 6 + uint8_t messagetype; + //7 + uint8_t portaddr; + //8 + uint16_t messagelength; + //10 + uint16_t sequence; + //12 + uint8_t idletime; + //13 + uint8_t timestatus; + //14 + uint16_t week; + //16 + uint32_t tow; + //20 + uint32_t recvstatus; + // 24 + uint16_t resv; + //26 + uint16_t recvswver; + }; + + struct PACKED psrdop + { + float gdop; + float pdop; + float hdop; + float htdop; + float tdop; + float cutoff; + uint32_t svcount; + // extra data for individual prns + }; + + struct PACKED bestpos + { + uint32_t solstat; + uint32_t postype; + double lat; + double lng; + double hgt; + float undulation; + uint32_t datumid; + float latsdev; + float lngsdev; + float hgtsdev; + // 4 bytes + uint8_t stnid[4]; + float diffage; + float sol_age; + uint8_t svstracked; + uint8_t svsused; + uint8_t svsl1; + uint8_t svsmultfreq; + uint8_t resv; + uint8_t extsolstat; + uint8_t galbeisigmask; + uint8_t gpsglosigmask; + }; + + struct PACKED bestvel + { + uint32_t solstat; + uint32_t veltype; + float latency; + float age; + double horspd; + double trkgnd; + // + up + double vertspd; + float resv; + }; + + union PACKED msgbuffer { + bestvel bestvelu; + bestpos bestposu; + psrdop psrdopu; + uint8_t bytes[256]; + }; + + union PACKED msgheader { + nova_header nova_headeru; + uint8_t data[28]; + }; + + struct PACKED nova_msg_parser + { + enum + { + PREAMBLE1 = 0, + PREAMBLE2, + PREAMBLE3, + HEADERLENGTH, + HEADERDATA, + DATA, + CRC1, + CRC2, + CRC3, + CRC4, + } nova_state; + + msgbuffer data; + uint32_t crc; + msgheader header; + uint16_t read; + } nova_msg; +}; diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 908c2336ee..b64fc5d369 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -118,6 +118,10 @@ private: void _sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); void _update_gps_sbp(const struct gps_data *d); void _update_gps_file(const struct gps_data *d); + void _update_gps_nova(const struct gps_data *d); + void _nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); + uint32_t CRC32Value(uint32_t icrc); + uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); void _update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, double speedD, bool have_lock); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 29a3f50596..63870399be 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -727,6 +727,174 @@ void SITL_State::_update_gps_sbp(const struct gps_data *d) do_every_count++; } +void SITL_State::_update_gps_nova(const struct gps_data *d) +{ + static struct PACKED nova_header + { + // 0 + uint8_t preamble[3]; + // 3 + uint8_t headerlength; + // 4 + uint16_t messageid; + // 6 + uint8_t messagetype; + //7 + uint8_t portaddr; + //8 + uint16_t messagelength; + //10 + uint16_t sequence; + //12 + uint8_t idletime; + //13 + uint8_t timestatus; + //14 + uint16_t week; + //16 + uint32_t tow; + //20 + uint32_t recvstatus; + // 24 + uint16_t resv; + //26 + uint16_t recvswver; + } header; + + struct PACKED psrdop + { + float gdop; + float pdop; + float hdop; + float htdop; + float tdop; + float cutoff; + uint32_t svcount; + // extra data for individual prns + } psrdop; + + struct PACKED bestpos + { + uint32_t solstat; + uint32_t postype; + double lat; + double lng; + double hgt; + float undulation; + uint32_t datumid; + float latsdev; + float lngsdev; + float hgtsdev; + // 4 bytes + uint8_t stnid[4]; + float diffage; + float sol_age; + uint8_t svstracked; + uint8_t svsused; + uint8_t svsl1; + uint8_t svsmultfreq; + uint8_t resv; + uint8_t extsolstat; + uint8_t galbeisigmask; + uint8_t gpsglosigmask; + } bestpos; + + struct PACKED bestvel + { + uint32_t solstat; + uint32_t veltype; + float latency; + float age; + double horspd; + double trkgnd; + // + up + double vertspd; + float resv; + } bestvel; + + uint16_t time_week; + uint32_t time_week_ms; + + gps_time(&time_week, &time_week_ms); + + header.preamble[0] = 0xaa; + header.preamble[1] = 0x44; + header.preamble[2] = 0x12; + header.headerlength = sizeof(header); + header.week = time_week; + header.tow = time_week_ms; + + header.messageid = 174; + header.messagelength = sizeof(psrdop); + header.sequence += 1; + + psrdop.hdop = 1.20; + psrdop.htdop = 1.20; + _nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop)); + + + header.messageid = 99; + header.messagelength = sizeof(bestvel); + header.sequence += 1; + + bestvel.horspd = norm(d->speedN, d->speedE); + bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN)); + bestvel.vertspd = -d->speedD; + + _nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel)); + + + header.messageid = 42; + header.messagelength = sizeof(bestpos); + header.sequence += 1; + + bestpos.lat = d->latitude; + bestpos.lng = d->longitude; + bestpos.hgt = d->altitude; + bestpos.svsused = _sitl->gps_numsats; + bestpos.latsdev=0.2; + bestpos.lngsdev=0.2; + bestpos.hgtsdev=0.2; + bestpos.solstat=0; + bestpos.postype=32; + + _nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos)); +} + +void SITL_State::_nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen) +{ + _gps_write(header, headerlength); + _gps_write(payload, payloadlen); + + uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0); + crc = CalculateBlockCRC32(payloadlen, payload, crc); + + _gps_write((uint8_t*)&crc, 4); +} + +#define CRC32_POLYNOMIAL 0xEDB88320L +uint32_t SITL_State::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 SITL_State::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 ); +} /* temporary method to use file as GPS data @@ -854,6 +1022,10 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, case SITL::SITL::GPS_TYPE_SBP: _update_gps_sbp(&d); break; + + case SITL::SITL::GPS_TYPE_NOVA: + _update_gps_nova(&d); + break; case SITL::SITL::GPS_TYPE_FILE: _update_gps_file(&d); diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 5c3e4fd381..b236db8794 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -48,7 +48,8 @@ public: GPS_TYPE_MTK19 = 4, GPS_TYPE_NMEA = 5, GPS_TYPE_SBP = 6, - GPS_TYPE_FILE = 7 + GPS_TYPE_FILE = 7, + GPS_TYPE_NOVA = 8, }; struct sitl_fdm state;