Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: SwiftNav RTK Driver and GPS AutoSwitch param

Browse Source
This commit is contained in:
Niels Joubert 2014-06-06 14:58:11 -07:00 committed by Andrew Tridgell
parent fee79c5bac
commit 2b1169b0ab
5 changed files with 925 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

183
libraries/AP_GPS/AP_GPS.cpp
View File

@ -27,15 +27,17 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Param: TYPE // @Param: TYPE
// @DisplayName: GPS type // @DisplayName: GPS type
// @Description: GPS type // @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1), AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1),
#if GPS_MAX_INSTANCES > 1 #if GPS_MAX_INSTANCES > 1
// @Param: TYPE2 // @Param: TYPE2
// @DisplayName: 2nd GPS type // @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS // @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftBinaryProtocol // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
#endif #endif
// @Param: NAVFILTER // @Param: NAVFILTER
@ -44,6 +46,24 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G // @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G
AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G), AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
#if GPS_MAX_INSTANCES > 1
// @Param: AUTO_SWITCH
// @DisplayName: Automatic Switchover Setting
// @Description: Automatic switchover to GPS reporting best lock
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1),
#endif
#if GPS_RTK_AVAILABLE
// @Param: DGPS_MIN_LOCK
// @DisplayName: Minimum Lock Type Accepted for DGPS
// @Description: Sets the minimum type of differential GPS corrections required before allowing to switch into DGPS mode.
// @Values: 0:Any,50:FloatRTK,100:IntegerRTK
// @User: Advanced
AP_GROUPINFO("MIN_DGPS", 4, AP_GPS, _min_dgps, 100),
#endif
AP_GROUPEND AP_GROUPEND
}; };
@ -53,6 +73,7 @@ void AP_GPS::init(DataFlash_Class *dataflash)
_DataFlash = dataflash; _DataFlash = dataflash;
hal.uartB->begin(38400UL, 256, 16); hal.uartB->begin(38400UL, 256, 16);
#if GPS_MAX_INSTANCES > 1 #if GPS_MAX_INSTANCES > 1
primary_instance = 0;
if (hal.uartE != NULL) { if (hal.uartE != NULL) {
hal.uartE->begin(38400UL, 256, 16); hal.uartE->begin(38400UL, 256, 16);
} }
@ -165,7 +186,7 @@ AP_GPS::detect_instance(uint8_t instance)
hal.console->print_P(PSTR(" MTK ")); hal.console->print_P(PSTR(" MTK "));
new_gps = new AP_GPS_MTK(*this, state[instance], port); new_gps = new AP_GPS_MTK(*this, state[instance], port);
} }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #if GPS_RTK_AVAILABLE
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
hal.console->print_P(PSTR(" SBP ")); hal.console->print_P(PSTR(" SBP "));
@ -198,6 +219,55 @@ AP_GPS::detect_instance(uint8_t instance)
} }
} }
bool
AP_GPS::can_calculate_base_pos(void)
{
#if GPS_RTK_AVAILABLE
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
return true;
}
}
#endif
return false;
}
/*
Tells the underlying GPS drivers to capture its current position as home.
*/
void
AP_GPS::calculate_base_pos(void)
{
#if GPS_RTK_AVAILABLE
for (uint8_t i = 0; i<GPS_MAX_INSTANCES; i++) {
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) {
drivers[i]->calculate_base_pos();
}
}
#endif
}
AP_GPS::GPS_Status
AP_GPS::highest_supported_status(uint8_t instance) const
{
#if GPS_RTK_AVAILABLE
if (drivers[instance] != NULL)
return drivers[instance]->highest_supported_status();
#endif
return AP_GPS::GPS_OK_FIX_3D;
}
AP_GPS::GPS_Status
AP_GPS::highest_supported_status(void) const
{
#if GPS_RTK_AVAILABLE
if (drivers[primary_instance] != NULL)
return drivers[primary_instance]->highest_supported_status();
#endif
return AP_GPS::GPS_OK_FIX_3D;
}
/* /*
update one GPS instance. This should be called at 10Hz or greater update one GPS instance. This should be called at 10Hz or greater
@ -260,6 +330,7 @@ AP_GPS::update_instance(uint8_t instance)
void void
AP_GPS::update(void) AP_GPS::update(void)
{ {
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
update_instance(i); update_instance(i);
} }
@ -273,22 +344,26 @@ AP_GPS::update(void)
if (state[i].status != NO_GPS) { if (state[i].status != NO_GPS) {
num_instances = i+1; num_instances = i+1;
} }
if (i == primary_instance) { if (_auto_switch) {
continue; if (i == primary_instance) {
} continue;
if (state[i].status > state[primary_instance].status) { }
// we have a higher status lock, change GPS if (state[i].status > state[primary_instance].status) {
primary_instance = i; // we have a higher status lock, change GPS
continue; primary_instance = i;
} continue;
if (state[i].status == state[primary_instance].status && }
state[i].num_sats >= state[primary_instance].num_sats + 2) { if (state[i].status == state[primary_instance].status &&
// this GPS has at least 2 more satellites than the state[i].num_sats >= state[primary_instance].num_sats + 2) {
// current primary, switch primary. Once we switch we will // this GPS has at least 2 more satellites than the
// then tend to stick to the new GPS as primary. We don't // current primary, switch primary. Once we switch we will
// want to switch too often as it will look like a // then tend to stick to the new GPS as primary. We don't
// position shift to the controllers. // want to switch too often as it will look like a
primary_instance = i; // position shift to the controllers.
primary_instance = i;
}
} else {
primary_instance = 0;
} }
} }
#else #else
@ -343,3 +418,73 @@ AP_GPS::lock_port(uint8_t instance, bool lock)
locked_ports &= ~(1U<<instance); locked_ports &= ~(1U<<instance);
} }
} }
void
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms;
if (last_send_time_ms == 0 || last_send_time_ms != last_message_time_ms(0)) {
last_send_time_ms = last_message_time_ms(0);
const Location &loc = location(0);
mavlink_msg_gps_raw_int_send(
chan,
last_fix_time_ms(0)*(uint64_t)1000,
status(0),
loc.lat, // in 1E7 degrees
loc.lng, // in 1E7 degrees
loc.alt * 10UL, // in mm
get_hdop(0),
65535,
ground_speed(0)*100, // cm/s
ground_course_cd(0), // 1/100 degrees,
num_sats(0));
}
}
#if GPS_MAX_INSTANCES > 1
void
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
{
static uint32_t last_send_time_ms2;
if (num_sensors() > 1 &&
status(1) > AP_GPS::NO_GPS &&
(last_send_time_ms2 == 0 || last_send_time_ms2 != last_message_time_ms(1))) {
const Location &loc = location(1);
last_send_time_ms2 = last_message_time_ms(1);
mavlink_msg_gps2_raw_send(
chan,
last_fix_time_ms(1)*(uint64_t)1000,
status(1),
loc.lat,
loc.lng,
loc.alt * 10UL,
get_hdop(1),
65535,
ground_speed(1)*100, // cm/s
ground_course_cd(1), // 1/100 degrees,
num_sats(1),
0,
0);
}
}
#endif
#if GPS_RTK_AVAILABLE
void
AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
if (drivers[0] != NULL && drivers[0]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) {
drivers[0]->send_mavlink_gps_rtk(chan);
}
}
#if GPS_MAX_INSTANCES > 1
void
AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan)
{
if (drivers[1] != NULL && drivers[1]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) {
drivers[1]->send_mavlink_gps_rtk(chan);
}
}
#endif
#endif

43
libraries/AP_GPS/AP_GPS.h
View File

@ -35,6 +35,12 @@
#define GPS_MAX_INSTANCES 1 #define GPS_MAX_INSTANCES 1
#endif #endif
#if HAL_CPU_CLASS > HAL_CPU_CLASS_75
#define GPS_RTK_AVAILABLE 1
#else
#define GPS_RTK_AVAILABLE 0
#endif
class DataFlash_Class; class DataFlash_Class;
class AP_GPS_Backend; class AP_GPS_Backend;
@ -56,6 +62,14 @@ public:
/// more) to process incoming data. /// more) to process incoming data.
void update(void); void update(void);
//True if any of the underlying GPS Drivers are ready to enter
//a dgps-based fix beyond 3D lock, such as RTK mode.
bool can_calculate_base_pos(void);
//Allows the underlying GPS Drivers to enter a differential lock
//Might cause a position jump, thus only do this on the ground.
void calculate_base_pos(void);
// GPS driver types // GPS driver types
enum GPS_Type { enum GPS_Type {
GPS_TYPE_NONE = 0, GPS_TYPE_NONE = 0,
@ -122,6 +136,10 @@ public:
return num_instances; return num_instances;
} }
uint8_t primary_sensor(void) const {
return primary_instance;
}
// using these macros saves some code space on APM2 // using these macros saves some code space on APM2
#if GPS_MAX_INSTANCES == 1 #if GPS_MAX_INSTANCES == 1
# define _GPS_STATE(instance) state[0] # define _GPS_STATE(instance) state[0]
@ -139,6 +157,10 @@ public:
return status(primary_instance); return status(primary_instance);
} }
// Query the highest status this GPS supports
GPS_Status highest_supported_status(uint8_t instance) const;
GPS_Status highest_supported_status(void) const;
// location of last fix // location of last fix
const Location &location(uint8_t instance) const { const Location &location(uint8_t instance) const {
return _GPS_STATE(instance).location; return _GPS_STATE(instance).location;
@ -256,7 +278,11 @@ public:
// configuration parameters // configuration parameters
AP_Int8 _type[GPS_MAX_INSTANCES]; AP_Int8 _type[GPS_MAX_INSTANCES];
AP_Int8 _navfilter; AP_Int8 _navfilter;
#if GPS_MAX_INSTANCES > 1
AP_Int8 _auto_switch;
AP_Int8 _min_dgps;
#endif
// handle sending of initialisation strings to the GPS // handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size); void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
void send_blob_update(uint8_t instance); void send_blob_update(uint8_t instance);
@ -264,6 +290,19 @@ public:
// lock out a GPS port, allowing another application to use the port // lock out a GPS port, allowing another application to use the port
void lock_port(uint8_t instance, bool locked); void lock_port(uint8_t instance, bool locked);
//MAVLink Status Sending
void send_mavlink_gps_raw(mavlink_channel_t chan);
#if GPS_MAX_INSTANCES > 1
void send_mavlink_gps2_raw(mavlink_channel_t chan);
#endif
#if GPS_RTK_AVAILABLE
void send_mavlink_gps_rtk(mavlink_channel_t chan);
#if GPS_MAX_INSTANCES > 1
void send_mavlink_gps2_rtk(mavlink_channel_t chan);
#endif
#endif
private: private:
struct GPS_timing { struct GPS_timing {
// the time we got our last fix in system milliseconds // the time we got our last fix in system milliseconds
@ -295,7 +334,7 @@ private:
struct MTK19_detect_state mtk19_detect_state; struct MTK19_detect_state mtk19_detect_state;
struct SIRF_detect_state sirf_detect_state; struct SIRF_detect_state sirf_detect_state;
struct NMEA_detect_state nmea_detect_state; struct NMEA_detect_state nmea_detect_state;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #if GPS_RTK_AVAILABLE
struct SBP_detect_state sbp_detect_state; struct SBP_detect_state sbp_detect_state;
#endif #endif
} detect_state[GPS_MAX_INSTANCES]; } detect_state[GPS_MAX_INSTANCES];

715
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -18,17 +18,25 @@
// Swift Navigation GPS driver for ArduPilot // Swift Navigation GPS driver for ArduPilot
// Origin code by Niels Joubert njoubert.com // Origin code by Niels Joubert njoubert.com
// //
#include <AP_GPS.h> #include <AP_GPS.h>
#include "AP_GPS_SBP.h" #include "AP_GPS_SBP.h"
#include <DataFlash.h> #include <DataFlash.h>
#if GPS_RTK_AVAILABLE
#define SBP_DEBUGGING 0 #define SBP_DEBUGGING 0
#define SBP_FAKE_3DLOCK 0 #define SBP_FAKE_3DLOCK 0
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define SBP_MILLIS_BETWEEN_HEALTHCHECKS 1500 #define SBP_MILLIS_BETWEEN_HEALTHCHECKS 2000U
#define SBP_BASELINE_TIMEOUT_MS 1000U
#define SBP_FIX_TIMEOUT_MS 1000U
#define SBP_HEARTBEAT_TIMEOUT_MS 5000U
#define SBP_MILLIS_BETWEEN_TRACKING_LOG 1800U
#define SBP_DEBUGGING 0 #define SBP_DEBUGGING 0
#if SBP_DEBUGGING #if SBP_DEBUGGING
@ -41,7 +49,7 @@ extern const AP_HAL::HAL& hal;
only do detailed hardware logging on boards likely to have more log only do detailed hardware logging on boards likely to have more log
storage space storage space
*/ */
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #if GPS_RTK_AVAILABLE
#define SBP_HW_LOGGING 1 #define SBP_HW_LOGGING 1
#else #else
#define SBP_HW_LOGGING 0 #define SBP_HW_LOGGING 0
@ -51,91 +59,311 @@ bool AP_GPS_SBP::logging_started = false;
AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port), AP_GPS_Backend(_gps, _state, _port),
has_updated_pos(false),
has_updated_vel(false), last_baseline_received_ms(0),
last_heatbeat_received_ms(0),
last_tracking_state_ms(0),
iar_num_hypotheses(-1),
dgps_corrections_incoming(false),
rtk_corrections_incoming(false),
has_new_pos_llh(false),
has_new_vel_ned(false),
has_new_baseline_ecef(false),
has_rtk_base_pos(false),
pos_msg_counter(0), pos_msg_counter(0),
vel_msg_counter(0), vel_msg_counter(0),
dops_msg_counter(0),
baseline_msg_counter(0), baseline_msg_counter(0),
full_update_counter(0),
crc_error_counter(0), crc_error_counter(0),
last_healthcheck_millis(0) last_healthcheck_millis(0)
{ {
Debug("Initializing SBP Driver");
port->begin(115200, 256, 16);
port->flush();
parser_state.state = sbp_parser_state_t::WAITING; parser_state.state = sbp_parser_state_t::WAITING;
state.status = AP_GPS::NO_FIX; state.status = AP_GPS::NO_FIX;
state.have_vertical_velocity = true; state.have_vertical_velocity = true;
state.last_gps_time_ms = last_heatbeat_received_ms = last_healthcheck_millis = hal.scheduler->millis();
}
bool
AP_GPS_SBP::can_calculate_base_pos(void)
{
return (rtk_corrections_incoming && !has_rtk_base_pos);
};
void
AP_GPS_SBP::calculate_base_pos(void)
{
//INVARIANT:
// Only ever capture home with motors not armed!
// External driver checks whether can_raise_fix_level becomes true
// and only if it can, AND motors are not armed, will be capture home!
if (state.status < AP_GPS::GPS_OK_FIX_3D) {
Debug("Attempting to capture home without GPS Fix available. Can't do RTK without home lat-lon.");
return;
}
if (!rtk_corrections_incoming) {
Debug("Attempting to capture home baseline without rtk corrections being received.");
return;
}
Vector3d current_llh;
Vector3d current_ecef;
Vector3d current_baseline_ecef;
current_llh[0] = last_sbp_pos_llh_msg.lat * DEG_TO_RAD_DOUBLE;
current_llh[1] = last_sbp_pos_llh_msg.lon * DEG_TO_RAD_DOUBLE;
current_llh[2] = last_sbp_pos_llh_msg.height;
wgsllh2ecef(current_llh, current_ecef);
current_baseline_ecef[0] = ((double)last_sbp_baseline_ecef_msg.x) / 1000.0;
current_baseline_ecef[1] = ((double)last_sbp_baseline_ecef_msg.y) / 1000.0;
current_baseline_ecef[2] = ((double)last_sbp_baseline_ecef_msg.z) / 1000.0;
base_pos_ecef = current_ecef - current_baseline_ecef;
has_rtk_base_pos = true;
Debug("SBP Got Base Position! has_rtk_base_pos=%d, (%.2f, %.2f, %.2f)", has_rtk_base_pos,
base_pos_ecef[0],
base_pos_ecef[1],
base_pos_ecef[2]);
}
void
AP_GPS_SBP::invalidate_base_pos()
{
has_rtk_base_pos = false;
} }
bool bool
AP_GPS_SBP::read(void) AP_GPS_SBP::read(void)
{ {
//Invariant: Calling this function processes *all* data current in the UART buffer.
//
//IMPORTANT NOTICE: This function is NOT CALLED for several seconds
// during arming. That should not cause the driver to die. Process *all* waiting messages
//First we process all data waiting for the queue. bool full_update = false;
sbp_process(); do {
//Attempt to process one message at a time
bool new_message = sbp_process();
//Attempt to update our internal state with this new message.
if (update_state(new_message)) {
full_update = true;
full_update_counter += 1;
}
} while (port->available() > 0);
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
uint32_t elapsed = now - last_healthcheck_millis; uint32_t elapsed = now - last_healthcheck_millis;
if (elapsed > SBP_MILLIS_BETWEEN_HEALTHCHECKS) { if (elapsed > SBP_MILLIS_BETWEEN_HEALTHCHECKS) {
last_healthcheck_millis = now; last_healthcheck_millis = now;
#if SBP_DEBUGGING || SBP_HW_LOGGING float pos_msg_hz = pos_msg_counter / (float) elapsed * 1000;
float pos_msg_hz = pos_msg_counter / (float) elapsed * 1000.0; float vel_msg_hz = vel_msg_counter / (float) elapsed * 1000;
float vel_msg_hz = vel_msg_counter / (float) elapsed * 1000.0; float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000;
float dops_msg_hz = dops_msg_counter / (float) elapsed * 1000.0; float full_update_hz = full_update_counter / (float) elapsed * 1000;
float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000.0;
float crc_error_hz = crc_error_counter / (float) elapsed * 1000.0;
pos_msg_counter = 0; pos_msg_counter = 0;
vel_msg_counter = 0; vel_msg_counter = 0;
dops_msg_counter = 0;
baseline_msg_counter = 0; baseline_msg_counter = 0;
crc_error_counter = 0; full_update_counter = 0;
Debug("SBP GPS perf: CRC=(%.2fHz) Pos=(%.2fHz) Vel=(%.2fHz) Dops=(%.2fHz) Baseline=(%.2fHz)\n", Debug("SBP GPS perf: Fix=(%d) CRC=(%d) Pos=(%.2fHz) Vel=(%.2fHz) Baseline=(%.2fHz) Update=(%.2fHz) DGPS=(%d) RTK=(%d) RTK_HOME=(%d) IAR=(%d)",
crc_error_hz, state.status,
crc_error_counter,
pos_msg_hz, pos_msg_hz,
vel_msg_hz, vel_msg_hz,
dops_msg_hz, baseline_msg_hz,
baseline_msg_hz); full_update_hz,
dgps_corrections_incoming,
rtk_corrections_incoming,
has_rtk_base_pos,
iar_num_hypotheses);
#if SBP_HW_LOGGING #if SBP_HW_LOGGING
logging_log_health(pos_msg_hz, logging_log_health(pos_msg_hz,
vel_msg_hz, vel_msg_hz,
dops_msg_hz,
baseline_msg_hz, baseline_msg_hz,
crc_error_hz); full_update_hz,
crc_error_counter,
dgps_corrections_incoming,
rtk_corrections_incoming,
has_rtk_base_pos,
iar_num_hypotheses);
#endif #endif
#endif
} }
//Now we check whether we've done a full update - is all the sticky bits set? return full_update;
if (has_updated_pos && has_updated_vel) {
state.status = AP_GPS::GPS_OK_FIX_3D;
has_updated_pos = false;
has_updated_vel = false;
return true;
}
return false;
} }
//This attempts to read all the SBP messages from the incoming port. //This consolidates all the latest messages,
void //and the current mode the driver is in
AP_GPS_SBP::sbp_process() //
// INVARIANT:
// If in a fix mode >= 3,
// returns true only if a full position and velocity update happened.
// If in fix mode 0 or 1,
// returns true if messages are being received or we haven't timed out
bool
AP_GPS_SBP::update_state(bool has_new_message)
{ {
while (port->available() > 0) { uint32_t now = hal.scheduler->millis();
//Determine the current mode the GPS is in: DGPS or plain
//Notice that this is sticky.
if (has_new_baseline_ecef && (now - last_baseline_received_ms < SBP_BASELINE_TIMEOUT_MS)) {
dgps_corrections_incoming = true;
if (gps._min_dgps >= 100) {
//Allow only IntegerRTK baselines
rtk_corrections_incoming = dgps_corrections_incoming && (last_sbp_baseline_ecef_msg.flags & 0x1);
} else {
//Allow floatRTK baselines
rtk_corrections_incoming = dgps_corrections_incoming;
}
}
//Currently we only use relative positioning if we have RTK-level fixes,
//we ignore float-level fixes
bool using_relative_positioning = rtk_corrections_incoming && has_rtk_base_pos;
//Drop out of RTK mode if we haven't seen a baseline for a while...
if (using_relative_positioning && (now - last_baseline_received_ms > SBP_BASELINE_TIMEOUT_MS)) {
dgps_corrections_incoming = false;
rtk_corrections_incoming = false;
using_relative_positioning = false;
}
//UPDATE POSITION AND VELOCITY
if (!using_relative_positioning &&
(has_new_pos_llh && has_new_vel_ned) &&
(last_sbp_pos_llh_msg.tow == last_sbp_vel_ned_msg.tow)) {
state.last_gps_time_ms = hal.scheduler->millis();
state.time_week_ms = last_sbp_pos_llh_msg.tow;
state.location.lat = (int32_t) (last_sbp_pos_llh_msg.lat*1e7);
state.location.lng = (int32_t) (last_sbp_pos_llh_msg.lon*1e7);
state.location.alt = (int32_t) (last_sbp_pos_llh_msg.height*1e2);
state.num_sats = last_sbp_pos_llh_msg.n_sats;
update_state_velocity();
has_new_pos_llh = false;
state.status = AP_GPS::GPS_OK_FIX_3D;
return true;
} else if (using_relative_positioning &&
(has_new_baseline_ecef && has_new_vel_ned) &&
(last_sbp_baseline_ecef_msg.tow == last_sbp_vel_ned_msg.tow)) {
state.last_gps_time_ms = hal.scheduler->millis();
//Generate a new lat-lon from baseline
//Grab the current baseline
Vector3d current_baseline_ecef; //units are currently in mm
current_baseline_ecef[0] = ((double)last_sbp_baseline_ecef_msg.x) / 1000.0;
current_baseline_ecef[1] = ((double)last_sbp_baseline_ecef_msg.y) / 1000.0;
current_baseline_ecef[2] = ((double)last_sbp_baseline_ecef_msg.z) / 1000.0;
//Offset the reference point from that
Vector3d current_pos_ecef;
current_pos_ecef = base_pos_ecef + current_baseline_ecef;
Vector3d current_pos_llh;
wgsecef2llh(current_pos_ecef, current_pos_llh);
current_pos_llh[0] *= RAD_TO_DEG_DOUBLE;
current_pos_llh[1] *= RAD_TO_DEG_DOUBLE;
state.time_week_ms = last_sbp_baseline_ecef_msg.tow;
state.location.lat = (int32_t) (current_pos_llh[0] * 1e7);
state.location.lng = (int32_t) (current_pos_llh[1] * 1e7);
state.location.alt = (int32_t) (current_pos_llh[2] * 1e3);
state.num_sats = last_sbp_baseline_ecef_msg.n_sats;
update_state_velocity();
has_new_baseline_ecef = false;
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
return true;
}
//If we get here,
//We have not been able to update the GPS state yet for this process call.
//Check whether the GPS is still alive and processing messages!
if (!using_relative_positioning && (now - state.last_gps_time_ms > SBP_FIX_TIMEOUT_MS)) {
state.status = AP_GPS::NO_FIX;
return (now - last_heatbeat_received_ms < SBP_HEARTBEAT_TIMEOUT_MS);
}
if (now - last_heatbeat_received_ms > SBP_HEARTBEAT_TIMEOUT_MS) {
state.status = AP_GPS::NO_GPS;
return false;
}
if (state.status < AP_GPS::GPS_OK_FIX_3D) {
//If we are receiving messages, but dont have a fix yet, thats okay.
return has_new_message;
} else {
//If we have a fix and we got here, then we're in between message synchronizations
return false;
}
return true;
}
void
AP_GPS_SBP::update_state_velocity(void)
{
state.time_week_ms = last_sbp_vel_ned_msg.tow;
state.velocity[0] = (float)(last_sbp_vel_ned_msg.n / 1000.0);
state.velocity[1] = (float)(last_sbp_vel_ned_msg.e / 1000.0);
state.velocity[2] = (float)(last_sbp_vel_ned_msg.d / 1000.0);
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0]));
if (state.ground_course_cd < 0) {
state.ground_course_cd += 36000;
}
has_new_vel_ned = false;
}
//This attempts to read a SINGLE SBP messages from the incoming port.
//Returns true if a new message was read, false if we failed to read a message.
bool
AP_GPS_SBP::sbp_process()
{
while (port->available() > 0) {
uint8_t temp = port->read(); uint8_t temp = port->read();
uint16_t crc; uint16_t crc;
//This switch reads one character at a time, //This switch reads one character at a time,
//parsing it into buffers until a full message is dispatched //parsing it into buffers until a full message is dispatched
switch(parser_state.state) { switch(parser_state.state) {
@ -190,15 +418,8 @@ AP_GPS_SBP::sbp_process()
crc = crc16_ccitt(&(parser_state.msg_len), 1, crc); crc = crc16_ccitt(&(parser_state.msg_len), 1, crc);
crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc); crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc);
if (parser_state.crc == crc) { if (parser_state.crc == crc) {
//OK, we have a valid message. Dispatch the appropriate function: //OK, we have a valid message. Dispatch the appropriate function:
switch(parser_state.msg_type) { switch(parser_state.msg_type) {
case SBP_GPS_TIME_MSGTYPE:
sbp_process_gpstime(parser_state.msg_buff);
break;
case SBP_DOPS_MSGTYPE:
sbp_process_dops(parser_state.msg_buff);
break;
case SBP_POS_ECEF_MSGTYPE: case SBP_POS_ECEF_MSGTYPE:
sbp_process_pos_ecef(parser_state.msg_buff); sbp_process_pos_ecef(parser_state.msg_buff);
break; break;
@ -217,12 +438,29 @@ AP_GPS_SBP::sbp_process()
case SBP_VEL_NED_MSGTYPE: case SBP_VEL_NED_MSGTYPE:
sbp_process_vel_ned(parser_state.msg_buff); sbp_process_vel_ned(parser_state.msg_buff);
break; break;
default: case SBP_GPS_TIME_MSGTYPE:
Debug("Unknown message received: msg_type=0x%x", parser_state.msg_type); sbp_process_gpstime(parser_state.msg_buff);
break;
case SBP_DOPS_MSGTYPE:
sbp_process_dops(parser_state.msg_buff);
break;
case SBP_TRACKING_STATE_MSGTYPE:
sbp_process_tracking_state(parser_state.msg_buff, parser_state.msg_len);
break;
case SBP_IAR_STATE_MSGTYPE:
sbp_process_iar_state(parser_state.msg_buff);
break;
case SBP_HEARTBEAT_MSGTYPE:
sbp_process_heartbeat(parser_state.msg_buff);
break;
case SBP_STARTUP_MSGTYPE:
sbp_process_startup(parser_state.msg_buff);
break;
} }
return true;
} else { } else {
Debug("CRC Error Occurred!\n"); Debug("CRC Error Occurred!");
crc_error_counter += 1; crc_error_counter += 1;
} }
@ -235,7 +473,13 @@ AP_GPS_SBP::sbp_process()
} }
} }
//We have parsed all the waiting messages //We have parsed all the waiting messages
return; return false;
}
void
AP_GPS_SBP::sbp_process_heartbeat(uint8_t* msg)
{
last_heatbeat_received_ms = hal.scheduler->millis();
} }
void void
@ -244,7 +488,6 @@ AP_GPS_SBP::sbp_process_gpstime(uint8_t* msg)
struct sbp_gps_time_t* t = (struct sbp_gps_time_t*)msg; struct sbp_gps_time_t* t = (struct sbp_gps_time_t*)msg;
state.time_week = t->wn; state.time_week = t->wn;
state.time_week_ms = t->tow; state.time_week_ms = t->tow;
state.last_gps_time_ms = hal.scheduler->millis();
} }
void void
@ -252,76 +495,113 @@ AP_GPS_SBP::sbp_process_dops(uint8_t* msg)
{ {
struct sbp_dops_t* d = (struct sbp_dops_t*) msg; struct sbp_dops_t* d = (struct sbp_dops_t*) msg;
state.time_week_ms = d->tow; state.time_week_ms = d->tow;
state.last_gps_time_ms = hal.scheduler->millis();
state.hdop = d->hdop; state.hdop = d->hdop;
dops_msg_counter += 1;
} }
void void
AP_GPS_SBP::sbp_process_pos_ecef(uint8_t* msg) AP_GPS_SBP::sbp_process_pos_ecef(uint8_t* msg)
{ {
//Ideally we'd like this data in LLH format, not ECEF //Using LLH, not ECEF
} }
void void
AP_GPS_SBP::sbp_process_pos_llh(uint8_t* msg) AP_GPS_SBP::sbp_process_pos_llh(uint8_t* msg)
{ {
struct sbp_pos_llh_t* pos = (struct sbp_pos_llh_t*)msg; struct sbp_pos_llh_t* pos = (struct sbp_pos_llh_t*)msg;
state.time_week_ms = pos->tow; last_sbp_pos_llh_msg = *pos;
state.last_gps_time_ms = hal.scheduler->millis();
state.location.lat = (int32_t) (pos->lat*1e7); has_new_pos_llh = true;
state.location.lng = (int32_t) (pos->lon*1e7);
state.location.alt = (int32_t) (pos->height*1e2); #if SBP_DEBUGGING || SBP_HW_LOGGING
state.num_sats = pos->n_sats;
pos_msg_counter += 1; pos_msg_counter += 1;
has_updated_pos = true; #endif
#if SBP_HW_LOGGING
logging_log_llh(pos);
#endif
} }
void void
AP_GPS_SBP::sbp_process_baseline_ecef(uint8_t* msg) AP_GPS_SBP::sbp_process_baseline_ecef(uint8_t* msg)
{ {
struct sbp_baseline_ecef_t* b = (struct sbp_baseline_ecef_t*)msg; struct sbp_baseline_ecef_t* b = (struct sbp_baseline_ecef_t*)msg;
last_sbp_baseline_ecef_msg = *b;
last_baseline_received_ms = hal.scheduler->millis();
has_new_baseline_ecef = true;
#if SBP_DEBUGGING || SBP_HW_LOGGING
baseline_msg_counter += 1; baseline_msg_counter += 1;
#endif
#if SBP_HW_LOGGING #if SBP_HW_LOGGING
logging_log_baseline(b); logging_log_baseline_ecef(b);
#endif #endif
} }
void void
AP_GPS_SBP::sbp_process_baseline_ned(uint8_t* msg) AP_GPS_SBP::sbp_process_baseline_ned(uint8_t* msg)
{ {
//Ideally we'd like this data in ECEF format, not NED //Currently we use ECEF baselines.
//This is just for logging purposes.
struct sbp_baseline_ned_t* b = (struct sbp_baseline_ned_t*)msg;
last_sbp_baseline_ned_msg = *b;
} }
void void
AP_GPS_SBP::sbp_process_vel_ecef(uint8_t* msg) AP_GPS_SBP::sbp_process_vel_ecef(uint8_t* msg)
{ {
//Ideally we'd like this data in NED format, not ECEF //Currently we use NED velocity.
} }
void void
AP_GPS_SBP::sbp_process_vel_ned(uint8_t* msg) AP_GPS_SBP::sbp_process_vel_ned(uint8_t* msg)
{ {
struct sbp_vel_ned_t* vel = (struct sbp_vel_ned_t*)msg; struct sbp_vel_ned_t* vel = (struct sbp_vel_ned_t*)msg;
state.time_week_ms = vel->tow; last_sbp_vel_ned_msg = *vel;
state.last_gps_time_ms = hal.scheduler->millis();
state.velocity[0] = (float)vel->n / 1000.0;
state.velocity[1] = (float)vel->e / 1000.0;
state.velocity[2] = (float)vel->d / 1000.0;
state.num_sats = vel->n_sats;
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1]; has_new_vel_ned = true;
state.ground_speed = safe_sqrt(ground_vector_sq);
#if SBP_DEBUGGING || SBP_HW_LOGGING
vel_msg_counter += 1;
#endif
}
void
AP_GPS_SBP::sbp_process_tracking_state(uint8_t* msg, uint8_t len)
{
uint32_t now = hal.scheduler->millis();
struct sbp_tracking_state_t* tracking_state = (struct sbp_tracking_state_t*)msg;
last_sbp_tracking_state_msg = *tracking_state;
uint8_t num = len / sizeof(sbp_tracking_state_t);
last_sbp_tracking_state_msg_num = num;
//Rate-limit the tracking state messages to no more than 1.8 seconds
if (now - last_tracking_state_ms > SBP_MILLIS_BETWEEN_TRACKING_LOG) {
last_tracking_state_ms = now;
#ifdef SBP_HW_LOGGING
logging_log_tracking_state(tracking_state, num);
#endif
state.ground_course_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0]));
if (state.ground_course_cd < 0) {
state.ground_course_cd += 36000;
} }
vel_msg_counter += 1; }
has_updated_vel = true;
void
AP_GPS_SBP::sbp_process_iar_state(uint8_t* msg)
{
struct sbp_iar_state_t* iar_state = (struct sbp_iar_state_t*)msg;
iar_num_hypotheses = (int32_t) iar_state->num_hypotheses;
}
void
AP_GPS_SBP::sbp_process_startup(uint8_t* msg)
{
invalidate_base_pos();
} }
bool bool
@ -390,42 +670,160 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
return false; return false;
} }
void
AP_GPS_SBP::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
uint8_t health = dgps_corrections_incoming |
(rtk_corrections_incoming << 1) |
(has_rtk_base_pos << 2);
mavlink_msg_gps_rtk_send(
chan,
last_baseline_received_ms, // Time since boot of last baseline message received in ms.
AP_GPS::GPS_TYPE_SBP, // Identification of connected RTK receiver.
state.time_week, // GPS Week Number of last baseline
last_sbp_baseline_ned_msg.tow, // GPS Time of Week of last baseline
health, // GPS-specific health report for RTK data.
baseline_recv_rate, // Rate of baseline messages being received by GPS, in HZ*10
last_sbp_baseline_ned_msg.n_sats, // Current number of sats used for RTK calculation.
1, // Coordinate system of baseline. 0 == ECEF, 1 == NED
last_sbp_baseline_ned_msg.n, // Current baseline in ECEF x or NED north component in mm
last_sbp_baseline_ned_msg.e, // Current baseline in ECEF y or NED east component in mm
last_sbp_baseline_ned_msg.d, // Current baseline in ECEF z or NED down component in mm
last_sbp_baseline_ned_msg.h_accuracy, // Current estimate of baseline accuracy.
iar_num_hypotheses // Current number of integer ambiguity hypotheses.
);
}
#if GPS_MAX_INSTANCES > 1
void
AP_GPS_SBP::send_mavlink_gps2_rtk(mavlink_channel_t chan)
{
uint8_t health = dgps_corrections_incoming |
(rtk_corrections_incoming << 1) |
(has_rtk_base_pos << 2);
mavlink_msg_gps2_rtk_send(
chan,
last_baseline_received_ms, // Time since boot of last baseline message received in ms.
AP_GPS::GPS_TYPE_SBP, // Identification of connected RTK receiver.
state.time_week, // GPS Week Number of last baseline
last_sbp_baseline_ned_msg.tow, // GPS Time of Week of last baseline
health, // GPS-specific health report for RTK data.
baseline_recv_rate, // Rate of baseline messages being received by GPS, in HZ*10
last_sbp_baseline_ned_msg.n_sats, // Current number of sats used for RTK calculation.
1, // Coordinate system of baseline. 0 == ECEF, 1 == NED
last_sbp_baseline_ned_msg.n, // Current baseline in ECEF x or NED north component in mm
last_sbp_baseline_ned_msg.e, // Current baseline in ECEF y or NED east component in mm
last_sbp_baseline_ned_msg.d, // Current baseline in ECEF z or NED down component in mm
last_sbp_baseline_ned_msg.h_accuracy, // Current estimate of baseline accuracy.
iar_num_hypotheses // Current number of integer ambiguity hypotheses.
);
}
#endif
#if SBP_HW_LOGGING #if SBP_HW_LOGGING
#define LOG_MSG_SBPHEALTH 202 #define LOG_MSG_SBPHEALTH 202
#define LOG_MSG_SBPBASELINE 203 #define LOG_MSG_SBPLLH 203
#define LOG_MSG_SBPBASELINE 204
#define LOG_MSG_SBPTRACKING1 205
#define LOG_MSG_SBPTRACKING2 206
struct PACKED log_SbpHealth { struct PACKED log_SbpHealth {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t timestamp; uint32_t timestamp;
float pos_msg_hz; float pos_msg_hz;
float vel_msg_hz; float vel_msg_hz;
float dops_msg_hz;
float baseline_msg_hz; float baseline_msg_hz;
float crc_error_hz; float full_update_hz;
uint32_t crc_error_counter;
uint8_t dgps_corrections_incoming;
uint8_t rtk_corrections_incoming;
uint8_t has_rtk_base_pos;
int32_t iar_num_hypotheses;
}; };
struct PACKED log_SbpLLH {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint32_t tow;
int32_t lat;
int32_t lon;
int32_t alt;
uint8_t n_sats;
};
struct PACKED log_SbpBaseline { struct PACKED log_SbpBaseline {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint32_t timestamp; uint32_t timestamp;
uint32_t tow; uint32_t tow; //< GPS Time of Week of ECEF Baseline (unit: ms)
int32_t baseline_x; int32_t x; //< Baseline ECEF X coordinate
int32_t baseline_y; int32_t y; //< Baseline ECEF Y coordinate
int32_t baseline_z; int32_t z; //< Baseline ECEF Z coordinate
uint16_t baseline_accuracy; int32_t length; //< Baseline length
uint8_t num_sats; uint16_t accuracy; //< Horizontal position accuracy estimate (unit: mm)
uint8_t flags; uint8_t n_sats; //< Number of satellites used in solution
uint8_t flags; //< Status flags (reserved)
};
struct PACKED log_SbpTracking1 {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint8_t ch1_prn;
float ch1_cn0;
uint8_t ch2_prn;
float ch2_cn0;
uint8_t ch3_prn;
float ch3_cn0;
uint8_t ch4_prn;
float ch4_cn0;
uint8_t ch5_prn;
float ch5_cn0;
uint8_t ch6_prn;
float ch6_cn0;
uint8_t ch7_prn;
float ch7_cn0;
};
struct PACKED log_SbpTracking2 {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint8_t ch8_prn;
float ch8_cn0;
uint8_t ch9_prn;
float ch9_cn0;
uint8_t ch10_prn;
float ch10_cn0;
uint8_t ch11_prn;
float ch11_cn0;
uint8_t ch12_prn;
float ch12_cn0;
uint8_t ch13_prn;
float ch13_cn0;
uint8_t ch14_prn;
float ch14_cn0;
}; };
static const struct LogStructure sbp_log_structures[] PROGMEM = { static const struct LogStructure sbp_log_structures[] PROGMEM = {
{ LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth), { LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth),
"SBPH", "Ifffff", "TimeMS,PosHz,VelHz,DopsHz,BaseHz,CrcHz" }, "SBPH", "IffffIBBBi", "TimeMS,PHz,VHz,BHz,UpHz,CrcError,dgpsOn,rtkOn,hasRtkBase,IAR" },
{ LOG_MSG_SBPLLH, sizeof(log_SbpLLH),
"SBPL", "IIiiiB", "TimeMS,tow,lat,lon,alt,num_sats" },
{ LOG_MSG_SBPBASELINE, sizeof(log_SbpBaseline), { LOG_MSG_SBPBASELINE, sizeof(log_SbpBaseline),
"SBPB", "IIiiiHBB", "TimeMS,tow,bx,by,bz,bacc,num_sats,flags" } "SBPB", "IIiiiiHBB", "TimeMS,tow,x,y,z,len,acc,num_sats,flags" },
{ LOG_MSG_SBPTRACKING1, sizeof(log_SbpTracking1),
"SBT1", "IBfBfBfBfBfBfBf", "TimeMS,s1,c1,s2,c2,s3,c3,s4,c4,s5,c5,s6,c6,s7,c7" },
{ LOG_MSG_SBPTRACKING2, sizeof(log_SbpTracking2),
"SBT2", "IBfBfBfBfBfBfBF", "TimeMS,s8,c8,s9,c9,s10,c10,s11,c11,s12,c12,s13,c13,s14,c14" }
}; };
void AP_GPS_SBP::logging_write_headers(void) void
AP_GPS_SBP::logging_write_headers(void)
{ {
if (!logging_started) { if (!logging_started) {
logging_started = true; logging_started = true;
@ -433,7 +831,10 @@ void AP_GPS_SBP::logging_write_headers(void)
} }
} }
void AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float dops_msg_hz, float baseline_msg_hz, float crc_error_hz) void
AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz,
uint32_t crc_error_counter, bool dgps_corrections_incoming, bool rtk_corrections_incoming,
bool has_rtk_base_pos, int32_t iar_num_hypotheses)
{ {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
@ -444,18 +845,23 @@ void AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float do
struct log_SbpHealth pkt = { struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
timestamp : hal.scheduler->millis(), timestamp : hal.scheduler->millis(),
pos_msg_hz : pos_msg_hz, pos_msg_hz : pos_msg_hz,
vel_msg_hz : vel_msg_hz, vel_msg_hz : vel_msg_hz,
dops_msg_hz : dops_msg_hz, baseline_msg_hz : baseline_msg_hz,
baseline_msg_hz : baseline_msg_hz, full_update_hz : full_update_hz,
crc_error_hz : crc_error_hz crc_error_counter : crc_error_counter,
dgps_corrections_incoming : dgps_corrections_incoming,
rtk_corrections_incoming : rtk_corrections_incoming,
has_rtk_base_pos : has_rtk_base_pos,
iar_num_hypotheses : iar_num_hypotheses
}; };
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
} };
void AP_GPS_SBP::logging_log_baseline(struct sbp_baseline_ecef_t* b) void
AP_GPS_SBP::logging_log_llh(struct sbp_pos_llh_t* p)
{ {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
@ -464,19 +870,108 @@ void AP_GPS_SBP::logging_log_baseline(struct sbp_baseline_ecef_t* b)
logging_write_headers(); logging_write_headers();
struct log_SbpBaseline pkt = { struct log_SbpLLH pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPBASELINE), LOG_PACKET_HEADER_INIT(LOG_MSG_SBPLLH),
timestamp : hal.scheduler->millis(), timestamp : hal.scheduler->millis(),
tow : b->tow, tow : p->tow,
baseline_x : b->x, lat : (int32_t) (p->lat*1e7),
baseline_y : b->y, lon : (int32_t) (p->lon*1e7),
baseline_z : b->z, alt : (int32_t) (p->height*1e2),
baseline_accuracy : b->accuracy, n_sats : p->n_sats,
num_sats : b->n_sats,
flags : b->flags
}; };
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
} };
void
AP_GPS_SBP::logging_log_baseline_ecef(struct sbp_baseline_ecef_t* b)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
float x = b->x / 1000.0;
float y = b->y / 1000.0;
float z = b->z / 1000.0;
int32_t len = (int32_t) safe_sqrt(x*x+y*y+z*z) * 1000.0;
struct log_SbpBaseline pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPBASELINE),
timestamp : hal.scheduler->millis(),
tow : b->tow,
x : b->x,
y : b->y,
z : b->z,
length : len,
accuracy : b->accuracy,
n_sats : b->n_sats,
flags : b->flags
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
};
void
AP_GPS_SBP::logging_log_tracking_state(struct sbp_tracking_state_t* state, uint8_t num)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpTracking1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPTRACKING1),
timestamp : hal.scheduler->millis(),
ch1_prn : state[0].prn,
ch1_cn0 : state[0].cn0,
ch2_prn : num < 1 ? 0 : state[1].prn,
ch2_cn0 : num < 1 ? 0 : state[1].cn0,
ch3_prn : num < 2 ? 0 : state[2].prn,
ch3_cn0 : num < 2 ? 0 : state[2].cn0,
ch4_prn : num < 3 ? 0 : state[3].prn,
ch4_cn0 : num < 3 ? 0 : state[3].cn0,
ch5_prn : num < 4 ? 0 : state[4].prn,
ch5_cn0 : num < 4 ? 0 : state[4].cn0,
ch6_prn : num < 5 ? 0 : state[5].prn,
ch6_cn0 : num < 5 ? 0 : state[5].cn0,
ch7_prn : num < 6 ? 0 : state[6].prn,
ch7_cn0 : num < 6 ? 0 : state[6].cn0,
};
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
if (num > 6) {
struct log_SbpTracking2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPTRACKING2),
timestamp : hal.scheduler->millis(),
ch8_prn : num < 7 ? 0 : state[7].prn,
ch8_cn0 : num < 7 ? 0 : state[7].cn0,
ch9_prn : num < 8 ? 0 : state[8].prn,
ch9_cn0 : num < 8 ? 0 : state[8].cn0,
ch10_prn : num < 9 ? 0 : state[9].prn,
ch10_cn0 : num < 9 ? 0 : state[9].cn0,
ch11_prn : num < 10 ? 0 : state[10].prn,
ch11_cn0 : num < 10 ? 0 : state[10].cn0,
ch12_prn : num < 11 ? 0 : state[11].prn,
ch12_cn0 : num < 11 ? 0 : state[11].cn0,
ch13_prn : num < 12 ? 0 : state[12].prn,
ch13_cn0 : num < 12 ? 0 : state[12].cn0,
ch14_prn : num < 13 ? 0 : state[13].prn,
ch14_cn0 : num < 13 ? 0 : state[13].cn0,
};
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt));
};
};
#endif // SBP_HW_LOGGING #endif // SBP_HW_LOGGING
#endif // GPS_RTK_AVAILABLE

