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 | |
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')
-rw-r--r-- | src/modules/uavcan/actuators/esc.cpp | 27 | ||||
-rw-r--r-- | src/modules/uavcan/actuators/esc.hpp | 8 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/mag.cpp | 2 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/sensor_bridge.cpp | 3 | ||||
-rw-r--r-- | src/modules/uavcan/sensors/sensor_bridge.hpp | 2 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.cpp | 29 | ||||
-rw-r--r-- | src/modules/uavcan/uavcan_main.hpp | 5 |
7 files changed, 65 insertions, 11 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) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e6..12c035542 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,11 @@ private: uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer; /* + * ESC states + */ + uint32_t _armed_mask = 0; + + /* * Perf counters */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f..0d9ea08c5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce680..0999938fc 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast<uint8_t>(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc..e31960537 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _channels(new Channel[MaxChannels]) { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; + _device_id.devid_s.bus = 0; } /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44..2c543462e 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,13 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + 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(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. @@ -384,15 +391,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); } @@ -429,7 +448,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db9741..274321f0d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> +#include <uORB/topics/test_motor.h> #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 |