mirror of https://github.com/ArduPilot/ardupilot
192 lines
7.1 KiB
C++
192 lines
7.1 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
//
|
|
// Swift Navigation SBP GPS driver for ArduPilot.
|
|
// Code by Niels Joubert
|
|
//
|
|
// Swift Binary Protocol format: http://docs.swift-nav.com/
|
|
//
|
|
#pragma once
|
|
|
|
#include "AP_GPS.h"
|
|
#include "GPS_Backend.h"
|
|
|
|
#if AP_GPS_SBP_ENABLED
|
|
class AP_GPS_SBP : public AP_GPS_Backend
|
|
{
|
|
public:
|
|
AP_GPS_SBP(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
|
|
|
|
AP_GPS::GPS_Status highest_supported_status(void) override { return AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; }
|
|
|
|
#if HAL_GCS_ENABLED
|
|
bool supports_mavlink_gps_rtk_message() const override { return true; }
|
|
#endif
|
|
|
|
// Methods
|
|
bool read() override;
|
|
|
|
void inject_data(const uint8_t *data, uint16_t len) override;
|
|
|
|
static bool _detect(struct SBP_detect_state &state, uint8_t data);
|
|
|
|
const char *name() const override { return "SBP"; }
|
|
|
|
private:
|
|
|
|
// ************************************************************************
|
|
// Swift Navigation SBP protocol types and definitions
|
|
// ************************************************************************
|
|
|
|
struct sbp_parser_state_t {
|
|
enum {
|
|
WAITING = 0,
|
|
GET_TYPE = 1,
|
|
GET_SENDER = 2,
|
|
GET_LEN = 3,
|
|
GET_MSG = 4,
|
|
GET_CRC = 5
|
|
} state:8;
|
|
uint16_t msg_type;
|
|
uint16_t sender_id;
|
|
uint16_t crc;
|
|
uint8_t msg_len;
|
|
uint8_t n_read;
|
|
uint8_t msg_buff[256];
|
|
} parser_state;
|
|
|
|
static const uint8_t SBP_PREAMBLE = 0x55;
|
|
|
|
//Message types supported by this driver
|
|
static const uint16_t SBP_STARTUP_MSGTYPE = 0xFF00;
|
|
static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF;
|
|
static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100;
|
|
static const uint16_t SBP_DOPS_MSGTYPE = 0x0206;
|
|
static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0200;
|
|
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201;
|
|
static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202;
|
|
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;
|
|
|
|
// Heartbeat
|
|
// struct sbp_heartbeat_t {
|
|
// uint32_t flags; //< Status flags (reserved)
|
|
// }; // 4 bytes
|
|
struct PACKED sbp_heartbeat_t {
|
|
bool sys_error : 1;
|
|
bool io_error : 1;
|
|
bool nap_error : 1;
|
|
uint8_t res : 5;
|
|
uint8_t protocol_minor : 8;
|
|
uint8_t protocol_major : 8;
|
|
uint8_t res2 : 7;
|
|
bool ext_antenna : 1;
|
|
}; // 4 bytes
|
|
|
|
// GPS Time
|
|
struct PACKED sbp_gps_time_t {
|
|
uint16_t wn; //< GPS week number (unit: weeks)
|
|
uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms)
|
|
int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns)
|
|
uint8_t flags; //< Status flags (reserved)
|
|
}; // 11 bytes
|
|
|
|
// Dilution of Precision
|
|
struct PACKED sbp_dops_t {
|
|
uint32_t tow; //< GPS Time of Week (unit: ms)
|
|
uint16_t gdop; //< Geometric Dilution of Precision (unit: 0.01)
|
|
uint16_t pdop; //< Position Dilution of Precision (unit: 0.01)
|
|
uint16_t tdop; //< Time Dilution of Precision (unit: 0.01)
|
|
uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01)
|
|
uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01)
|
|
}; // 14 bytes
|
|
|
|
// Geodetic position solution.
|
|
struct PACKED sbp_pos_llh_t {
|
|
uint32_t tow; //< GPS Time of Week (unit: ms)
|
|
double lat; //< Latitude (unit: degrees)
|
|
double lon; //< Longitude (unit: degrees)
|
|
double height; //< Height (unit: meters)
|
|
uint16_t h_accuracy; //< Horizontal position accuracy estimate (unit: mm)
|
|
uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm)
|
|
uint8_t n_sats; //< Number of satellites used in solution
|
|
uint8_t flags; //< Status flags
|
|
}; // 34 bytes
|
|
|
|
// Velocity in NED Velocity in local North East Down (NED) coordinates.
|
|
struct PACKED sbp_vel_ned_t {
|
|
uint32_t tow; //< GPS Time of Week (unit: ms)
|
|
int32_t n; //< Velocity North coordinate (unit: mm/s)
|
|
int32_t e; //< Velocity East coordinate (unit: mm/s)
|
|
int32_t d; //< Velocity Down coordinate (unit: mm/s)
|
|
uint16_t h_accuracy; //< Horizontal velocity accuracy estimate (unit: mm/s)
|
|
uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s)
|
|
uint8_t n_sats; //< Number of satellites used in solution
|
|
uint8_t flags; //< Status flags (reserved)
|
|
}; // 22 bytes
|
|
|
|
// 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;
|
|
};
|
|
|
|
void _sbp_process();
|
|
void _sbp_process_message();
|
|
bool _attempt_state_update();
|
|
|
|
// ************************************************************************
|
|
// Internal Received Messages State
|
|
// ************************************************************************
|
|
uint32_t last_heatbeat_received_ms;
|
|
uint32_t last_injected_data_ms;
|
|
|
|
struct sbp_gps_time_t last_gps_time;
|
|
struct sbp_dops_t last_dops;
|
|
struct sbp_pos_llh_t last_pos_llh_spp;
|
|
struct sbp_pos_llh_t last_pos_llh_rtk;
|
|
struct sbp_vel_ned_t last_vel_ned;
|
|
uint32_t last_iar_num_hypotheses;
|
|
|
|
uint32_t last_full_update_tow;
|
|
uint32_t last_full_update_cpu_ms;
|
|
|
|
// ************************************************************************
|
|
// Monitoring and Performance Counting
|
|
// ************************************************************************
|
|
|
|
uint32_t crc_error_counter;
|
|
|
|
// ************************************************************************
|
|
// Logging to AP_Logger
|
|
// ************************************************************************
|
|
|
|
void logging_log_full_update();
|
|
void logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff);
|
|
|
|
|
|
};
|
|
#endif
|