void AP_UAVCAN::do_cyclic(void)
{
if (!_initialized) {
hal.scheduler->delay_microseconds(1000);
return;
}
auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));
if (error < 0) {
hal.scheduler->delay_microseconds(1000);
return;
}
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++) {
// mark as transmitted
_rco_conf[i].active = false;
}
rc_out_sem_give();
}
}
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) {
bool repeat_send;
// if we have any Servos in bitmask
if (_servo_bm > 0) {
uint8_t starting_servo = 0;
do {
repeat_send = false;
uavcan::equipment::actuator::ArrayCommand msg;
uint8_t i;
// UAVCAN can hold maximum of 15 commands in one frame
for (i = 0; starting_servo < UAVCAN_RCO_NUMBER && i < 15; starting_servo++) {
uavcan::equipment::actuator::Command cmd;
/*
* Servo output uses a range of 1000-2000 PWM for scaling.
* This converts output PWM from [1000:2000] range to [-1:1] range that
* is passed to servo as unitless type via UAVCAN.
* This approach allows for MIN/TRIM/MAX values to be used fully on
* autopilot side and for servo it should have the setup to provide maximum
* physically possible throws at [-1:1] limits.
*/
if (_rco_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
cmd.actuator_id = starting_servo + 1;
// TODO: other types
cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
// TODO: failsafe, safety
cmd.command_value = constrain_float(((float) _rco_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);
msg.commands.push_back(cmd);
i++;
}
}
if (i > 0) {
act_out_array[_uavcan_i]->broadcast(msg);
if (i == 15) {
repeat_send = true;
}
}
} while (repeat_send);
}
// if we have any ESC's in bitmask
if (_esc_bm > 0) {
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
uavcan::equipment::esc::RawCommand esc_msg;
uint8_t active_esc_num = 0, max_esc_num = 0;
uint8_t k = 0;
// find out how many esc we have enabled and if they are active at all
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
if ((((uint32_t) 1) << i) & _esc_bm) {
max_esc_num = i + 1;
if (_rco_conf[i].active) {
active_esc_num++;
}
}
}
// if at least one is active (update) we need to send to all
if (active_esc_num > 0) {
k = 0;
for (uint8_t i = 0; i < max_esc_num && k < 20; i++) {
uavcan::equipment::actuator::Command cmd;
if ((((uint32_t) 1) << i) & _esc_bm) {
// TODO: ESC negative scaling for reverse thrust and reverse rotation
float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_rco_conf[i].pulse) + 1.0) / 2.0;
scaled = constrain_float(scaled, 0, cmd_max);
esc_msg.cmd.push_back(static_cast<int>(scaled));
} else {
esc_msg.cmd.push_back(static_cast<unsigned>(0));
}
k++;
}
esc_raw[_uavcan_i]->broadcast(esc_msg);
}
}
}
for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) {
// mark as transmitted
_rco_conf[i].active = false;
}
rc_out_sem_give();
}
}
} else {
hal.scheduler->delay_microseconds(1000);
}
}
