diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 5ce98b39c1..cd9ff51b70 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -132,6 +132,16 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA _uavcan_pub_raw_cmd.broadcast(msg); } +void +UavcanEscController::set_rotor_count(uint8_t count) +{ + _rotor_count = count; + + if (_rotor_count != 0u) { + _orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ)); + } +} + void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index ac40bf989c..1783f7fa4b 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -72,9 +72,9 @@ public: void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs); /** - * Sets the number of rotors + * Sets the number of rotors and enable timer */ - void set_rotor_count(uint8_t count) { _rotor_count = count; } + void set_rotor_count(uint8_t count); static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }