From a65533b46986e32254b64b7c92469afb8178e370 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 13 Sep 2021 14:54:32 +0200 Subject: [PATCH] mixer_module: add output functions for servos, landing gear, parachute, RC passthrough & gimbal --- msg/manual_control_setpoint.msg | 10 +-- src/lib/mixer_module/functions.cpp | 59 +++++++++++++ src/lib/mixer_module/functions.hpp | 98 ++++++++++++++++++++++ src/lib/mixer_module/mixer_module.cpp | 5 ++ src/lib/mixer_module/output_functions.yaml | 17 ++++ 5 files changed, 184 insertions(+), 5 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 16abfedb26..35ad76f70b 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -40,10 +40,10 @@ float32 r # yaw stick/twist position, -1..1 float32 flaps # flap position -float32 aux1 # default function: camera yaw / azimuth -float32 aux2 # default function: camera pitch / tilt -float32 aux3 # default function: camera trigger -float32 aux4 # default function: camera roll -float32 aux5 # default function: payload drop +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 float32 aux6 diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp index 6e83289b02..73d39672aa 100644 --- a/src/lib/mixer_module/functions.cpp +++ b/src/lib/mixer_module/functions.cpp @@ -58,6 +58,14 @@ void FunctionMotors::update() } } +FunctionServos::FunctionServos(const Context &context) + : _topic(&context.work_item, ORB_ID(actuator_servos)) +{ + for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) { + _data.control[i] = NAN; + } +} + FunctionActuatorSet::FunctionActuatorSet() { for (int i = 0; i < max_num_actuators; ++i) { @@ -84,3 +92,54 @@ void FunctionActuatorSet::update() } } } + +void FunctionLandingGear::update() +{ + landing_gear_s landing_gear; + + if (_topic.update(&landing_gear)) { + if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) { + _data = -1.f; + + } else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) { + _data = 1.f; + } + } +} + +FunctionManualRC::FunctionManualRC() +{ + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } +} + +void FunctionManualRC::update() +{ + manual_control_setpoint_s manual_control_setpoint; + + if (_topic.update(&manual_control_setpoint)) { + _data[0] = manual_control_setpoint.y; // roll + _data[1] = manual_control_setpoint.x; // pitch + _data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle + _data[3] = manual_control_setpoint.r; // yaw + _data[4] = manual_control_setpoint.flaps; + _data[5] = manual_control_setpoint.aux1; + _data[6] = manual_control_setpoint.aux2; + _data[7] = manual_control_setpoint.aux3; + _data[8] = manual_control_setpoint.aux4; + _data[9] = manual_control_setpoint.aux5; + _data[10] = manual_control_setpoint.aux6; + } +} + +void FunctionGimbal::update() +{ + actuator_controls_s actuator_controls; + + if (_topic.update(&actuator_controls)) { + _data[0] = actuator_controls.control[0]; + _data[1] = actuator_controls.control[1]; + _data[2] = actuator_controls.control[2]; + } +} diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 4d6712ea23..1285fd75f1 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/src/lib/mixer_module/functions.hpp @@ -39,9 +39,12 @@ #include #include +#include #include #include #include +#include +#include #include class FunctionProviderBase @@ -125,6 +128,30 @@ private: const bool _reversible_motors; }; +/** + * Functions: Servo1 ... ServoMax + */ +class FunctionServos : public FunctionProviderBase +{ +public: + static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1, + "Unexpected num servos"); + + FunctionServos(const Context &context); + static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); } + + void update() override { _topic.update(&_data); } + float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; } + + uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; } + + float defaultFailsafeValue(OutputFunction func) const override { return 0.f; } +private: + uORB::SubscriptionCallbackWorkItem _topic; + actuator_servos_s _data{}; +}; + + /** * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6 */ @@ -143,3 +170,74 @@ private: uORB::Subscription _topic{ORB_ID(vehicle_command)}; float _data[max_num_actuators]; }; + +/** + * Functions: Landing_Gear + */ +class FunctionLandingGear : public FunctionProviderBase +{ +public: + FunctionLandingGear() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); } + + void update() override; + float value(OutputFunction func) override { return _data; } + +private: + uORB::Subscription _topic{ORB_ID(landing_gear)}; + float _data{-1.f}; +}; + +/** + * Functions: Parachute + */ +class FunctionParachute : public FunctionProviderBase +{ +public: + FunctionParachute() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); } + + void update() override {} + float value(OutputFunction func) override { return -1.f; } + float defaultFailsafeValue(OutputFunction func) const override { return 1.f; } +}; + +/** + * Functions: RC_Roll .. RCAUX_Max + */ +class FunctionManualRC : public FunctionProviderBase +{ +public: + FunctionManualRC(); + static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } + + void update() override; + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; } + +private: + static constexpr int num_data_points = 11; + + static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, + "number of functions mismatch"); + + uORB::Subscription _topic{ORB_ID(manual_control_setpoint)}; + float _data[num_data_points]; +}; + +/** + * Functions: Gimbal_Roll .. Gimbal_Yaw + */ +class FunctionGimbal : public FunctionProviderBase +{ +public: + FunctionGimbal() = default; + static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); } + + void update() override; + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; } + +private: + uORB::Subscription _topic{ORB_ID(actuator_controls_2)}; + float _data[3] { NAN, NAN, NAN }; +}; + diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 573fc4b96b..b0cdea7d21 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -58,7 +58,12 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Constant_Min, &FunctionConstantMin::allocate}, {OutputFunction::Constant_Max, &FunctionConstantMax::allocate}, {OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate}, + {OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate}, {OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate}, + {OutputFunction::Landing_Gear, &FunctionLandingGear::allocate}, + {OutputFunction::Parachute, &FunctionParachute::allocate}, + {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, + {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, }; MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index 33f3561f4c..4e4cfec1a5 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -17,6 +17,23 @@ functions: start: 301 count: 6 + Landing_Gear: 400 + Parachute: 401 + + # Manual RC input (passthrough) + RC_Roll: 402 + RC_Pitch: 403 + RC_Throttle: 404 + RC_Yaw: 405 + RC_Flaps: 406 + RC_AUX: + start: 407 + count: 6 + + Gimbal_Roll: 420 + Gimbal_Pitch: 421 + Gimbal_Yaw: 422 + # Add your own here: #MyCustomFunction: 10000