diff --git a/src/modules/uavcan/actuators/hardpoint.cpp b/src/modules/uavcan/actuators/hardpoint.cpp index c419790ecb..ea1c165309 100644 --- a/src/modules/uavcan/actuators/hardpoint.cpp +++ b/src/modules/uavcan/actuators/hardpoint.cpp @@ -60,27 +60,15 @@ int UavcanHardpointController::init() * Setup timer and call back function for periodic updates */ _timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update)); - _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); return 0; } void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command) { + (void)pthread_mutex_lock(&_node_mutex); _cmd.command = command; _cmd.hardpoint_id = hardpoint_id; - _cmd_set = true; - - /* - * Rate limiting - we don't want to congest the bus - */ - const auto timestamp = _node.getMonotonicTime(); - - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - - _prev_cmd_pub = timestamp; /* * Publish the command message to the bus @@ -93,6 +81,7 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma if (!_timer.isRunning()) { _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); } + (void)pthread_mutex_lock(&_node_mutex); } void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &) { diff --git a/src/modules/uavcan/actuators/hardpoint.hpp b/src/modules/uavcan/actuators/hardpoint.hpp index 770aa9ba3c..0435897f26 100644 --- a/src/modules/uavcan/actuators/hardpoint.hpp +++ b/src/modules/uavcan/actuators/hardpoint.hpp @@ -73,13 +73,12 @@ private: uavcan::equipment::hardpoint::Command _cmd; - bool _cmd_set = false; - void periodic_update(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCbBinder; + pthread_mutex_t _node_mutex; /* * libuavcan related things */