From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 14 Aug 2014 15:43:27 +0200 Subject: [PATCH 1/3] UAVCAN: allow to arm single ESCs --- src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++----- src/modules/uavcan/actuators/esc.hpp | 9 ++++++++- 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 1990651ef2..fbd4f0bcd4 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include + +#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(scaled)); } + else { + msg.cmd.push_back(static_cast(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) { - _armed = arm; + if (arm) + _armed_mask = -1; + else + _armed_mask = 0; +} + +void UavcanEscController::arm_single_esc(int num, bool arm) +{ + if (arm) + _armed_mask = MOTOR_BIT(num); + else + _armed_mask = 0; } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e62..ff3ecfb215 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: /** @@ -98,6 +99,12 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + /* + * ESC states + */ + uint32_t _armed_mask = 0; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + /* * Perf counters */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44e..59f0f43004 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; } From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 18:47:07 +0100 Subject: [PATCH 2/3] UAVCAN: implemented motor testing --- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 28 ++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 5 +++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index ff3ecfb215..12c0355422 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -103,7 +103,6 @@ private: * ESC states */ uint32_t _armed_mask = 0; - uavcan::equipment::esc::Status _states[MAX_ESCS]; /* * Perf counters diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 59f0f43004..653d4f98cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -279,6 +279,7 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -344,7 +345,14 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + float outputs[NUM_ACTUATOR_OUTPUTS] = {}; + outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + + // Output to the bus + _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); + } + else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +392,27 @@ int UavcanNode::run() } } - // Check arming state + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db97411..274321f0db 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -103,6 +104,10 @@ private: actuator_armed_s _armed; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus + int _test_motor_sub = -1; ///< uORB subscription of the test_motor status + test_motor_s _test_motor; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer From 7bc9a6257731a741ff309f060f6cf87c33c4a266 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 09:52:35 +0100 Subject: [PATCH 3/3] code style, warnings --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++------ src/modules/uavcan/uavcan_main.cpp | 9 ++++----- src/systemcmds/motor_test/motor_test.c | 19 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index fbd4f0bcd4..1d23099f3d 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -113,8 +113,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); - } - else { + } else { msg.cmd.push_back(static_cast(0)); } } @@ -128,18 +127,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) void UavcanEscController::arm_all_escs(bool arm) { - if (arm) + if (arm) { _armed_mask = -1; - else + } else { _armed_mask = 0; + } } void UavcanEscController::arm_single_esc(int num, bool arm) { - if (arm) + if (arm) { _armed_mask = MOTOR_BIT(num); - else + } else { _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 653d4f98cf..2c543462ec 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -346,13 +346,12 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { - float outputs[NUM_ACTUATOR_OUTPUTS] = {}; - outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; + test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; // Output to the bus - _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); - } - else if (controls_updated && (_mixers != nullptr)) { + _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674a..087699b05d 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -67,19 +67,15 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); } static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } errx(1, "usage:\n" @@ -90,8 +86,9 @@ static void usage(const char *reason) int motor_test_main(int argc, char *argv[]) { - unsigned long channel, lval; - float value; + unsigned long channel = 0; + unsigned long lval; + float value = 0.0f; int ch; if (argc != 5) { @@ -122,7 +119,7 @@ int motor_test_main(int argc, char *argv[]) motor_test(channel, value); - printf("motor %d set to %.2f\n", channel, value); + printf("motor %d set to %.2f\n", channel, (double)value); exit(0); }