Skip to content

Fixed support for RTK GPS on UAVCAN #98

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: mttr-rebase-3.6.7
Choose a base branch
from
Open

Fixed support for RTK GPS on UAVCAN #98

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
76e5ed1
AP_UAVCAN: send RTCMStream packets for RTK GPS on UAVCAN
tridge Nov 8, 2019
70e3655
AP_GPS: adapt for new RTCMStream API for RTK UAVCAN support
tridge Nov 8, 2019
4d48df4
AP_UAVCAN: implement Fix2 for RTK GPS
tridge Nov 8, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 0 additions & 7 deletions libraries/AP_GPS/AP_GPS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1076,13 +1076,6 @@ bool AP_GPS::blend_health_check() const
*/
void AP_GPS::handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len)
{
// allow backends access to the unfragmented data (for UAVCAN)
for (uint8_t instance=0; instance<num_instances; instance++) {
if (drivers[instance]) {
drivers[instance]->handle_rtcm_data(flags, data, len);
}
}

if ((flags & 1) == 0) {
// it is not fragmented, pass direct
inject_data(data, len);
Expand Down
9 changes: 2 additions & 7 deletions libraries/AP_GPS/AP_GPS_UAVCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,20 +85,15 @@ void AP_GPS_UAVCAN::handle_gnss_msg(const AP_GPS::GPS_State &msg)
}
}

/*
handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over
MAVLink. We don't use the inject_data method as we want to retain
the fragmentation and let the GPS de-fragment it
*/
void AP_GPS_UAVCAN::handle_rtcm_data(uint8_t flags, const uint8_t *data, uint16_t len)
void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len)
{
// we only handle this if we are the first UAVCAN GPS, as we send
// the data as broadcast on all UAVCAN devive ports and we don't
// want to send duplicates
if (instance == 0) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager);
if (ap_uavcan != nullptr) {
ap_uavcan->send_GNSS_Inject(flags, data, len);
ap_uavcan->send_RTCMStream(data, len);
}
}
}
Expand Down
3 changes: 1 addition & 2 deletions libraries/AP_GPS/AP_GPS_UAVCAN.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,7 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend {

const char *name() const override { return "UAVCAN"; }

// handling of fragmented RTCM data
void handle_rtcm_data(uint8_t flags, const uint8_t *data, uint16_t len) override;
void inject_data(const uint8_t *data, uint16_t len) override;

private:
bool _new_data;
Expand Down
4 changes: 0 additions & 4 deletions libraries/AP_GPS/GPS_Backend.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,6 @@ class AP_GPS_Backend

virtual void inject_data(const uint8_t *data, uint16_t len);

// allow handling of fragmented RTCM data by backends. Used to
// forward GPS_RTCM_DATA from MAVLink over UAVCAN
virtual void handle_rtcm_data(uint8_t flags, const uint8_t *data, uint16_t len) {}

//MAVLink methods
virtual bool supports_mavlink_gps_rtk_message() { return false; }
virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
Expand Down
144 changes: 90 additions & 54 deletions libraries/AP_UAVCAN/AP_UAVCAN.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>

// Zubax GPS and other GPS, baro, magnetic sensors
#include <uavcan/equipment/gnss/Fix.hpp>
#include <uavcan/equipment/gnss/Fix2.hpp>
#include <uavcan/equipment/gnss/Auxiliary.hpp>

#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
Expand All @@ -32,7 +32,7 @@
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/RGB565.hpp>
#include <ardupilot/equipment/gnss/Inject.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp>

#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
Expand Down Expand Up @@ -109,7 +109,7 @@ static void uwb_range_cb(const uavcan::ReceivedDataStructure<com::matternet::equ
}
}

