Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: move dronecan led, buzzer and rtcm stream to their respective drivers

This commit is contained in:
bugobliterator 2023-05-02 16:02:46 +10:00 committed by Andrew Tridgell
parent 3fd2865857
commit a277547248
2 changed files with 23 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -312,8 +312,6 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
node_info_server.set_timeout_ms(20);
_led_conf.devices_count = 0;
// setup node status
node_status_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK;
node_status_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL;
@ -346,6 +344,24 @@ void AP_DroneCAN::loop(void)
canard_iface.process(1);
safety_state_send();
notify_state_send();
send_parameter_request();
send_parameter_save_request();
send_node_status();
_dna_server.verify_nodes();
#if AP_DRONECAN_SEND_GPS
if (option_is_set(AP_DroneCAN::Options::SEND_GNSS) && !AP_GPS_DroneCAN::instance_exists(this)) {
// send if enabled and this interface/driver is not used by the AP_GPS driver
gnss_send_fix();
gnss_send_yaw();
}
#endif
logging();
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
hobbywing_ESC_update();
#endif
if (_SRV_armed) {
bool sent_servos = false;
@ -383,32 +399,6 @@ void AP_DroneCAN::loop(void)
_SRV_conf[i].esc_pending = false;
}
}
led_out_send();
buzzer_send();
rtcm_stream_send();
safety_state_send();
notify_state_send();
send_parameter_request();
send_parameter_save_request();
send_node_status();
_dna_server.verify_nodes();
#if AP_OPENDRONEID_ENABLED
AP::opendroneid().dronecan_send(this);
#endif
#if AP_DRONECAN_SEND_GPS
if (option_is_set(AP_DroneCAN::Options::SEND_GNSS) && !AP_GPS_DroneCAN::instance_exists(this)) {
// send if enabled and this interface/driver is not used by the AP_GPS driver
gnss_send_fix();
gnss_send_yaw();
}
#endif
logging();
#if AP_DRONECAN_HOBBYWING_ESC_SUPPORT
hobbywing_ESC_update();
#endif
}
}
@ -720,95 +710,6 @@ void AP_DroneCAN::SRV_push_servos()
_SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
}
///// LED /////
void AP_DroneCAN::led_out_send()
{
uint64_t now = AP_HAL::native_micros64();
if ((now - _led_conf.last_update) < LED_DELAY_US) {
return;
}
uavcan_equipment_indication_LightsCommand msg;
{
WITH_SEMAPHORE(_led_out_sem);
if (_led_conf.devices_count == 0) {
return;
}
msg.commands.len = _led_conf.devices_count;
for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
msg.commands.data[i].light_id =_led_conf.devices[i].led_index;
msg.commands.data[i].color.red = _led_conf.devices[i].red >> 3;
msg.commands.data[i].color.green = _led_conf.devices[i].green >> 2;
msg.commands.data[i].color.blue = _led_conf.devices[i].blue >> 3;
}
}
rgb_led.broadcast(msg);
_led_conf.last_update = now;
}
bool AP_DroneCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue)
{
if (_led_conf.devices_count >= AP_DRONECAN_MAX_LED_DEVICES) {
return false;
}
WITH_SEMAPHORE(_led_out_sem);
// check if a device instance exists. if so, break so the instance index is remembered
uint8_t instance = 0;
for (; instance < _led_conf.devices_count; instance++) {
if (_led_conf.devices[instance].led_index == led_index) {
break;
}
}
// load into the correct instance.
// if an existing instance was found in above for loop search,
// then instance value is < _led_conf.devices_count.
// otherwise a new one was just found so we increment the count.
// Either way, the correct instance is the current value of instance
_led_conf.devices[instance].led_index = led_index;
_led_conf.devices[instance].red = red;
_led_conf.devices[instance].green = green;
_led_conf.devices[instance].blue = blue;
if (instance == _led_conf.devices_count) {
_led_conf.devices_count++;
}
return true;
}
// buzzer send
void AP_DroneCAN::buzzer_send()
{
uavcan_equipment_indication_BeepCommand msg;
WITH_SEMAPHORE(_buzzer.sem);
uint8_t mask = (1U << _driver_index);
if ((_buzzer.pending_mask & mask) == 0) {
return;
}
_buzzer.pending_mask &= ~mask;
msg.frequency = _buzzer.frequency;
msg.duration = _buzzer.duration;
buzzer.broadcast(msg);
}
// buzzer support
void AP_DroneCAN::set_buzzer_tone(float frequency, float duration_s)
{
WITH_SEMAPHORE(_buzzer.sem);
_buzzer.frequency = frequency;
_buzzer.duration = duration_s;
_buzzer.pending_mask = 0xFF;
}
// notify state send
void AP_DroneCAN::notify_state_send()
{
@ -1047,38 +948,6 @@ void AP_DroneCAN::gnss_send_yaw()
}
#endif // AP_DRONECAN_SEND_GPS
void AP_DroneCAN::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::native_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.data[i] = b;
}
msg.data.len = len;
rtcm_stream.broadcast(msg);
}
// SafetyState send
void AP_DroneCAN::safety_state_send()
{
@ -1114,23 +983,6 @@ void AP_DroneCAN::safety_state_send()
}
}
/*
send RTCMStream packet on all active DroneCAN drivers
*/
void AP_DroneCAN::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
*/

