From 76e5ed17c0dad2c1bc72cb8aedea0606d55c9f62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Nov 2019 11:18:30 +1100 Subject: [PATCH 1/3] AP_UAVCAN: send RTCMStream packets for RTK GPS on UAVCAN --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 67 +++++++++++++++++++++++-------- libraries/AP_UAVCAN/AP_UAVCAN.h | 14 +++++-- 2 files changed, 62 insertions(+), 19 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 891e060e6d..da2f79bfe3 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include #include @@ -385,7 +385,7 @@ static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS]; -static uavcan::Publisher* gnss_inject[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Publisher* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS]; // handler TrafficReport UC_REGISTRY_BINDER(TrafficReportCb, com::matternet::equipment::trafficmonitor::TrafficReport); @@ -439,6 +439,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"); } @@ -601,9 +602,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(*_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(*_node); + rtcm_stream[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); + rtcm_stream[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); _led_conf.devices_count = 0; @@ -775,6 +776,7 @@ void AP_UAVCAN::do_cyclic(void) led_out_sem_give(); } + rtcm_stream_send(); } bool AP_UAVCAN::led_out_sem_take() @@ -1278,21 +1280,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; ibroadcast(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; iread_byte(&b)) { + _rtcm_stream.sem->give(); + return; } + msg.data.push_back(b); } + rtcm_stream[_uavcan_i]->broadcast(msg); + _rtcm_stream.sem->give(); } /* diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 95d3a96427..0d0b5f17b9 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -21,7 +21,7 @@ #include #ifndef UAVCAN_NODE_POOL_SIZE -#define UAVCAN_NODE_POOL_SIZE 32 +#define UAVCAN_NODE_POOL_SIZE 128 #endif #ifndef UAVCAN_NODE_POOL_BLOCK_SIZE @@ -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 class RegistryBinder { @@ -306,6 +306,8 @@ class AP_UAVCAN { uavcan::PoolAllocator _node_allocator; + // send GNSS injection + void rtcm_stream_send(); AP_Int8 _uavcan_node; AP_Int32 _servo_bm; AP_Int32 _esc_bm; @@ -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); }; From 70e3655a57bf0f93b6bc487cb0ac7832d551c62c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Nov 2019 11:18:50 +1100 Subject: [PATCH 2/3] AP_GPS: adapt for new RTCMStream API for RTK UAVCAN support --- libraries/AP_GPS/AP_GPS.cpp | 7 ------- libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 9 ++------- libraries/AP_GPS/AP_GPS_UAVCAN.h | 3 +-- libraries/AP_GPS/GPS_Backend.h | 4 ---- 4 files changed, 3 insertions(+), 20 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 609ed8f3da..f766314510 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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; instancehandle_rtcm_data(flags, data, len); - } - } - if ((flags & 1) == 0) { // it is not fragmented, pass direct inject_data(data, len); diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 6560540cbd..4bd143a559 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -85,12 +85,7 @@ 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 @@ -98,7 +93,7 @@ void AP_GPS_UAVCAN::handle_rtcm_data(uint8_t flags, const uint8_t *data, uint16_ 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); } } } diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index db674d19b3..a122a36fd1 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -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; diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 0eb68cd76f..66825f04a6 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -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); From 4d48df444754f8684efc781715c36251c35ebed6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Nov 2019 11:24:32 +1100 Subject: [PATCH 3/3] AP_UAVCAN: implement Fix2 for RTK GPS --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 77 ++++++++++++++++--------------- 1 file changed, 39 insertions(+), 38 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index da2f79bfe3..8c54d7bd9c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -16,7 +16,7 @@ #include // Zubax GPS and other GPS, baro, magnetic sensors -#include +#include #include #include @@ -109,7 +109,7 @@ static void uwb_range_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) +static void gnss_fix_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) { AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr); if (ap_uavcan == nullptr) { @@ -123,20 +123,32 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructurestatus = 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; @@ -163,38 +175,27 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructurehave_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; @@ -212,11 +213,11 @@ static void gnss_fix_cb(const uavcan::ReceivedDataStructureupdate_gps_state(msg.getSrcNodeID().get()); } -static void gnss_fix_cb0(const uavcan::ReceivedDataStructure& msg) +static void gnss_fix_cb0(const uavcan::ReceivedDataStructure& msg) { gnss_fix_cb(msg, 0); } -static void gnss_fix_cb1(const uavcan::ReceivedDataStructure& msg) +static void gnss_fix_cb1(const uavcan::ReceivedDataStructure& msg) { gnss_fix_cb(msg, 1); } -static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) +static void (*gnss_fix_cb_arr[2])(const uavcan::ReceivedDataStructure& msg) = { gnss_fix_cb0, gnss_fix_cb1 }; static void gnss_aux_cb(const uavcan::ReceivedDataStructure& msg, uint8_t mgr) @@ -509,8 +510,8 @@ bool AP_UAVCAN::try_init(void) debug_uavcan(1, "UAVCAN: node start problem\n\r"); } - uavcan::Subscriber *gnss_fix; - gnss_fix = new uavcan::Subscriber(*node); + uavcan::Subscriber *gnss_fix; + gnss_fix = new uavcan::Subscriber(*node); uavcan::Subscriber *uwb_range;