static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2>& msg, uint8_t mgr)
{
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr);
if (ap_uavcan == nullptr) {
Expand All @@ -123,20 +123,32 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::g

bool process = false;

if (msg.status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) {
if (msg.status == uavcan::equipment::gnss::Fix2::STATUS_NO_FIX) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else {
if (msg.status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) {
if (msg.status == uavcan::equipment::gnss::Fix2::STATUS_TIME_ONLY) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) {
} else if (msg.status == uavcan::equipment::gnss::Fix2::STATUS_2D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_2D;
process = true;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_3D_FIX) {
} else if (msg.status == uavcan::equipment::gnss::Fix2::STATUS_3D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D;
process = true;
}

if (msg.gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) {
if (state->status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
if (msg.mode == uavcan::equipment::gnss::Fix2::MODE_DGPS) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS;
} else if (msg.mode == uavcan::equipment::gnss::Fix2::MODE_RTK) {
if (msg.sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT;
} else if (msg.sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED;
}
}
}

if (msg.gnss_time_standard == uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC) {
uint64_t epoch_ms = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
epoch_ms /= 1000;
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
Expand All @@ -163,38 +175,27 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::g
state->have_vertical_velocity = false;
}

float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
if (!uavcan::isNaN(pos_cov[8])) {
if (pos_cov[8] > 0) {
state->vertical_accuracy = sqrtf(pos_cov[8]);
if (msg.covariance.size() == 6) {
if (!uavcan::isNaN(msg.covariance[0])) {
state->horizontal_accuracy = sqrtf(msg.covariance[0]);
state->have_horizontal_accuracy = true;
} else {
state->have_horizontal_accuracy = false;
}
if (!uavcan::isNaN(msg.covariance[2])) {
state->vertical_accuracy = sqrtf(msg.covariance[2]);
state->have_vertical_accuracy = true;
} else {
state->have_vertical_accuracy = false;
}
} else {
state->have_vertical_accuracy = false;
}

const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]);
if (!uavcan::isNaN(horizontal_pos_variance)) {
if (horizontal_pos_variance > 0) {
state->horizontal_accuracy = sqrtf(horizontal_pos_variance);
state->have_horizontal_accuracy = true;
if (!uavcan::isNaN(msg.covariance[3]) &&
!uavcan::isNaN(msg.covariance[4]) &&
!uavcan::isNaN(msg.covariance[5])) {
state->speed_accuracy = sqrtf((msg.covariance[3] + msg.covariance[4] + msg.covariance[5])/3);
state->have_speed_accuracy = true;
} else {
state->have_horizontal_accuracy = false;
state->have_speed_accuracy = false;
}
} else {
state->have_horizontal_accuracy = false;
}

float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
if (!uavcan::isNaN(vel_cov[0])) {
state->speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0);
state->have_speed_accuracy = true;
} else {
state->have_speed_accuracy = false;
}

state->num_sats = msg.sats_used;
Expand All @@ -212,11 +213,11 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::g
ap_uavcan->update_gps_state(msg.getSrcNodeID().get());
}

static void gnss_fix_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
static void gnss_fix_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2>& msg)
{ gnss_fix_cb(msg, 0); }
static void gnss_fix_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
static void gnss_fix_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2>& msg)
{ gnss_fix_cb(msg, 1); }
static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg)
static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2>& msg)
= { gnss_fix_cb0, gnss_fix_cb1 };

static void gnss_aux_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary>& msg, uint8_t mgr)
Expand Down Expand Up @@ -385,7 +386,7 @@ static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uav
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<ardupilot::equipment::gnss::Inject>* gnss_inject[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS];

// handler TrafficReport
UC_REGISTRY_BINDER(TrafficReportCb, com::matternet::equipment::trafficmonitor::TrafficReport);
Expand Down Expand Up @@ -439,6 +440,7 @@ AP_UAVCAN::AP_UAVCAN() :

SRV_sem = hal.util->new_semaphore();
_led_out_sem = hal.util->new_semaphore();
_rtcm_stream.sem = hal.util->new_semaphore();

debug_uavcan(2, "AP_UAVCAN constructed\n\r");
}
Expand Down Expand Up @@ -508,8 +510,8 @@ bool AP_UAVCAN::try_init(void)
debug_uavcan(1, "UAVCAN: node start problem\n\r");
}

uavcan::Subscriber<uavcan::equipment::gnss::Fix> *gnss_fix;
gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix>(*node);
uavcan::Subscriber<uavcan::equipment::gnss::Fix2> *gnss_fix;
gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix2>(*node);


uavcan::Subscriber<com::matternet::equipment::uwb::RangeObservation> *uwb_range;
Expand Down Expand Up @@ -601,9 +603,9 @@ bool AP_UAVCAN::try_init(void)
rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

