diff options
| -rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 14 | ||||
| -rw-r--r-- | src/modules/uavcan/actuators/esc.hpp | 1 |
2 files changed, 15 insertions, 0 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e1..995c8987c 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,12 +49,24 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } + + if (_perfcnt_broadcast_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); + } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); + perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -129,7 +141,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ + perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); + perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c035542..498fb9dd8 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,4 +109,5 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); + perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; |