diff --git a/src/modules/uavcan/actuators/hardpoint.cpp b/src/modules/uavcan/actuators/hardpoint.cpp index 828ad90245..86d6d8b425 100644 --- a/src/modules/uavcan/actuators/hardpoint.cpp +++ b/src/modules/uavcan/actuators/hardpoint.cpp @@ -74,14 +74,17 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma * Publish the command message to the bus */ (void)_uavcan_pub_raw_cmd.broadcast(_cmd); + + /* + * Start the periodic update timer after a command is set + */ + if (!_timer.isRunning()) + { + _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); + } } -void UavcanHardpointController::periodic_update() +void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &) { - //lets not broadcast if command is not set - if(_cmd_set == false){ - return; - } - (void)_uavcan_pub_raw_cmd.broadcast(_cmd); //do something } diff --git a/src/modules/uavcan/actuators/hardpoint.hpp b/src/modules/uavcan/actuators/hardpoint.hpp index b245330825..cd21bf95e0 100644 --- a/src/modules/uavcan/actuators/hardpoint.hpp +++ b/src/modules/uavcan/actuators/hardpoint.hpp @@ -66,7 +66,7 @@ private: bool _cmd_set = false; - void UavcanHardpointController::periodic_update(); + void periodic_update(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCbBinder;