diff options
author | Holger Steinhaus <holger@steinhaus-home.de> | 2014-08-14 15:43:27 +0200 |
---|---|---|
committer | Holger Steinhaus <holger@steinhaus-home.de> | 2014-11-03 18:51:43 +0100 |
commit | 58f36714f808c9d06527da1952d348201c4f7e72 (patch) | |
tree | 681869ff16fb9e5909c42f143b39813ab77c4b1c | |
parent | 1cab08cc2aba8b256c21729d45eb7dd5802a0728 (diff) | |
download | px4-firmware-58f36714f808c9d06527da1952d348201c4f7e72.tar.gz px4-firmware-58f36714f808c9d06527da1952d348201c4f7e72.tar.bz2 px4-firmware-58f36714f808c9d06527da1952d348201c4f7e72.zip |
UAVCAN: allow to arm single ESCs
-rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 26 | ||||
-rw-r--r-- | src/modules/uavcan/actuators/esc.hpp | 9 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 2 |
3 files changed, 30 insertions, 7 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef..fbd4f0bcd 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include <systemlib/err.h> + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + for (unsigned i = 0; i < num_outputs; i++) { + if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; if (scaled < 1.0F) { scaled = 1.0F; // Since we're armed, we don't want to stop it completely @@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) _esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled)); } + else { + msg.cmd.push_back(static_cast<unsigned>(0)); + } } /* @@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) +{ + if (arm) + _armed_mask = -1; + else + _armed_mask = 0; +} + +void UavcanEscController::arm_single_esc(int num, bool arm) { - _armed = arm; + if (arm) + _armed_mask = MOTOR_BIT(num); + else + _armed_mask = 0; } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e6..ff3ecfb21 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -99,6 +100,12 @@ private: uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; /* + * ESC states + */ + uint32_t _armed_mask = 0; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + + /* * Perf counters */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..59f0f4300 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -429,7 +429,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } |