[WIP] Multicopter Controllers Muxing

.vscode/settings.json

@@ -178,5 +178,6 @@
         "workbench.statusBar.feedback.visible": false,
         "yaml.schemas": {
                 "${workspaceRoot}/validation/module_schema.yaml": "${workspaceRoot}/src/modules/*/module.yaml"
-        }
+        },
+        "C_Cpp.errorSquiggles": "Disabled"
 }

msg/CMakeLists.txt

@@ -84,6 +84,7 @@ set(msg_files
 	mission_result.msg
 	mount_orientation.msg
 	multirotor_motor_limits.msg
+	mc_ctl_chain.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg

msg/mc_ctl_chain.msg

@@ -0,0 +1,20 @@
+# Multicopter Control Chain: used to select which Position, Attitude or Rate controller will be used during the mission
+# Paired with mavlink chain
+uint64 timestamp;
+
+uint8 MC_DISABLE_CONTROLLER = 0
+uint8 MC_DEFAULT_CONTROLLER = 1
+
+# Set of custom controllers
+# Position controllers: 1xx
+uint8 MC_POS_CUSTOM_CONTROLLER = 100
+
+# Attitude controllers: 2xx
+uint8 MC_POS_CUSTOM_CONTROLLER = 200
+
+# Rate controllers:	3xx
+uint8 MC_POS_CUSTOM_CONTROLLER = 300
+
+uint8 pos_ctl;
+uint8 att_ctl;
+uint8 rate_ctl;

src/modules/mavlink/mavlink_receiver.cpp

@@ -141,6 +141,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_set_mode(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_MC_CONTROL_CHAIN: // TODO : create mavlink msg type
+		handle_message_set_mc_control_chain(msg):
+		break;
+
 	case MAVLINK_MSG_ID_ATT_POS_MOCAP:
 		handle_message_att_pos_mocap(msg);
 		break;
@@ -733,6 +737,21 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	_cmd_pub.publish(vcmd);
 }
 
+void
+MavlinkReceiver::handle_message_set_mc_control_chain(mavlink_message_t *msg)
+{
+	mavlink_mc_ctl_chain_t ctl_chain;
+	mavlink_msg_mc_ctl_chain_decode(msg, &ctl_chain);
+
+	mc_ctl_chain_s mcc{}; // TODO create structure
+
+	mcc.timestamp	= hrt_absolute_time();
+	mcc.pos_ctl	= ctl_chain.pos_ctl;
+	mcc.att_ctl	= ctl_chain.att_ctl;
+	mcc.rate_ctl	= ctl_chain.rate_ctl;
+
+	_mc_ctl_chain_pub.publish(mcc);
+}
 void
 MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -75,6 +75,7 @@
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/log_message.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/mc_ctl_chain.h>
 #include <uORB/topics/obstacle_distance.h>
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
@@ -164,6 +165,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
+	void handle_message_distance_sensor(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
 	void handle_message_set_position_target_global_int(mavlink_message_t *msg);
 	void handle_message_statustext(mavlink_message_t *msg);
@@ -250,7 +252,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Publication<vehicle_odometry_s>			_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Publication<vehicle_odometry_s>			_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
-	uORB::Publication<vehicle_trajectory_bezier_s>	_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
+	uORB::Publication<vehicle_trajectory_bezier_s>		_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
 
 	// ORB publications (multi)
@@ -260,6 +262,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
+	uORB::PublicationMulti<mc_ctl_chain_s>			_mc_ctl_chain_pub{ORB_ID(mc_ctl_chain), ORB_PRIO_LOW};
 
 	// ORB publications (queue length > 1)
 	uORB::PublicationQueued<gps_inject_data_s>	_gps_inject_data_pub{ORB_ID(gps_inject_data)};

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -72,6 +72,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
 	_v_att_sp.q_d[0] = 1.f;
 
 	parameters_updated();
+
+	// get desired control and mux it
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()