107
libraries/AP_GPS/AP_GPS_SBP.h
View File

@ -23,6 +23,8 @@
#ifndef __AP_GPS_SBP_H__ #ifndef __AP_GPS_SBP_H__
#define __AP_GPS_SBP_H__ #define __AP_GPS_SBP_H__
#if GPS_RTK_AVAILABLE
#include <AP_GPS.h> #include <AP_GPS.h>
@ -48,11 +50,24 @@ class AP_GPS_SBP : public AP_GPS_Backend
public: public:
AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// Methods bool can_calculate_base_pos(void);
void calculate_base_pos(void);
void invalidate_base_pos(void);
AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; }
bool read(); bool read();
static bool _detect(struct SBP_detect_state &state, uint8_t data); static bool _detect(struct SBP_detect_state &state, uint8_t data);
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
#if GPS_MAX_INSTANCES > 1
virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan);
#endif
private: private:
// ************************************************************************ // ************************************************************************
@ -79,15 +94,19 @@ private:
static const uint8_t SBP_PREAMBLE = 0x55; static const uint8_t SBP_PREAMBLE = 0x55;
//Message types supported by this driver //Message types supported by this driver
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100; static const uint16_t SBP_STARTUP_MSGTYPE = 0xFF00;
static const uint16_t SBP_DOPS_MSGTYPE = 0x0206; static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF;
static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0200; static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201; static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202; static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0200;
static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x0203; static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x0204; static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205; static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x0203;
static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x0204;
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205;
static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0016;
static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019;
// GPS Time // GPS Time
struct PACKED sbp_gps_time_t { struct PACKED sbp_gps_time_t {
uint16_t wn; //< GPS week number (unit: weeks) uint16_t wn; //< GPS week number (unit: weeks)
@ -175,17 +194,36 @@ private:
uint8_t flags; //< Status flags (reserved) uint8_t flags; //< Status flags (reserved)
}; };
// Activity and Signal-to-Noise data of a tracking channel on the GPS.
struct PACKED sbp_tracking_state_t {
uint8_t state; //< 0 if disabled, 1 if running
uint8_t prn; //< PRN identifier of tracked satellite
float cn0; //< carrier to noise power ratio.
};
// Integer Ambiguity Resolution state - how is the RTK resolution doing?
struct PACKED sbp_iar_state_t {
uint32_t num_hypotheses;
};
// ************************************************************************ // ************************************************************************
// Swift Navigation SBP protocol parsing and processing // Swift Navigation SBP protocol parsing and processing
// ************************************************************************ // ************************************************************************
//Pulls data from the port, dispatches messages to processing functions //Pulls data from the port, dispatches messages to processing functions
void sbp_process(); //Returns true if a new message was successfully decoded.
bool sbp_process();
bool update_state(bool has_new_message);
void update_state_velocity(void);
//Processes individual messages //Processes individual messages
//When a message is received, it sets a sticky bit that it has updated //When a message is received, it sets a sticky bit that it has updated
//itself. This is used to track when a full update of GPS_State has occurred //itself. This is used to track when a full update of GPS_State has occurred
void sbp_process_heartbeat(uint8_t* msg);
void sbp_process_gpstime(uint8_t* msg); void sbp_process_gpstime(uint8_t* msg);
void sbp_process_dops(uint8_t* msg); void sbp_process_dops(uint8_t* msg);
void sbp_process_pos_ecef(uint8_t* msg); void sbp_process_pos_ecef(uint8_t* msg);
@ -194,21 +232,46 @@ private:
void sbp_process_baseline_ned(uint8_t* msg); void sbp_process_baseline_ned(uint8_t* msg);
void sbp_process_vel_ecef(uint8_t* msg); void sbp_process_vel_ecef(uint8_t* msg);
void sbp_process_vel_ned(uint8_t* msg); void sbp_process_vel_ned(uint8_t* msg);
void sbp_process_tracking_state(uint8_t* msg, uint8_t len);
void sbp_process_iar_state(uint8_t* msg);
void sbp_process_startup(uint8_t* msg);
//Internal last-received-messages
sbp_pos_llh_t last_sbp_pos_llh_msg;
sbp_vel_ned_t last_sbp_vel_ned_msg;
sbp_baseline_ecef_t last_sbp_baseline_ecef_msg;
sbp_baseline_ned_t last_sbp_baseline_ned_msg;
sbp_tracking_state_t last_sbp_tracking_state_msg;
uint8_t last_sbp_tracking_state_msg_num;
//Tracking GPS health and received time-of-week
uint32_t last_baseline_received_ms;
uint32_t last_heatbeat_received_ms;
uint32_t last_tracking_state_ms;
int32_t iar_num_hypotheses;
//Sticky bits to track updating of state //Sticky bits to track updating of state
bool has_updated_pos:1; bool dgps_corrections_incoming:1;
bool has_updated_vel:1; bool rtk_corrections_incoming:1;
bool has_new_pos_llh:1;
bool has_new_vel_ned:1;
bool has_new_baseline_ecef:1;
//RTK-specific relative-to-absolute positioning
bool has_rtk_base_pos:1;
Vector3d base_pos_ecef;
// ************************************************************************ // ************************************************************************
// Monitoring and Performance Counting // Monitoring and Performance Counting
// ************************************************************************ // ************************************************************************
uint8_t pos_msg_counter; uint8_t pos_msg_counter;
uint8_t vel_msg_counter; uint8_t vel_msg_counter;
uint8_t dops_msg_counter;
uint8_t baseline_msg_counter; uint8_t baseline_msg_counter;
uint16_t crc_error_counter; uint8_t full_update_counter;
uint32_t crc_error_counter;
uint32_t last_healthcheck_millis; uint32_t last_healthcheck_millis;
// ************************************************************************ // ************************************************************************
@ -219,8 +282,16 @@ private:
static bool logging_started; static bool logging_started;
void logging_write_headers(); void logging_write_headers();
void logging_log_health(float pos_msg_hz, float vel_msg_hz, float dops_msg_hz, float baseline_msg_hz, float crc_error_hz);
void logging_log_baseline(struct sbp_baseline_ecef_t*); void logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz,
uint32_t crc_error_counter, bool dgps_corrections_incoming, bool rtk_corrections_incoming,
bool has_rtk_base_pos, int32_t iar_num_hypotheses);
void logging_log_llh(struct sbp_pos_llh_t* p);
void logging_log_baseline_ecef(struct sbp_baseline_ecef_t*);
void logging_log_tracking_state(struct sbp_tracking_state_t*, uint8_t num);
}; };
#endif // __AP_GPS_SBP_H__ #endif // GPS_RTK_AVAILABLE
#endif // __AP_GPS_SBP_H__