gnss_inject[_uavcan_i] = new uavcan::Publisher<ardupilot::equipment::gnss::Inject>(*_node);
gnss_inject[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
gnss_inject[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
rtcm_stream[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
rtcm_stream[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rtcm_stream[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

_led_conf.devices_count = 0;

Expand Down Expand Up @@ -775,6 +777,7 @@ void AP_UAVCAN::do_cyclic(void)
led_out_sem_give();
}

rtcm_stream_send();
}

bool AP_UAVCAN::led_out_sem_take()
Expand Down Expand Up @@ -1278,21 +1281,54 @@ AP_UAVCAN::Mag_Info *AP_UAVCAN::find_mag_node(uint8_t node, uint8_t sensor_id)
}

/*
send GNSS Inject packet on all active UAVCAN drivers
Same conventions as MAVLink GPS_RTCM_DATA
send RTCMStream packet on all active UAVCAN drivers
*/
void AP_UAVCAN::send_GNSS_Inject(uint8_t flags, const uint8_t *data, uint8_t len)
void AP_UAVCAN::send_RTCMStream(const uint8_t *data, uint32_t len)
{
ardupilot::equipment::gnss::Inject msg;
msg.flags = flags;
while (len--) {
msg.data.push_back(*data++);
}
for (uint8_t i=0; i<MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (gnss_inject[i] != nullptr) {
gnss_inject[i]->broadcast(msg);
_rtcm_stream.sem->take_blocking();
if (_rtcm_stream.buf == nullptr) {
// give enough space for a full round from a NTRIP server with all
// constellations
_rtcm_stream.buf = new ByteBuffer(2400);
}
if (_rtcm_stream.buf != nullptr) {
_rtcm_stream.buf->write(data, len);
}
_rtcm_stream.sem->give();
}

void AP_UAVCAN::rtcm_stream_send()
{
_rtcm_stream.sem->take_blocking();
if (_rtcm_stream.buf == nullptr ||
_rtcm_stream.buf->available() == 0) {
// nothing to send
_rtcm_stream.sem->give();
return;
}
uint32_t now = AP_HAL::millis();
if (now - _rtcm_stream.last_send_ms < 10) {
// don't send more than 100 per second
_rtcm_stream.sem->give();
return;
}
_rtcm_stream.last_send_ms = now;
uavcan::equipment::gnss::RTCMStream msg;
uint32_t len = _rtcm_stream.buf->available();
if (len > 64) {
len = 64;
}
msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3;
for (uint8_t i=0; i<len; i++) {
uint8_t b;
if (!_rtcm_stream.buf->read_byte(&b)) {
_rtcm_stream.sem->give();
return;
}
msg.data.push_back(b);
}
rtcm_stream[_uavcan_i]->broadcast(msg);
_rtcm_stream.sem->give();
}

/*
Expand Down
14 changes: 11 additions & 3 deletions libraries/AP_UAVCAN/AP_UAVCAN.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <uavcan/equipment/indication/RGB565.hpp>

#ifndef UAVCAN_NODE_POOL_SIZE
#define UAVCAN_NODE_POOL_SIZE 32
#define UAVCAN_NODE_POOL_SIZE 128
#endif

#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
Expand Down Expand Up @@ -155,8 +155,8 @@ class AP_UAVCAN {
void SRV_send_servos();
void SRV_send_esc();

// send GNSS Inject packets. Same conventions as MAVLink GPS_RTCM_DATA
void send_GNSS_Inject(uint8_t flags, const uint8_t *data, uint8_t len);
// send RTCMStream packets
void send_RTCMStream(const uint8_t *data, uint32_t len);

template <typename DataType_>
class RegistryBinder {
Expand Down Expand Up @@ -306,6 +306,8 @@ class AP_UAVCAN {

uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE*UAVCAN_NODE_POOL_BLOCK_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;

// send GNSS injection
void rtcm_stream_send();
AP_Int8 _uavcan_node;
AP_Int32 _servo_bm;
AP_Int32 _esc_bm;
Expand All @@ -332,6 +334,12 @@ class AP_UAVCAN {
{
_parent_can_mgr = parent_can_mgr;
}
// GNSS RTCM injection
struct {
AP_HAL::Semaphore *sem;
uint32_t last_send_ms;
ByteBuffer *buf;
} _rtcm_stream;
static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
};

Expand Down