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

AP_UAVCAN: added buzzer support

This commit is contained in:
Andrew Tridgell 2019-08-31 12:45:02 +10:00
parent f84dac85ea
commit d55f76c88e
2 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -34,6 +34,7 @@
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uavcan/equipment/indication/RGB565.hpp>
#include <AP_Baro/AP_Baro_UAVCAN.h>
@ -98,6 +99,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
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<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
AP_UAVCAN::AP_UAVCAN() :
_node_allocator()
@ -222,6 +224,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
buzzer[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::BeepCommand>(*_node);
buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
_led_conf.devices_count = 0;
if (enable_filters) {
configureCanAcceptanceFilters(*_node);
@ -291,6 +297,7 @@ void AP_UAVCAN::loop(void)
}
led_out_send();
buzzer_send();
}
}
@ -474,4 +481,28 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
return true;
}
// buzzer send
void AP_UAVCAN::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[_driver_index]->broadcast(msg);
}
// buzzer support
void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s)
{
WITH_SEMAPHORE(_buzzer.sem);
_buzzer.frequency = frequency;
_buzzer.duration = duration_s;
_buzzer.pending_mask = 0xFF;
}
#endif // HAL_WITH_UAVCAN

13
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -81,6 +81,8 @@ public:
///// LED /////
bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
// buzzer
void set_buzzer_tone(float frequency, float duration_s);
template <typename DataType_>
class RegistryBinder {
@ -147,6 +149,9 @@ private:
///// LED /////
void led_out_send();
// buzzer
void buzzer_send();
uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
// UAVCAN parameters
@ -190,6 +195,14 @@ private:
} _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;
};
#endif /* AP_UAVCAN_H_ */