diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-07 02:10:09 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-07-07 02:10:09 +0400 |
commit | 324322cb29720dd78b6eb534bb679532d5ed83f2 (patch) | |
tree | f71d24ddf6b8d25cbe0eff722b59603655086bee /src | |
parent | 1492323f0327916435d806c2af1e0c8296278c9d (diff) | |
download | px4-firmware-324322cb29720dd78b6eb534bb679532d5ed83f2.tar.gz px4-firmware-324322cb29720dd78b6eb534bb679532d5ed83f2.tar.bz2 px4-firmware-324322cb29720dd78b6eb534bb679532d5ed83f2.zip |
UAVCAN ESC perf counters
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/uavcan/esc_controller.cpp | 32 | ||||
-rw-r--r-- | src/modules/uavcan/esc_controller.hpp | 8 |
2 files changed, 32 insertions, 8 deletions
diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp index f603d9448..406eba88c 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/esc_controller.cpp @@ -48,6 +48,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : { } +UavcanEscController::~UavcanEscController() +{ + perf_free(_perfcnt_invalid_input); + perf_free(_perfcnt_scaling_error); +} + int UavcanEscController::init() { // ESC status subscription @@ -67,8 +73,10 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - assert(outputs != nullptr); - assert(num_outputs <= MAX_ESCS); + if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + perf_count(_perfcnt_invalid_input); + return; + } /* * Rate limiting - we don't want to congest the bus @@ -89,13 +97,21 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX; - if (scaled < 1.0F) + if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely - - assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN); - assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX); - - msg.cmd.push_back(scaled); + } + + if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MIN; + perf_count(_perfcnt_scaling_error); + } else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) { + scaled = uavcan::equipment::esc::RawCommand::CMD_MAX; + perf_count(_perfcnt_scaling_error); + } else { + ; // Correct value + } + + msg.cmd.push_back(static_cast<unsigned>(scaled)); } } diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp index 0ed0c59b5..559ede561 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/esc_controller.hpp @@ -47,11 +47,13 @@ #include <uavcan/uavcan.hpp> #include <uavcan/equipment/esc/RawCommand.hpp> #include <uavcan/equipment/esc/Status.hpp> +#include <systemlib/perf_counter.h> class UavcanEscController { public: UavcanEscController(uavcan::INode& node); + ~UavcanEscController(); int init(); @@ -96,4 +98,10 @@ private: */ bool _armed = false; uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* + * Perf counters + */ + 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"); }; |