remove the landing gear from the attitude setpoint

- An new message is created just for the landing gear
- The old logic is repalaced by publishing this new topic

msg/CMakeLists.txt

@@ -66,6 +66,7 @@ set(msg_files
 	input_rc.msg
 	iridiumsbd_status.msg
 	irlock_report.msg
+	landing_gear.msg
 	landing_target_innovations.msg
 	landing_target_pose.msg
 	led_control.msg

msg/landing_gear.msg

@@ -0,0 +1,6 @@
+uint64 timestamp            # time since system start (microseconds)
+
+int8 LANDING_GEAR_UP = 1	# landing gear up
+int8 LANDING_GEAR_DOWN = -1	# landing gear down
+
+float32 landing_gear

msg/vehicle_attitude_setpoint.msg

@@ -1,6 +1,4 @@
 uint64 timestamp		# time since system start (microseconds)
-int8 LANDING_GEAR_UP = 1	# landing gear up
-int8 LANDING_GEAR_DOWN = -1	# landing gear down
 
 float32 roll_body		# body angle in NED frame (can be NaN for FW)
 float32 pitch_body		# body angle in NED frame (can be NaN for FW)
@@ -27,6 +25,4 @@ uint8 FLAPS_OFF = 0     	# no flaps
 uint8 FLAPS_LAND = 1    	# landing config flaps
 uint8 FLAPS_TAKEOFF = 2 	# take-off config flaps
 
-float32 landing_gear
-
 # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint

src/modules/mc_att_control/mc_att_control.hpp

@@ -56,6 +56,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/landing_gear.h>
 
 /**
  * Multicopter attitude control app start / stop handling function
@@ -109,6 +110,7 @@ private:
 	void		vehicle_motor_limits_poll();
 	bool		vehicle_rates_setpoint_poll();
 	void		vehicle_status_poll();
+	void 		landing_gear_state_poll();
 
 	void		publish_actuator_controls();
 	void		publish_rates_setpoint();
@@ -157,6 +159,7 @@ private:
 	int		_sensor_correction_sub{-1};	/**< sensor thermal correction subscription */
 	int		_sensor_bias_sub{-1};		/**< sensor in-run bias correction subscription */
 	int		_vehicle_land_detected_sub{-1};	/**< vehicle land detected subscription */
+	int		_landing_gear_sub{-1};
 
 	unsigned _gyro_count{1};
 	int _selected_gyro{0};
@@ -165,6 +168,7 @@ private:
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
 	orb_advert_t	_controller_status_pub{nullptr};	/**< controller status publication */
 	orb_advert_t	_vehicle_attitude_setpoint_pub{nullptr};
+	orb_advert_t	_landing_gear_pub{nullptr};
 
 	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
 
@@ -182,6 +186,7 @@ private:
 	struct sensor_correction_s		_sensor_correction {};	/**< sensor thermal corrections */
 	struct sensor_bias_s			_sensor_bias {};	/**< sensor in-run bias corrections */
 	struct vehicle_land_detected_s		_vehicle_land_detected {};
+	struct landing_gear_s 			_landing_gear_state {};
 
 	MultirotorMixer::saturation_status _saturation_status{};
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -376,6 +376,17 @@ MulticopterAttitudeControl::vehicle_land_detected_poll()
 
 }
 
+void
+MulticopterAttitudeControl::landing_gear_state_poll()
+{
+	bool updated;
+	orb_check(_landing_gear_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(landing_gear), _landing_gear_sub, &_landing_gear_state);
+	}
+}
+
 float
 MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
@@ -404,9 +415,9 @@ MulticopterAttitudeControl::get_landing_gear_state()
 	if (_vehicle_land_detected.landed) {
 		_gear_state_initialized = false;
 	}
-	float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down
+	float landing_gear = landing_gear_s::LANDING_GEAR_DOWN; // default to down
 	if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
-		landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
+		landing_gear = landing_gear_s::LANDING_GEAR_UP;
 
 	} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
 		// Switching the gear off does put it into a safe defined state
@@ -420,6 +431,7 @@ void
 MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
 {
 	vehicle_attitude_setpoint_s attitude_setpoint{};
+	landing_gear_s landing_gear{};
 	const float yaw = Eulerf(Quatf(_v_att.q)).psi();
 
 	/* reset yaw setpoint to current position if needed */
@@ -508,9 +520,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 
 	attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z);
 
-	attitude_setpoint.landing_gear = get_landing_gear_state();
-	attitude_setpoint.timestamp = hrt_absolute_time();
+	_landing_gear_state.landing_gear = get_landing_gear_state();
+
+	attitude_setpoint.timestamp = landing_gear.timestamp = hrt_absolute_time();
 	orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
+	orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
 }
 
 /**
@@ -759,7 +773,7 @@ MulticopterAttitudeControl::publish_actuator_controls()
 	_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
 	_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
 	_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
-	_actuators.control[7] = _v_att_sp.landing_gear;
+	_actuators.control[7] = _landing_gear_state.landing_gear;
 	_actuators.timestamp = hrt_absolute_time();
 	_actuators.timestamp_sample = _sensor_gyro.timestamp;
 
@@ -805,6 +819,7 @@ MulticopterAttitudeControl::run()
 	_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
 	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_landing_gear_sub =  orb_subscribe(ORB_ID(landing_gear));
 
 	/* wakeup source: gyro data from sensor selected by the sensor app */
 	px4_pollfd_struct_t poll_fds = {};
@@ -873,6 +888,7 @@ MulticopterAttitudeControl::run()
 			sensor_correction_poll();
 			sensor_bias_poll();
 			vehicle_land_detected_poll();
+			landing_gear_state_poll();
 			const bool manual_control_updated = vehicle_manual_poll();
 			const bool attitude_updated = vehicle_attitude_poll();
 			attitude_dt += dt;
@@ -982,6 +998,7 @@ MulticopterAttitudeControl::run()
 	orb_unsubscribe(_sensor_correction_sub);
 	orb_unsubscribe(_sensor_bias_sub);
 	orb_unsubscribe(_vehicle_land_detected_sub);
+	orb_unsubscribe(_landing_gear_sub);
 }
 
 int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -55,6 +55,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/landing_gear.h>
 
 #include <float.h>
 #include <mathlib/mathlib.h>
@@ -110,6 +111,7 @@ private:
 	orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
 	orb_advert_t _pub_vehicle_command{nullptr};           /**< vehicle command publication */
 	orb_id_t _attitude_setpoint_id{nullptr};
+	orb_advert_t	_landing_gear_pub{nullptr};
 
 	int		_vehicle_status_sub{-1};		/**< vehicle status subscription */
 	int		_vehicle_land_detected_sub{-1};	/**< vehicle land detected subscription */
@@ -135,6 +137,7 @@ private:
 	home_position_s				_home_pos{};			/**< home position */
 	vehicle_trajectory_waypoint_s		_traj_wp_avoidance{};		/**< trajectory waypoint */
 	vehicle_trajectory_waypoint_s		_traj_wp_avoidance_desired{};	/**< desired waypoints, inputs to an obstacle avoidance module */
+	landing_gear_s _landing_gear_state{};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
@@ -593,7 +596,15 @@ MulticopterPositionControl::run()
 	poll_subscriptions();
 
 	// Let's be safe and have the landing gear down by default
-	_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN;
+	_landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN;
+	_landing_gear_state.timestamp = hrt_absolute_time();
+
+	if (_landing_gear_pub != nullptr) {
+		orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state);
+
+	} else {
+		_landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state);
+	}
 
 	// setup file descriptor to poll the local position as loop wakeup source
 	px4_pollfd_struct_t poll_fd = {.fd = _local_pos_sub};
@@ -792,16 +803,6 @@ MulticopterPositionControl::run()
 			_att_sp.fw_control_yaw = false;
 			_att_sp.apply_flaps = false;
 
-			if (!constraints.landing_gear) {
-				if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
-					_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
-				}
-
-				if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) {
-					_att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN;
-				}
-			}
-
 			// publish attitude setpoint
 			// Note: this requires review. The reason for not sending
 			// an attitude setpoint is because for non-flighttask modes
@@ -809,6 +810,25 @@ MulticopterPositionControl::run()
 			// they might conflict with each other such as in offboard attitude control.
 			publish_attitude();
 
+			if (!constraints.landing_gear) {
+				if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
+					_landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_UP;
+				}
+
+				if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) {
+					_landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN;
+				}
+			}
+
+			_landing_gear_state.timestamp = hrt_absolute_time();
+
+			if (_landing_gear_pub != nullptr) {
+				orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state);
+
+			} else {
+				_landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state);
+			}
+
 		} else {
 			// no flighttask is active: set attitude setpoint to idle
 			_att_sp.roll_body = _att_sp.pitch_body = 0.0f;

src/modules/navigator/gpsfailure.cpp

@@ -46,6 +46,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <mathlib/mathlib.h>
 
 using matrix::Eulerf;

src/modules/navigator/navigator.h

@@ -67,7 +67,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>