[DO NOT MERGE] Fixed wing land-detector block

msg/position_controller_status.msg

@@ -14,3 +14,5 @@ float32 acceptance_radius		# the optimal distance to a waypoint to switch to the
 float32 yaw_acceptance			# NaN if not set
 
 float32 altitude_acceptance		# the optimal vertical distance to a waypoint to switch to the next
+
+bool launch_detection_running           # true if the launch detection is running

src/modules/commander/Commander.cpp

@@ -110,6 +110,7 @@ static struct actuator_armed_s armed = {};
 
 static struct vehicle_status_flags_s status_flags = {};
 
+static struct position_controller_status_s controller_status = {}; //Struct for holding controller status
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -1547,9 +1548,12 @@ Commander::run()
 			_was_falling = _land_detector.freefall;
 		}
 
+		if (_controller_status_sub.updated()) {
+			_controller_status_sub.copy(&controller_status);
+		}
 
-		// Auto disarm when landed or kill switch engaged
-		if (armed.armed) {
+		// Auto disarm when landed or kill switch engaged and do not auto-disarm if launch detection is running
+		if (armed.armed && !controller_status.launch_detection_running) {
 
 			// Check for auto-disarm on landing or pre-flight
 			if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {

src/modules/commander/Commander.hpp

@@ -80,6 +80,8 @@
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/esc_status.h>
+#include <uORB/topics/position_controller_status.h>
 
 using math::constrain;
 using systemlib::Hysteresis;
@@ -390,6 +392,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<position_controller_status_s>      _controller_status_sub{ORB_ID(position_controller_status)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -534,6 +534,12 @@ FixedwingPositionControl::status_publish()
 
 	pos_ctrl_status.yaw_acceptance = NAN;
 
+	pos_ctrl_status.launch_detection_running = _launch_detection_state == LAUNCHDETECTION_RES_NONE
+			&& _control_mode.flag_armed
+			&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
+			&& _pos_sp_triplet.current.valid
+			&& _launchDetector.launchDetectionEnabled();
+
 	pos_ctrl_status.timestamp = hrt_absolute_time();
 
 	_pos_ctrl_status_pub.publish(pos_ctrl_status);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -165,7 +165,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
-	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
+	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};					///< TECS status publication
+
 
 	manual_control_setpoint_s	_manual {};			///< r/c channel data
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -55,6 +55,16 @@ void FixedwingLandDetector::_update_topics()
 {
 	LandDetector::_update_topics();
 	_airspeed_sub.update(&_airspeed);
+	_controller_sub.update(&_status);
+	_attitude_sub.update(&_attitude);
+}
+void FixedwingLandDetector::vehicle_calcPitch()
+{
+	// This will calculate the current pitch of the vehicle.
+	// This is used for limiting the pitch of a fixedwing vehicle in a launch state.
+	_PQ = Quatf(_attitude.q);
+	Eulerf euler_angles(_PQ);
+	_pitch = euler_angles(1);
 }
 
 bool FixedwingLandDetector::_get_landed_state()
@@ -92,11 +102,17 @@ bool FixedwingLandDetector::_get_landed_state()
 
 		_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
 
-		// Crude land detector for fixedwing.
-		landDetected = _airspeed_filtered       < _param_lndfw_airspd.get()
-			       && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
-			       && _velocity_z_filtered  < _param_lndfw_vel_z_max.get()
-			       && _xy_accel_filtered    < _param_lndfw_xyaccel_max.get();
+		// Crude land detector for fixedwing that changes values based on launch state of the vehicle.
+		if (_status.launch_detection_running) {
+			vehicle_calcPitch();
+			landDetected = accel(0) < _param_laun_cat_accel.get() || fabsf(_pitch) > (float)radians(_param_laun_cat_pmax.get());
+
+		} else {
+			landDetected = _airspeed_filtered       < _param_lndfw_airspd.get()
+				       && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get()
+				       && _velocity_z_filtered  < _param_lndfw_vel_z_max.get()
+				       && _xy_accel_filtered    < _param_lndfw_xyaccel_max.get();
+		}
 
 	} else {
 		// Control state topic has timed out and we need to assume we're landed.

src/modules/land_detector/FixedwingLandDetector.h

@@ -42,12 +42,20 @@
 
 #pragma once
 
+#include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
+
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "LandDetector.h"
 
 using namespace time_literals;
+using math::radians;
+using matrix::Quatf;
+using matrix::Eulerf;
+using matrix::Dcmf;
 
 namespace land_detector
 {
@@ -70,8 +78,17 @@ class FixedwingLandDetector final : public LandDetector
 	static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us;
 
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
+	uORB::Subscription _controller_sub{ORB_ID(position_controller_status)};
+	uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)};
 
 	airspeed_s _airspeed{};
+	vehicle_attitude_s _attitude{};
+	position_controller_status_s _status{};
+
+	//variables for calculating the pitch of the vehicle
+	void vehicle_calcPitch();
+	Dcmf _PQ;
+	float _pitch{0.0};
 
 	float _airspeed_filtered{0.0f};
 	float _velocity_xy_filtered{0.0f};
@@ -83,7 +100,9 @@ class FixedwingLandDetector final : public LandDetector
 		(ParamFloat<px4::params::LNDFW_XYACC_MAX>)  _param_lndfw_xyaccel_max,
 		(ParamFloat<px4::params::LNDFW_AIRSPD_MAX>) _param_lndfw_airspd,
 		(ParamFloat<px4::params::LNDFW_VEL_XY_MAX>) _param_lndfw_vel_xy_max,
-		(ParamFloat<px4::params::LNDFW_VEL_Z_MAX>)  _param_lndfw_vel_z_max
+		(ParamFloat<px4::params::LNDFW_VEL_Z_MAX>)  _param_lndfw_vel_z_max,
+		(ParamFloat<px4::params::LAUN_CAT_PMAX>) _param_laun_cat_pmax,
+		(ParamFloat<px4::params::LAUN_CAT_A>) _param_laun_cat_accel
 	);
 };