ardupilot/libraries/AP_GPS/AP_GPS_NOVA.h

188 lines
4.6 KiB
C++

/*
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
#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_FIXED; }
// Methods
bool read();
void inject_data(const uint8_t *data, uint16_t len) override;
const char *name() const override { return "NOVA"; }
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 crc_error_counter = 0;
uint32_t last_injected_data_ms = 0;
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;
};