// -*- 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 .
*/
//
// Swift Navigation GPS driver for ArduPilot
// Origin code by Niels Joubert njoubert.com
//
#include
#include "AP_GPS_SBP.h"
#include
#if GPS_RTK_AVAILABLE
#define SBP_DEBUGGING 0
#define SBP_FAKE_3DLOCK 0
extern const AP_HAL::HAL& hal;
#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
#if SBP_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
# define Debug(fmt, args ...)
#endif
/*
only do detailed hardware logging on boards likely to have more log
storage space
*/
#if GPS_RTK_AVAILABLE
#define SBP_HW_LOGGING 1
#else
#define SBP_HW_LOGGING 0
#endif
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_Backend(_gps, _state, _port),
last_baseline_received_ms(0),
last_heatbeat_received_ms(0),
last_tracking_state_ms(0),
iar_num_hypotheses(-1),
baseline_recv_rate(0),
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),
vel_msg_counter(0),
baseline_msg_counter(0),
full_update_counter(0),
crc_error_counter(0),
last_healthcheck_millis(0)
{
parser_state.state = sbp_parser_state_t::WAITING;
state.status = AP_GPS::NO_FIX;
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
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
bool full_update = false;
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 elapsed = now - last_healthcheck_millis;
if (elapsed > SBP_MILLIS_BETWEEN_HEALTHCHECKS) {
last_healthcheck_millis = now;
float pos_msg_hz = pos_msg_counter / (float) elapsed * 1000;
float vel_msg_hz = vel_msg_counter / (float) elapsed * 1000;
float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000;
float full_update_hz = full_update_counter / (float) elapsed * 1000;
baseline_recv_rate = uint8_t (baseline_msg_hz * 10);
pos_msg_counter = 0;
vel_msg_counter = 0;
baseline_msg_counter = 0;
full_update_counter = 0;
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)",
state.status,
crc_error_counter,
pos_msg_hz,
vel_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
logging_log_health(pos_msg_hz,
vel_msg_hz,
baseline_msg_hz,
full_update_hz);
#endif
}
return full_update;
}
//This consolidates all the latest messages,
//and the current mode the driver is in
//
// 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)
{
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();
uint16_t crc;
//This switch reads one character at a time,
//parsing it into buffers until a full message is dispatched
switch(parser_state.state) {
case sbp_parser_state_t::WAITING:
if (temp == SBP_PREAMBLE) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_TYPE;
}
break;
case sbp_parser_state_t::GET_TYPE:
*((uint8_t*)&(parser_state.msg_type) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_SENDER;
}
break;
case sbp_parser_state_t::GET_SENDER:
*((uint8_t*)&(parser_state.sender_id) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_LEN;
}
break;
case sbp_parser_state_t::GET_LEN:
parser_state.msg_len = temp;
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_MSG;
break;
case sbp_parser_state_t::GET_MSG:
*((uint8_t*)&(parser_state.msg_buff) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= parser_state.msg_len) {
parser_state.n_read = 0;
parser_state.state = sbp_parser_state_t::GET_CRC;
}
break;
case sbp_parser_state_t::GET_CRC:
*((uint8_t*)&(parser_state.crc) + parser_state.n_read) = temp;
parser_state.n_read += 1;
if (parser_state.n_read >= 2) {
parser_state.state = sbp_parser_state_t::WAITING;
crc = crc16_ccitt((uint8_t*)&(parser_state.msg_type), 2, 0);
crc = crc16_ccitt((uint8_t*)&(parser_state.sender_id), 2, crc);
crc = crc16_ccitt(&(parser_state.msg_len), 1, crc);
crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc);
if (parser_state.crc == crc) {
//OK, we have a valid message. Dispatch the appropriate function:
switch(parser_state.msg_type) {
case SBP_POS_ECEF_MSGTYPE:
sbp_process_pos_ecef(parser_state.msg_buff);
break;
case SBP_POS_LLH_MSGTYPE:
sbp_process_pos_llh(parser_state.msg_buff);
break;
case SBP_BASELINE_ECEF_MSGTYPE:
sbp_process_baseline_ecef(parser_state.msg_buff);
break;
case SBP_BASELINE_NED_MSGTYPE:
sbp_process_baseline_ned(parser_state.msg_buff);
break;
case SBP_VEL_ECEF_MSGTYPE:
sbp_process_vel_ecef(parser_state.msg_buff);
break;
case SBP_VEL_NED_MSGTYPE:
sbp_process_vel_ned(parser_state.msg_buff);
break;
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_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 {
Debug("CRC Error Occurred!");
crc_error_counter += 1;
}
}
break;
default:
parser_state.state = sbp_parser_state_t::WAITING;
break;
}
}
//We have parsed all the waiting messages
return false;
}
void
AP_GPS_SBP::sbp_process_heartbeat(uint8_t* msg)
{
last_heatbeat_received_ms = hal.scheduler->millis();
}
void
AP_GPS_SBP::sbp_process_gpstime(uint8_t* msg)
{
struct sbp_gps_time_t* t = (struct sbp_gps_time_t*)msg;
state.time_week = t->wn;
state.time_week_ms = t->tow;
}
void
AP_GPS_SBP::sbp_process_dops(uint8_t* msg)
{
struct sbp_dops_t* d = (struct sbp_dops_t*) msg;
state.time_week_ms = d->tow;
state.hdop = d->hdop;
}
void
AP_GPS_SBP::sbp_process_pos_ecef(uint8_t* msg)
{
//Using LLH, not ECEF
}
void
AP_GPS_SBP::sbp_process_pos_llh(uint8_t* msg)
{
struct sbp_pos_llh_t* pos = (struct sbp_pos_llh_t*)msg;
last_sbp_pos_llh_msg = *pos;
has_new_pos_llh = true;
#if SBP_DEBUGGING || SBP_HW_LOGGING
pos_msg_counter += 1;
#endif
#if SBP_HW_LOGGING
logging_log_llh(pos);
#endif
}
void
AP_GPS_SBP::sbp_process_baseline_ecef(uint8_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;
#endif
#if SBP_HW_LOGGING
logging_log_baseline_ecef(b);
#endif
}
void
AP_GPS_SBP::sbp_process_baseline_ned(uint8_t* msg)
{
//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
AP_GPS_SBP::sbp_process_vel_ecef(uint8_t* msg)
{
//Currently we use NED velocity.
}
void
AP_GPS_SBP::sbp_process_vel_ned(uint8_t* msg)
{
struct sbp_vel_ned_t* vel = (struct sbp_vel_ned_t*)msg;
last_sbp_vel_ned_msg = *vel;
has_new_vel_ned = true;
#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
}
}
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
AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
{
//This switch reads one character at a time,
//if we find something that looks like our preamble
//we'll try to read the full message length, calculating the CRC.
//If the CRC matches, we have a SBP GPS!
switch(state.state) {
case SBP_detect_state::WAITING:
if (data == SBP_PREAMBLE) {
state.n_read = 0;
state.crc_so_far = 0;
state.state = SBP_detect_state::GET_TYPE;
}
break;
case SBP_detect_state::GET_TYPE:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= 2) {
state.n_read = 0;
state.state = SBP_detect_state::GET_SENDER;
}
break;
case SBP_detect_state::GET_SENDER:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= 2) {
state.n_read = 0;
state.state = SBP_detect_state::GET_LEN;
}
break;
case SBP_detect_state::GET_LEN:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.msg_len = data;
state.n_read = 0;
state.state = SBP_detect_state::GET_MSG;
break;
case SBP_detect_state::GET_MSG:
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
state.n_read += 1;
if (state.n_read >= state.msg_len) {
state.n_read = 0;
state.state = SBP_detect_state::GET_CRC;
}
break;
case SBP_detect_state::GET_CRC:
*((uint8_t*)&(state.crc) + state.n_read) = data;
state.n_read += 1;
if (state.n_read >= 2) {
state.state = SBP_detect_state::WAITING;
return state.crc == state.crc_so_far;
}
break;
default:
state.state = SBP_detect_state::WAITING;
break;
}
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
#define LOG_MSG_SBPHEALTH 202
#define LOG_MSG_SBPLLH 203
#define LOG_MSG_SBPBASELINE 204
#define LOG_MSG_SBPTRACKING1 205
#define LOG_MSG_SBPTRACKING2 206
struct PACKED log_SbpHealth {
LOG_PACKET_HEADER;
uint32_t timestamp;
float pos_msg_hz;
float vel_msg_hz;
float baseline_msg_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 {
LOG_PACKET_HEADER;
uint32_t timestamp;
uint32_t tow; //< GPS Time of Week of ECEF Baseline (unit: ms)
int32_t x; //< Baseline ECEF X coordinate
int32_t y; //< Baseline ECEF Y coordinate
int32_t z; //< Baseline ECEF Z coordinate
int32_t length; //< Baseline length
uint16_t accuracy; //< Horizontal position accuracy estimate (unit: mm)
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 = {
{ LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth),
"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),
"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)
{
if (!logging_started) {
logging_started = true;
gps._DataFlash->AddLogFormats(sbp_log_structures, sizeof(sbp_log_structures) / sizeof(LogStructure));
}
}
void
AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpHealth pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
timestamp : hal.scheduler->millis(),
pos_msg_hz : pos_msg_hz,
vel_msg_hz : vel_msg_hz,
baseline_msg_hz : baseline_msg_hz,
full_update_hz : full_update_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));
};
void
AP_GPS_SBP::logging_log_llh(struct sbp_pos_llh_t* p)
{
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
logging_write_headers();
struct log_SbpLLH pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPLLH),
timestamp : hal.scheduler->millis(),
tow : p->tow,
lat : (int32_t) (p->lat*1e7),
lon : (int32_t) (p->lon*1e7),
alt : (int32_t) (p->height*1e2),
n_sats : p->n_sats,
};
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.0f);
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* tstate, 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 : tstate[0].prn,
ch1_cn0 : tstate[0].cn0,
ch2_prn : (uint8_t)(num < 1 ? 0 : tstate[1].prn),
ch2_cn0 : num < 1 ? 0 : tstate[1].cn0,
ch3_prn : (uint8_t)(num < 2 ? 0 : tstate[2].prn),
ch3_cn0 : num < 2 ? 0 : tstate[2].cn0,
ch4_prn : (uint8_t)(num < 3 ? 0 : tstate[3].prn),
ch4_cn0 : num < 3 ? 0 : tstate[3].cn0,
ch5_prn : (uint8_t)(num < 4 ? 0 : tstate[4].prn),
ch5_cn0 : num < 4 ? 0 : tstate[4].cn0,
ch6_prn : (uint8_t)(num < 5 ? 0 : tstate[5].prn),
ch6_cn0 : num < 5 ? 0 : tstate[5].cn0,
ch7_prn : (uint8_t)(num < 6 ? 0 : tstate[6].prn),
ch7_cn0 : num < 6 ? 0 : tstate[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 : (uint8_t)(num < 7 ? 0 : tstate[7].prn),
ch8_cn0 : num < 7 ? 0 : tstate[7].cn0,
ch9_prn : (uint8_t)(num < 8 ? 0 : tstate[8].prn),
ch9_cn0 : num < 8 ? 0 : tstate[8].cn0,
ch10_prn : (uint8_t)(num < 9 ? 0 : tstate[9].prn),
ch10_cn0 : num < 9 ? 0 : tstate[9].cn0,
ch11_prn : (uint8_t)(num < 10 ? 0 : tstate[10].prn),
ch11_cn0 : num < 10 ? 0 : tstate[10].cn0,
ch12_prn : (uint8_t)(num < 11 ? 0 : tstate[11].prn),
ch12_cn0 : num < 11 ? 0 : tstate[11].cn0,
ch13_prn : (uint8_t)(num < 12 ? 0 : tstate[12].prn),
ch13_cn0 : num < 12 ? 0 : tstate[12].cn0,
ch14_prn : (uint8_t)(num < 13 ? 0 : tstate[13].prn),
ch14_cn0 : num < 13 ? 0 : tstate[13].cn0,
};
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt));
};
};
#endif // SBP_HW_LOGGING
#endif // GPS_RTK_AVAILABLE