48
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -131,6 +131,10 @@ public:
CanardInterface& get_canard_iface() { return canard_iface; }
Canard::Publisher<uavcan_equipment_indication_LightsCommand> rgb_led{canard_iface};
Canard::Publisher<uavcan_equipment_indication_BeepCommand> buzzer{canard_iface};
Canard::Publisher<uavcan_equipment_gnss_RTCMStream> rtcm_stream{canard_iface};
private:
void loop(void);
@ -139,21 +143,12 @@ private:
void SRV_send_esc();
void SRV_send_himark();
///// LED /////
void led_out_send();
// buzzer
void buzzer_send();
// SafetyState
void safety_state_send();
// send notify vehicle state
void notify_state_send();
// send GNSS injection
void rtcm_stream_send();
// send parameter get/set request
void send_parameter_request();
@ -212,30 +207,6 @@ private:
// last log time
uint32_t last_log_ms;
///// LED /////
struct led_device {
uint8_t led_index;
uint8_t red;
uint8_t green;
uint8_t blue;
};
struct {
led_device devices[AP_DRONECAN_MAX_LED_DEVICES];
uint8_t devices_count;
uint64_t last_update;
} _led_conf;
HAL_Semaphore _led_out_sem;
// buzzer
struct {
HAL_Semaphore sem;
float frequency;
float duration;
uint8_t pending_mask; // mask of interfaces to send to
} _buzzer;
#if AP_DRONECAN_SEND_GPS
// send GNSS Fix and yaw, same thing AP_GPS_DroneCAN would receive
void gnss_send_fix();
@ -248,13 +219,6 @@ private:
uint32_t last_lib_yaw_time_ms;
} _gnss;
#endif
// GNSS RTCM injection
struct {
HAL_Semaphore sem;
uint32_t last_send_ms;
ByteBuffer *buf;
} _rtcm_stream;
static HAL_Semaphore _telem_sem;
@ -269,14 +233,12 @@ private:
uavcan_protocol_NodeStatus node_status_msg;
CanardInterface canard_iface;
Canard::Publisher<uavcan_protocol_NodeStatus> node_status{canard_iface};
Canard::Publisher<uavcan_equipment_actuator_ArrayCommand> act_out_array{canard_iface};
Canard::Publisher<uavcan_equipment_esc_RawCommand> esc_raw{canard_iface};
Canard::Publisher<uavcan_equipment_indication_LightsCommand> rgb_led{canard_iface};
Canard::Publisher<uavcan_equipment_indication_BeepCommand> buzzer{canard_iface};
Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
Canard::Publisher<uavcan_equipment_gnss_RTCMStream> rtcm_stream{canard_iface};
Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
Canard::Publisher<com_himark_servo_ServoCmd> himark_out{canard_iface};