Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS_NOVA: support tersus/novatel/comnav gps

Browse Source
This commit is contained in:
Michael Oborne 2016-05-16 21:09:06 +08:00 committed by Andrew Tridgell
parent 8f376944aa
commit 11c376588d
7 changed files with 686 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_GPS/AP_GPS.cpp
View File

@ -21,6 +21,7 @@
#include <AP_Notify/AP_Notify.h>
#include <GCS_MAVLink/GCS.h>
#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

1
libraries/AP_GPS/AP_GPS.h
View File

@ -70,6 +70,7 @@ public:
GPS_TYPE_QURT = 12,
GPS_TYPE_ERB = 13,
GPS_TYPE_MAV = 14,
GPS_TYPE_NOVA = 15,
};
/// GPS status codes

314
libraries/AP_GPS/AP_GPS_NOVA.cpp Normal file
View File

@ -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 <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 <DataFlash/DataFlash.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),
_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 );
}

188
libraries/AP_GPS/AP_GPS_NOVA.h Normal file
View File

@ -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 <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; }
// 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;
};

4
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -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);

172
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -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);

3
libraries/SITL/SITL.h
View File

@ -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;