Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_UAVCAN: refactor RC Out functions

Browse Source 
- non-functional change
This commit is contained in:
Tom Pittenger 2018-02-08 15:42:58 -08:00 committed by Tom Pittenger
parent 5e55784707
commit 52589f3c22
2 changed files with 98 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -455,23 +455,10 @@ void AP_UAVCAN::rc_out_sem_give()
_rc_out_sem->give();
}
void AP_UAVCAN::do_cyclic(void)
void AP_UAVCAN::rc_out_send_servos(void)
{
if (_initialized) {
auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
if (error < 0) {
hal.scheduler->delay_microseconds(1000);
} else {
if (rc_out_sem_take()) {
if (_rco_armed) {
bool repeat_send;
// if we have any Servos in bitmask
if (_servo_bm > 0) {
uint8_t starting_servo = 0;
bool repeat_send;
do {
repeat_send = false;
@ -516,8 +503,8 @@ void AP_UAVCAN::do_cyclic(void)
} while (repeat_send);
}
// if we have any ESC's in bitmask
if (_esc_bm > 0) {
void AP_UAVCAN::rc_out_send_esc(void)
{
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
uavcan::equipment::esc::RawCommand esc_msg;
@ -558,6 +545,30 @@ void AP_UAVCAN::do_cyclic(void)
esc_raw[_uavcan_i]->broadcast(esc_msg);
}
}
void AP_UAVCAN::do_cyclic(void)
{
if (_initialized) {
auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
if (error < 0) {
hal.scheduler->delay_microseconds(1000);
} else {
if (rc_out_sem_take()) {
if (_rco_armed) {
// if we have any Servos in bitmask
if (_servo_bm > 0) {
rc_out_send_servos();
}
// if we have any ESC's in bitmask
if (_esc_bm > 0) {
rc_out_send_esc();
}
}
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {

4
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -98,6 +98,10 @@ public:
bool rc_out_sem_take();
void rc_out_sem_give();
// output from do_cyclic
void rc_out_send_servos();
void rc_out_send_esc();
private:
// ------------------------- GPS
// 255 - means free node