diff options
author | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-20 17:26:27 +0100 |
---|---|---|
committer | philipoe <philipp.oettershagen@mavt.ethz.ch> | 2014-11-20 17:26:27 +0100 |
commit | ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f (patch) | |
tree | b5c5f7d4781281a8209a5580a5bb3e1268a7e6b5 /src/modules/uavcan/actuators/esc.cpp | |
parent | 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 (diff) | |
parent | 97a1410ec99e880207e4ee6d2a03451c2e11f4cf (diff) | |
download | px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.tar.gz px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.tar.bz2 px4-firmware-ec165b3f7ec5ca1d47fa1f8fcba72e8c1c72654f.zip |
Merge remote-tracking branch 'upstream/master' into PR_RCLossDur2
Diffstat (limited to 'src/modules/uavcan/actuators/esc.cpp')
-rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 27 |
1 files changed, 22 insertions, 5 deletions
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef..1d23099f3 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 @@ -111,6 +113,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast<int>(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled)); + } else { + msg.cmd.push_back(static_cast<unsigned>(0)); } } @@ -121,9 +125,22 @@ 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) |