diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 9f4922516a..9ee5fa6ee2 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -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);
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 9dd98f2235..742229fc21 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -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
diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp
index df574a9a34..1b92a348b7 100644
--- a/libraries/AP_GPS/AP_GPS_SBP.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBP.cpp
@@ -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));
}
diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h
index 30e5237cdb..a1dd3c6003 100644
--- a/libraries/AP_GPS/AP_GPS_SBP.h
+++ b/libraries/AP_GPS/AP_GPS_SBP.h
@@ -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);
-
+
};
diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp
new file mode 100644
index 0000000000..8991852f85
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp
@@ -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 .
+ */
+
+//
+// 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
+#include
+#include
+
+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));
+ }
+};
diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h
new file mode 100644
index 0000000000..306b28b1c2
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_SBP2.h
@@ -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 .
+ */
+
+//
+// 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);
+
+};
+
diff --git a/libraries/AP_GPS/GPS_detect_state.h b/libraries/AP_GPS/GPS_detect_state.h
index 53ef21d045..4b7862c204 100644
--- a/libraries/AP_GPS/GPS_detect_state.h
+++ b/libraries/AP_GPS/GPS_detect_state.h
@@ -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];
};