Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: support RTCMStream for RTCM injection

Browse Source
This commit is contained in:
Andrew Tridgell 2019-11-17 11:58:52 +11:00
parent 0080f3c011
commit 9035195fca
2 changed files with 68 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -38,6 +38,7 @@
#include <uavcan/equipment/indication/RGB565.hpp>
#include <ardupilot/indication/SafetyState.hpp>
#include <ardupilot/indication/Button.hpp>
#include <uavcan/equipment/gnss/RTCMStream.hpp>
#include <AP_Baro/AP_Baro_UAVCAN.h>
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@ -103,6 +104,7 @@ static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS];
// subscribers
// handler SafteyButton
@ -240,6 +242,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
rtcm_stream[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
if (safety_button_listener[driver_index]) {
safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@ -315,6 +321,7 @@ void AP_UAVCAN::loop(void)
led_out_send();
buzzer_send();
rtcm_stream_send();
safety_state_send();
}
}
@ -523,6 +530,36 @@ void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s)
_buzzer.pending_mask = 0xFF;
}
void AP_UAVCAN::rtcm_stream_send()
{
WITH_SEMAPHORE(_rtcm_stream.sem);
if (_rtcm_stream.buf == nullptr ||
_rtcm_stream.buf->available() == 0) {
// nothing to send
return;
}
uint32_t now = AP_HAL::millis();
if (now - _rtcm_stream.last_send_ms < 20) {
// don't send more than 50 per second
return;
}
_rtcm_stream.last_send_ms = now;
uavcan::equipment::gnss::RTCMStream msg;
uint32_t len = _rtcm_stream.buf->available();
if (len > 128) {
len = 128;
}
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)) {
return;
}
msg.data.push_back(b);
}
rtcm_stream[_driver_index]->broadcast(msg);
}
// SafetyState send
void AP_UAVCAN::safety_state_send()
{
@ -547,6 +584,23 @@ void AP_UAVCAN::safety_state_send()
safety_state[_driver_index]->broadcast(msg);
}
/*
send RTCMStream packet on all active UAVCAN drivers
*/
void AP_UAVCAN::send_RTCMStream(const uint8_t *data, uint32_t len)
{
WITH_SEMAPHORE(_rtcm_stream.sem);
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) {
return;
}
_rtcm_stream.buf->write(data, len);
}
/*
handle Button message
*/

15
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -87,6 +87,9 @@ public:
// buzzer
void set_buzzer_tone(float frequency, float duration_s);
// send RTCMStream packets
void send_RTCMStream(const uint8_t *data, uint32_t len);
template <typename DataType_>
class RegistryBinder {
protected:
@ -157,7 +160,10 @@ private:
// SafetyState
void safety_state_send();
// send GNSS injection
void rtcm_stream_send();
uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
// UAVCAN parameters
@ -210,6 +216,13 @@ private:
uint8_t pending_mask; // mask of interfaces to send to
} _buzzer;
// GNSS RTCM injection
struct {
HAL_Semaphore sem;
uint32_t last_send_ms;
ByteBuffer *buf;
} _rtcm_stream;
// safety status send state
uint32_t _last_safety_state_ms;