mixer_module: add output functions for servos, landing gear, parachute, RC passthrough & gimbal

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
 

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];
+	}
+}

src/lib/mixer_module/functions.hpp

@@ -39,9 +39,12 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/actuator_servos.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 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 };
+};
+

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,

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