26
libraries/AP_GPS/GPS_Backend.h
View File

@ -20,6 +20,9 @@
#ifndef __AP_GPS_BACKEND_H__ #ifndef __AP_GPS_BACKEND_H__
#define __AP_GPS_BACKEND_H__ #define __AP_GPS_BACKEND_H__
#include <GCS_MAVLink.h>
#include <AP_GPS.h>
class AP_GPS_Backend class AP_GPS_Backend
{ {
public: public:
@ -34,6 +37,29 @@ public:
// valid packet from the GPS. // valid packet from the GPS.
virtual bool read() = 0; virtual bool read() = 0;
#if GPS_RTK_AVAILABLE
// true once an RTK GPS Driver has a converged baseline vector and
// absolute single point solution to enter into an RTK Fix.
virtual bool can_calculate_base_pos(void) { return false; };
// tells a RTK GPS Driver to capture the current single-point solution
// and baseline solution as the current home data.
virtual void calculate_base_pos(void) {};
// Highest status supported by this GPS.
// Allows external system to identify type of receiver connected.
virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }
//MAVLink methods
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan) { return ; }
#if GPS_MAX_INSTANCES > 1
virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan) { return ; }
#endif
#endif
protected: protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters) AP_GPS &gps; ///< access to frontend (for parameters)