mirror of https://github.com/ArduPilot/ardupilot
AP_GPS: Adds SBP Version 2: Swift Navigation Piksi Multi support
This commit adds: - New driver for SBPv2 --- Support Piksi Multi, Swift Navigation's multi-band multi-constellation GPS --- Proper parsing of SBP flags --- Instant response to Piksi status changes (no more timeouts) --- Support for Piksi Multi as a Single-Point-Positioning GPS or only onboard GPS --- Incorporates horizontal and vertical accuracy estimates, and vdop. - Updates driver for SBPv0 --- Continue support for previous Piksi - Dispatches correct driver based on SBP version.
This commit is contained in:
Niels Joubert
Francisco Ferreira
parent
d5ec7b0aad
commit
4bf9fc071c
|
|
@ -30,6 +30,7 @@
|
|||
#include "AP_GPS_QURT.h"
|
||||
#include "AP_GPS_SBF.h"
|
||||
#include "AP_GPS_SBP.h"
|
||||
#include "AP_GPS_SBP2.h"
|
||||
#include "AP_GPS_SIRF.h"
|
||||
#include "AP_GPS_UBLOX.h"
|
||||
#include "AP_GPS_MAV.h"
|
||||
|
|
@ -526,6 +527,11 @@ void AP_GPS::detect_instance(uint8_t instance)
|
|||
new_gps = new AP_GPS_MTK(*this, state[instance], _port[instance]);
|
||||
}
|
||||
#endif
|
||||
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
|
||||
AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) {
|
||||
_broadcast_gps_type("SBP", instance, dstate->current_baud);
|
||||
new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]);
|
||||
}
|
||||
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) &&
|
||||
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) {
|
||||
_broadcast_gps_type("SBP", instance, dstate->current_baud);
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ public:
|
|||
friend class AP_GPS_QURT;
|
||||
friend class AP_GPS_SBF;
|
||||
friend class AP_GPS_SBP;
|
||||
friend class AP_GPS_SBP2;
|
||||
friend class AP_GPS_SIRF;
|
||||
friend class AP_GPS_UBLOX;
|
||||
friend class AP_GPS_Backend;
|
||||
|
|
@ -83,7 +84,7 @@ public:
|
|||
GPS_TYPE_QURT = 12,
|
||||
GPS_TYPE_ERB = 13,
|
||||
GPS_TYPE_MAV = 14,
|
||||
GPS_TYPE_NOVA = 15,
|
||||
GPS_TYPE_NOVA = 15
|
||||
};
|
||||
|
||||
/// GPS status codes
|
||||
|
|
@ -131,15 +132,15 @@ public:
|
|||
float ground_course; ///< ground course in degrees
|
||||
uint16_t hdop; ///< horizontal dilution of precision in cm
|
||||
uint16_t vdop; ///< vertical dilution of precision in cm
|
||||
uint8_t num_sats; ///< Number of visible satellites
|
||||
uint8_t num_sats; ///< Number of visible satellites
|
||||
Vector3f velocity; ///< 3D velocitiy in m/s, in NED format
|
||||
float speed_accuracy;
|
||||
float horizontal_accuracy;
|
||||
float vertical_accuracy;
|
||||
bool have_vertical_velocity:1; ///< does this GPS give vertical velocity?
|
||||
bool have_speed_accuracy:1;
|
||||
bool have_horizontal_accuracy:1;
|
||||
bool have_vertical_accuracy:1;
|
||||
float speed_accuracy; ///< 3D velocity accuracy estimate in m/s
|
||||
float horizontal_accuracy; ///< horizontal accuracy estimate in m
|
||||
float vertical_accuracy; ///< vertical accuracy estimate in m
|
||||
bool have_vertical_velocity:1; ///< does GPS give vertical velocity? Set to true only once available.
|
||||
bool have_speed_accuracy:1; ///< does GPS give speed accuracy? Set to true only once available.
|
||||
bool have_horizontal_accuracy:1; ///< does GPS give horizontal position accuracy? Set to true only once available.
|
||||
bool have_vertical_accuracy:1; ///< does GPS give vertical position accuract? Set to true only once available.
|
||||
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
|
||||
};
|
||||
|
||||
|
|
@ -297,10 +298,10 @@ public:
|
|||
}
|
||||
|
||||
// return true if the GPS supports vertical velocity values
|
||||
bool have_vertical_velocity(uint8_t instance) const {
|
||||
return state[instance].have_vertical_velocity;
|
||||
bool have_vertical_velocity(uint8_t instance) const {
|
||||
return state[instance].have_vertical_velocity;
|
||||
}
|
||||
bool have_vertical_velocity(void) const {
|
||||
bool have_vertical_velocity(void) const {
|
||||
return have_vertical_velocity(primary_instance);
|
||||
}
|
||||
|
||||
|
|
@ -312,7 +313,7 @@ public:
|
|||
const Vector3f &get_antenna_offset(uint8_t instance) const;
|
||||
|
||||
// set position for HIL
|
||||
void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms,
|
||||
void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms,
|
||||
const Location &location, const Vector3f &velocity, uint8_t num_sats,
|
||||
uint16_t hdop);
|
||||
|
||||
|
|
@ -425,6 +426,7 @@ private:
|
|||
struct SIRF_detect_state sirf_detect_state;
|
||||
struct NMEA_detect_state nmea_detect_state;
|
||||
struct SBP_detect_state sbp_detect_state;
|
||||
struct SBP2_detect_state sbp2_detect_state;
|
||||
struct ERB_detect_state erb_detect_state;
|
||||
} detect_state[GPS_MAX_RECEIVERS];
|
||||
|
||||
|
|
@ -442,7 +444,7 @@ private:
|
|||
void _broadcast_gps_type(const char *type, uint8_t instance, int8_t baud_index);
|
||||
|
||||
/*
|
||||
buffer for re-assembling RTCM data for GPS injection.
|
||||
buffer for re-assembling RTCM data for GPS injection.
|
||||
The 8 bit flags field in GPS_RTCM_DATA is interpreted as:
|
||||
1 bit for "is fragmented"
|
||||
2 bits for fragment number
|
||||
|
|
|
|||
|
|
@ -61,7 +61,6 @@ AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state,
|
|||
|
||||
//Externally visible state
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
state.have_vertical_velocity = true;
|
||||
state.last_gps_time_ms = last_heatbeat_received_ms = AP_HAL::millis();
|
||||
|
||||
}
|
||||
|
|
@ -83,7 +82,7 @@ AP_GPS_SBP::read(void)
|
|||
|
||||
}
|
||||
|
||||
void
|
||||
void
|
||||
AP_GPS_SBP::inject_data(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
|
||||
|
|
@ -99,7 +98,7 @@ AP_GPS_SBP::inject_data(const uint8_t *data, uint16_t len)
|
|||
//This attempts to reads all SBP messages from the incoming port.
|
||||
//Returns true if a new message was read, false if we failed to read a message.
|
||||
void
|
||||
AP_GPS_SBP::_sbp_process()
|
||||
AP_GPS_SBP::_sbp_process()
|
||||
{
|
||||
|
||||
while (port->available() > 0) {
|
||||
|
|
@ -223,9 +222,9 @@ AP_GPS_SBP::_sbp_process_message() {
|
|||
}
|
||||
|
||||
default:
|
||||
// log anyway if it's an unsupported message.
|
||||
// log anyway if it's an unsupported message.
|
||||
// The log mask will be used to adjust or suppress logging
|
||||
break;
|
||||
break;
|
||||
}
|
||||
|
||||
logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff);
|
||||
|
|
@ -245,7 +244,7 @@ AP_GPS_SBP::_attempt_state_update()
|
|||
|
||||
if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
|
||||
|
||||
state.status = AP_GPS::NO_GPS;
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
Debug("No Heartbeats from Piksi! Driver Ready to Die!");
|
||||
|
||||
} else if (last_pos_llh_rtk.tow == last_vel_ned.tow
|
||||
|
|
@ -263,9 +262,10 @@ AP_GPS_SBP::_attempt_state_update()
|
|||
state.hdop = last_dops.hdop;
|
||||
|
||||
// Update velocity state
|
||||
state.velocity[0] = (float)(last_vel_ned.n / 1000.0);
|
||||
state.velocity[1] = (float)(last_vel_ned.e / 1000.0);
|
||||
state.velocity[2] = (float)(last_vel_ned.d / 1000.0);
|
||||
state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
|
||||
state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
|
||||
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
|
||||
state.have_vertical_velocity = true;
|
||||
|
||||
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
|
||||
state.ground_speed = safe_sqrt(ground_vector_sq);
|
||||
|
|
@ -329,6 +329,7 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
|
|||
break;
|
||||
|
||||
case SBP_detect_state::GET_TYPE:
|
||||
*((uint8_t*)&(state.msg_type) + state.n_read) = data;
|
||||
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
|
||||
state.n_read += 1;
|
||||
if (state.n_read >= 2) {
|
||||
|
|
@ -354,6 +355,9 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
|
|||
break;
|
||||
|
||||
case SBP_detect_state::GET_MSG:
|
||||
if (state.msg_type == SBP_HEARTBEAT_MSGTYPE && state.n_read < 4) {
|
||||
*((uint8_t*)&(state.heartbeat_buff) + state.n_read) = data;
|
||||
}
|
||||
state.crc_so_far = crc16_ccitt(&data, 1, state.crc_so_far);
|
||||
state.n_read += 1;
|
||||
if (state.n_read >= state.msg_len) {
|
||||
|
|
@ -367,7 +371,12 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
|
|||
state.n_read += 1;
|
||||
if (state.n_read >= 2) {
|
||||
state.state = SBP_detect_state::WAITING;
|
||||
return state.crc == state.crc_so_far;
|
||||
if (state.crc == state.crc_so_far
|
||||
&& state.msg_type == SBP_HEARTBEAT_MSGTYPE) {
|
||||
struct sbp_heartbeat_t* heartbeat = ((struct sbp_heartbeat_t*)state.heartbeat_buff);
|
||||
return heartbeat->protocol_major == 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -380,7 +389,7 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data)
|
|||
|
||||
#if SBP_HW_LOGGING
|
||||
|
||||
void
|
||||
void
|
||||
AP_GPS_SBP::logging_log_full_update()
|
||||
{
|
||||
|
||||
|
|
@ -395,14 +404,14 @@ AP_GPS_SBP::logging_log_full_update()
|
|||
last_injected_data_ms : last_injected_data_ms,
|
||||
last_iar_num_hypotheses : last_iar_num_hypotheses,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
};
|
||||
|
||||
void
|
||||
AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
||||
uint16_t sender_id,
|
||||
uint8_t msg_len,
|
||||
AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
||||
uint16_t sender_id,
|
||||
uint8_t msg_len,
|
||||
uint8_t *msg_buff) {
|
||||
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
|
|
@ -423,8 +432,8 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
|||
sender_id : sender_id,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data1, msg_buff, MIN(msg_len,64));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
memcpy(pkt.data1, msg_buff, MIN(msg_len,64));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
if (msg_len > 64) {
|
||||
|
||||
|
|
@ -434,7 +443,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
|
|||
msg_type : msg_type,
|
||||
};
|
||||
memcpy(pkt2.data2, &msg_buff[64], msg_len - 64);
|
||||
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ private:
|
|||
// ************************************************************************
|
||||
// Swift Navigation SBP protocol types and definitions
|
||||
// ************************************************************************
|
||||
|
||||
|
||||
struct sbp_parser_state_t {
|
||||
enum {
|
||||
WAITING = 0,
|
||||
|
|
@ -62,10 +62,10 @@ private:
|
|||
} 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_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;
|
||||
|
|
@ -76,7 +76,21 @@ private:
|
|||
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 {
|
||||
|
|
@ -116,7 +130,7 @@ private:
|
|||
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 n_sats; //< Number of satellites used in solution
|
||||
uint8_t flags; //< Status flags (reserved)
|
||||
}; // 22 bytes
|
||||
|
||||
|
|
@ -164,6 +178,6 @@ private:
|
|||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,482 @@
|
|||
/*
|
||||
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/
|
||||
//
|
||||
|
||||
#include "AP_GPS.h"
|
||||
#include "AP_GPS_SBP2.h"
|
||||
#include <DataFlash/DataFlash.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define SBP_DEBUGGING 0
|
||||
#define SBP_INFOREPORTING 1
|
||||
|
||||
//INVARIANT: We expect SBP to give us a heartbeat in less than 2 seconds.
|
||||
// This is more lax than the default Piksi settings,
|
||||
// and we assume the user hasn't reconfigured their Piksi to longer heartbeat intervals
|
||||
#define SBP_TIMEOUT_HEARTBEAT 2000
|
||||
|
||||
#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
|
||||
|
||||
#if SBP_INFOREPORTING
|
||||
# define Info(fmt, args ...) \
|
||||
do { \
|
||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, fmt "\n", ## args); \
|
||||
} while(0)
|
||||
#else
|
||||
# define Info(fmt, args ...)
|
||||
#endif
|
||||
|
||||
|
||||
AP_GPS_SBP2::AP_GPS_SBP2(AP_GPS &_gps, AP_GPS::GPS_State &_state,
|
||||
AP_HAL::UARTDriver *_port) :
|
||||
AP_GPS_Backend(_gps, _state, _port)
|
||||
{
|
||||
Debug("SBP Driver Initialized");
|
||||
parser_state.state = sbp_parser_state_t::WAITING;
|
||||
}
|
||||
|
||||
// Process all bytes available from the stream
|
||||
//
|
||||
bool
|
||||
AP_GPS_SBP2::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
|
||||
|
||||
_sbp_process();
|
||||
return _attempt_state_update();
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_SBP2::inject_data(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
if (port->txspace() > len) {
|
||||
last_injected_data_ms = AP_HAL::millis();
|
||||
port->write(data, len);
|
||||
} else {
|
||||
Debug("PIKSI: Not enough TXSPACE");
|
||||
}
|
||||
}
|
||||
|
||||
//This attempts to reads all SBP messages from the incoming port.
|
||||
//Returns true if a new message was read, false if we failed to read a message.
|
||||
void
|
||||
AP_GPS_SBP2::_sbp_process()
|
||||
{
|
||||
uint32_t nleft = port->available();
|
||||
while (nleft > 0) {
|
||||
nleft--;
|
||||
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) {
|
||||
_sbp_process_message();
|
||||
} else {
|
||||
Debug("CRC Error Occurred!");
|
||||
crc_error_counter += 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
parser_state.state = sbp_parser_state_t::WAITING;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//INVARIANT: A fully received message with correct CRC is currently in parser_state
|
||||
void
|
||||
AP_GPS_SBP2::_sbp_process_message() {
|
||||
//Here, we copy messages into local structs.
|
||||
switch (parser_state.msg_type) {
|
||||
case SBP_HEARTBEAT_MSGTYPE:
|
||||
last_heartbeat = *((struct sbp_heartbeat_t*)parser_state.msg_buff);
|
||||
last_heartbeat_received_ms = AP_HAL::millis();
|
||||
break;
|
||||
|
||||
case SBP_GPS_TIME_MSGTYPE:
|
||||
last_gps_time = *((struct sbp_gps_time_t*)parser_state.msg_buff);
|
||||
break;
|
||||
|
||||
case SBP_VEL_NED_MSGTYPE:
|
||||
last_vel_ned = *((struct sbp_vel_ned_t*)parser_state.msg_buff);
|
||||
break;
|
||||
|
||||
case SBP_POS_LLH_MSGTYPE:
|
||||
last_pos_llh = *((struct sbp_pos_llh_t*)parser_state.msg_buff);
|
||||
break;
|
||||
|
||||
case SBP_DOPS_MSGTYPE:
|
||||
last_dops = *((struct sbp_dops_t*)parser_state.msg_buff);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
// send all messages we receive to log, even if it's an unsupported message,
|
||||
// so we can do annditional post-processing from Dataflash logs.
|
||||
// The log mask will be used to adjust or suppress logging
|
||||
logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff);
|
||||
}
|
||||
|
||||
int32_t
|
||||
AP_GPS_SBP2::distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod) {
|
||||
return MIN(abs(tow1_ms - tow2_ms), mod - abs(tow1_ms - tow2_ms));
|
||||
}
|
||||
|
||||
bool
|
||||
AP_GPS_SBP2::_attempt_state_update()
|
||||
{
|
||||
if (last_heartbeat_received_ms == 0)
|
||||
return false;
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
if (now - last_heartbeat_received_ms > SBP_TIMEOUT_HEARTBEAT) {
|
||||
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
Info("No Heartbeats from Piksi! Status to NO_FIX.");
|
||||
return false;
|
||||
|
||||
} else if (last_heartbeat.protocol_major != 2) {
|
||||
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
Info("Received a heartbeat from non-SBPv2 device. Current driver only supports SBPv2. Status to NO_FIX.");
|
||||
return false;
|
||||
|
||||
} else if (last_heartbeat.nap_error == 1 ||
|
||||
last_heartbeat.io_error == 1 ||
|
||||
last_heartbeat.sys_error == 1) {
|
||||
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
|
||||
Info("Piksi reported an error. Status to NO_FIX.");
|
||||
Debug(" ext_antenna: %d", last_heartbeat.ext_antenna);
|
||||
Debug(" res2: %d", last_heartbeat.res2);
|
||||
Debug(" protocol_major: %d", last_heartbeat.protocol_major);
|
||||
Debug(" protocol_minor: %d", last_heartbeat.protocol_minor);
|
||||
Debug(" res: %d", last_heartbeat.res);
|
||||
Debug(" nap_error: %d", last_heartbeat.nap_error);
|
||||
Debug(" io_error: %d", last_heartbeat.io_error);
|
||||
Debug(" sys_error: %d", last_heartbeat.sys_error);
|
||||
|
||||
return false;
|
||||
|
||||
} else if (last_pos_llh.tow == last_vel_ned.tow
|
||||
&& (distMod(last_gps_time.tow, last_vel_ned.tow, MSEC_PER_WEEK) < 10000)
|
||||
&& (distMod(last_dops.tow, last_vel_ned.tow, MSEC_PER_WEEK) < 60000)
|
||||
&& (last_vel_ned.tow > last_full_update_tow || (last_gps_time.wn > last_full_update_wn && last_vel_ned.tow < last_full_update_tow))) {
|
||||
|
||||
//We have an aligned VEL and LLH, and a recent DOPS and TIME.
|
||||
|
||||
//
|
||||
// Check Flags for Valid Messages
|
||||
//
|
||||
if (last_gps_time.flags.fix_mode == 0 ||
|
||||
last_vel_ned.flags.fix_mode == 0 ||
|
||||
last_pos_llh.flags.fix_mode == 0 ||
|
||||
last_dops.flags.fix_mode == 0) {
|
||||
|
||||
Debug("Message Marked as Invalid. NO FIX! Flags: {GPS_TIME: %d, VEL_NED: %d, POS_LLH: %d, DOPS: %d}",
|
||||
last_gps_time.flags.fix_mode,
|
||||
last_vel_ned.flags.fix_mode,
|
||||
last_pos_llh.flags.fix_mode,
|
||||
last_dops.flags.fix_mode);
|
||||
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
return false;
|
||||
}
|
||||
|
||||
//
|
||||
// Update external time and accuracy state
|
||||
//
|
||||
state.time_week = last_gps_time.wn;
|
||||
state.time_week_ms = last_vel_ned.tow;
|
||||
state.hdop = last_dops.hdop;
|
||||
state.vdop = last_dops.vdop;
|
||||
state.last_gps_time_ms = now;
|
||||
|
||||
//
|
||||
// Update velocity state
|
||||
//
|
||||
state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
|
||||
state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
|
||||
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
|
||||
|
||||
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 = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
|
||||
|
||||
state.speed_accuracy = safe_sqrt(
|
||||
pow((float)last_vel_ned.h_accuracy * 1.0e-3f, 2) +
|
||||
pow((float)last_vel_ned.v_accuracy * 1.0e-3f, 2));
|
||||
state.horizontal_accuracy = (float) last_pos_llh.h_accuracy * 1.0e-3f;
|
||||
state.vertical_accuracy = (float) last_pos_llh.v_accuracy * 1.0e-3f;
|
||||
|
||||
//
|
||||
// Set flags appropriartely
|
||||
//
|
||||
state.have_vertical_velocity = true;
|
||||
state.have_speed_accuracy = !is_zero(state.speed_accuracy);
|
||||
state.have_horizontal_accuracy = !is_zero(state.horizontal_accuracy);
|
||||
state.have_vertical_accuracy = !is_zero(state.vertical_accuracy);
|
||||
|
||||
//
|
||||
// Update position state
|
||||
//
|
||||
state.location.lat = (int32_t) (last_pos_llh.lat * (double)1e7);
|
||||
state.location.lng = (int32_t) (last_pos_llh.lon * (double)1e7);
|
||||
state.location.alt = (int32_t) (last_pos_llh.height * 100);
|
||||
state.num_sats = last_pos_llh.n_sats;
|
||||
|
||||
switch (last_pos_llh.flags.fix_mode) {
|
||||
case 1:
|
||||
state.status = AP_GPS::GPS_OK_FIX_3D;
|
||||
break;
|
||||
case 2:
|
||||
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
|
||||
break;
|
||||
case 3:
|
||||
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
|
||||
break;
|
||||
case 4:
|
||||
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
|
||||
break;
|
||||
default:
|
||||
state.status = AP_GPS::NO_FIX;
|
||||
break;
|
||||
}
|
||||
|
||||
//
|
||||
// Update Internal Timing
|
||||
//
|
||||
last_full_update_tow = last_vel_ned.tow;
|
||||
last_full_update_wn = last_gps_time.wn;
|
||||
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool
|
||||
AP_GPS_SBP2::_detect(struct SBP2_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 an SBP GPS!
|
||||
switch (state.state) {
|
||||
case SBP2_detect_state::WAITING:
|
||||
if (data == SBP_PREAMBLE) {
|
||||
state.n_read = 0;
|
||||
state.crc_so_far = 0;
|
||||
state.state = SBP2_detect_state::GET_TYPE;
|
||||
}
|
||||
break;
|
||||
|
||||
case SBP2_detect_state::GET_TYPE:
|
||||
*((uint8_t*)&(state.msg_type) + state.n_read) = data;
|
||||
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 = SBP2_detect_state::GET_SENDER;
|
||||
}
|
||||
break;
|
||||
|
||||
case SBP2_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 = SBP2_detect_state::GET_LEN;
|
||||
}
|
||||
break;
|
||||
|
||||
case SBP2_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 = SBP2_detect_state::GET_MSG;
|
||||
break;
|
||||
|
||||
case SBP2_detect_state::GET_MSG:
|
||||
if (state.msg_type == SBP_HEARTBEAT_MSGTYPE && state.n_read < 4) {
|
||||
*((uint8_t*)&(state.heartbeat_buff) + state.n_read) = data;
|
||||
}
|
||||
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 = SBP2_detect_state::GET_CRC;
|
||||
}
|
||||
break;
|
||||
|
||||
case SBP2_detect_state::GET_CRC:
|
||||
*((uint8_t*)&(state.crc) + state.n_read) = data;
|
||||
state.n_read += 1;
|
||||
if (state.n_read >= 2) {
|
||||
state.state = SBP2_detect_state::WAITING;
|
||||
if (state.crc == state.crc_so_far
|
||||
&& state.msg_type == SBP_HEARTBEAT_MSGTYPE) {
|
||||
struct sbp_heartbeat_t* heartbeat = ((struct sbp_heartbeat_t*)state.heartbeat_buff);
|
||||
return heartbeat->protocol_major == 2;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
state.state = SBP2_detect_state::WAITING;
|
||||
break;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void
|
||||
AP_GPS_SBP2::logging_log_full_update()
|
||||
{
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
return;
|
||||
}
|
||||
|
||||
//TODO: Expand with heartbeat info.
|
||||
//TODO: Get rid of IAR NUM HYPO
|
||||
|
||||
struct log_SbpHealth pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
|
||||
time_us : AP_HAL::micros64(),
|
||||
crc_error_counter : crc_error_counter,
|
||||
last_injected_data_ms : last_injected_data_ms,
|
||||
last_iar_num_hypotheses : 0,
|
||||
};
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
};
|
||||
|
||||
void
|
||||
AP_GPS_SBP2::logging_log_raw_sbp(uint16_t msg_type,
|
||||
uint16_t sender_id,
|
||||
uint8_t msg_len,
|
||||
uint8_t *msg_buff) {
|
||||
if (gps._DataFlash == nullptr || !gps._DataFlash->logging_started()) {
|
||||
return;
|
||||
}
|
||||
|
||||
//MASK OUT MESSAGES WE DON'T WANT TO LOG
|
||||
if (( ((uint16_t) gps._sbp_logmask) & msg_type) == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t time_us = AP_HAL::micros64();
|
||||
|
||||
struct log_SbpRAW1 pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW1),
|
||||
time_us : time_us,
|
||||
msg_type : msg_type,
|
||||
sender_id : sender_id,
|
||||
msg_len : msg_len,
|
||||
};
|
||||
memcpy(pkt.data1, msg_buff, MIN(msg_len,64));
|
||||
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
|
||||
|
||||
if (msg_len > 64) {
|
||||
struct log_SbpRAW2 pkt2 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW2),
|
||||
time_us : time_us,
|
||||
msg_type : msg_type,
|
||||
};
|
||||
memcpy(pkt2.data2, &msg_buff[64], MIN(msg_len - 64,192));
|
||||
gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2));
|
||||
}
|
||||
};
|
||||
|
|
@ -0,0 +1,184 @@
|
|||
/*
|
||||
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"
|
||||
|
||||
class AP_GPS_SBP2 : public AP_GPS_Backend
|
||||
{
|
||||
public:
|
||||
AP_GPS_SBP2(AP_GPS &_gps, 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; }
|
||||
|
||||
// Methods
|
||||
bool read() override;
|
||||
|
||||
void inject_data(const uint8_t *data, uint16_t len) override;
|
||||
|
||||
static bool _detect(struct SBP2_detect_state &state, uint8_t data);
|
||||
|
||||
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 = 0x0102;
|
||||
static const uint16_t SBP_DOPS_MSGTYPE = 0x0208;
|
||||
static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0209;
|
||||
static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A;
|
||||
static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x020B;
|
||||
static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x020C;
|
||||
static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x020D;
|
||||
static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E;
|
||||
static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0013;
|
||||
static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019;
|
||||
|
||||
// Heartbeat
|
||||
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)
|
||||
struct PACKED flags {
|
||||
uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX
|
||||
uint8_t res:5; //< Reserved
|
||||
} flags;
|
||||
}; // 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)
|
||||
struct PACKED flags {
|
||||
uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX
|
||||
uint8_t res:4; //< Reserved
|
||||
bool raim_repair:1; //< Solution from RAIM?
|
||||
} flags;
|
||||
}; // 15 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
|
||||
struct PACKED flags {
|
||||
uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX
|
||||
uint8_t res:4; //< Reserved
|
||||
bool raim_repair:1; //< Solution from RAIM?
|
||||
} 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
|
||||
struct PACKED flags {
|
||||
uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX
|
||||
uint8_t res:5; //< Reserved
|
||||
} flags;
|
||||
}; // 22 bytes
|
||||
|
||||
void _sbp_process();
|
||||
void _sbp_process_message();
|
||||
bool _attempt_state_update();
|
||||
|
||||
// ************************************************************************
|
||||
// Internal Received Messages State
|
||||
// ************************************************************************
|
||||
uint32_t last_heartbeat_received_ms;
|
||||
uint32_t last_injected_data_ms;
|
||||
|
||||
struct sbp_heartbeat_t last_heartbeat;
|
||||
struct sbp_gps_time_t last_gps_time;
|
||||
struct sbp_dops_t last_dops;
|
||||
struct sbp_pos_llh_t last_pos_llh;
|
||||
struct sbp_vel_ned_t last_vel_ned;
|
||||
|
||||
uint32_t last_full_update_tow;
|
||||
uint16_t last_full_update_wn;
|
||||
|
||||
// ************************************************************************
|
||||
// Monitoring and Performance Counting
|
||||
// ************************************************************************
|
||||
|
||||
uint32_t crc_error_counter;
|
||||
|
||||
// ************************************************************************
|
||||
// Logging to DataFlash
|
||||
// ************************************************************************
|
||||
|
||||
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);
|
||||
|
||||
int32_t distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -68,8 +68,28 @@ struct SBP_detect_state {
|
|||
GET_MSG = 4,
|
||||
GET_CRC = 5
|
||||
} state:8;
|
||||
uint16_t msg_type;
|
||||
uint8_t n_read;
|
||||
uint8_t msg_len;
|
||||
uint16_t crc_so_far;
|
||||
uint16_t crc;
|
||||
uint8_t heartbeat_buff[4];
|
||||
};
|
||||
|
||||
|
||||
struct SBP2_detect_state {
|
||||
enum {
|
||||
WAITING = 0,
|
||||
GET_TYPE = 1,
|
||||
GET_SENDER = 2,
|
||||
GET_LEN = 3,
|
||||
GET_MSG = 4,
|
||||
GET_CRC = 5
|
||||
} state:8;
|
||||
uint16_t msg_type;
|
||||
uint8_t n_read;
|
||||
uint8_t msg_len;
|
||||
uint16_t crc_so_far;
|
||||
uint16_t crc;
|
||||
uint8_t heartbeat_buff[4];
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue