diff options
author | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-10-12 20:56:45 +0400 |
---|---|---|
committer | Pavel Kirienko <pavel.kirienko@gmail.com> | 2014-10-12 20:56:45 +0400 |
commit | e5a77a638a53aa9baec2ffe9d8ad96fb095b0966 (patch) | |
tree | 433f687417ddc46adf36e7c59a25ea0b34cc1695 /src | |
parent | ced75deebe527581930419155fb0d73f18b7a5fe (diff) | |
download | px4-firmware-e5a77a638a53aa9baec2ffe9d8ad96fb095b0966.tar.gz px4-firmware-e5a77a638a53aa9baec2ffe9d8ad96fb095b0966.tar.bz2 px4-firmware-e5a77a638a53aa9baec2ffe9d8ad96fb095b0966.zip |
ESC status feedback
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 29 | ||||
-rw-r--r-- | src/modules/uavcan/actuators/esc.hpp | 15 |
2 files changed, 33 insertions, 11 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index b516536c7..0601d9fa2 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -73,7 +73,7 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) { + if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { perf_count(_perfcnt_invalid_input); return; } @@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) { - // TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb() + if (msg.esc_index < CONNECTED_ESC_MAX) { + _esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1); + _esc_status.timestamp = msg.getMonotonicTimestamp().toUSec(); + + auto &ref = _esc_status.esc[msg.esc_index]; + + ref.esc_address = msg.getSrcNodeID().get(); + + // >0 checks allow to weed out NaNs and negative values that aren't supported. + ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0; + ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0; + ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0; + + ref.esc_setpoint = msg.power_rating_pct; + ref.esc_rpm = abs(msg.rpm); + ref.esc_errorcount = msg.error_count; + } } void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&) { - // TODO publish to ORB + _esc_status.counter += 1; + _esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN; + + if (_esc_status_pub > 0) { + (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); + } else { + _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); + } } diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 0add7e0ad..f4a3877e6 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -48,6 +48,8 @@ #include <uavcan/equipment/esc/RawCommand.hpp> #include <uavcan/equipment/esc/Status.hpp> #include <systemlib/perf_counter.h> +#include <uORB/topics/esc_status.h> + class UavcanEscController { @@ -74,8 +76,7 @@ private: static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable - static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5; - static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize; + static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> @@ -84,6 +85,10 @@ private: typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)> TimerCbBinder; + bool _armed = false; + esc_status_s _esc_status = {}; + orb_advert_t _esc_status_pub = -1; + /* * libuavcan related things */ @@ -94,12 +99,6 @@ private: uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; /* - * ESC states - */ - 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"); |