FW Launch Detection: refactor state machine and pubish launch_detection_status message

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

msg/CMakeLists.txt

@@ -112,6 +112,7 @@ set(msg_files
 	LandingGearWheel.msg
 	LandingTargetInnovations.msg
 	LandingTargetPose.msg
+	LaunchDetectionStatus.msg
 	LedControl.msg
 	LoggerStatus.msg
 	LogMessage.msg

msg/LaunchDetectionStatus.msg

@@ -0,0 +1,9 @@
+# Status of the launch detection state machine (fixed-wing only)
+
+uint64 timestamp # time since system start (microseconds)
+
+uint8 STATE_WAITING_FOR_LAUNCH 			= 0 # waiting for launch
+uint8 STATE_LAUNCH_DETECTED_DISABLED_MOTOR 	= 1 # launch detected, but keep motor(s) disabled (e.g. because it can't spin freely while on catapult)
+uint8 STATE_FLYING 				= 2 # launch detected, use normal takeoff/flying configuration
+
+uint8 launch_detection_state

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -66,6 +66,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	_pos_ctrl_status_pub.advertise();
 	_pos_ctrl_landing_status_pub.advertise();
 	_tecs_status_pub.advertise();
+	_launch_detection_status_pub.advertise();
 
 	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
 
@@ -1530,31 +1531,22 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 	} else {
 		/* Perform launch detection */
 		if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() &&
-		    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+		    _launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
 
 			if (_control_mode.flag_armed) {
 				/* Perform launch detection */
 
-				/* Inform user that launchdetection is running every 4s */
-				if ((now - _last_time_launch_detection_notified) > 4_s) {
-					mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t");
-					events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running");
-					_last_time_launch_detection_notified = now;
-				}
-
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(control_interval, _body_acceleration(0));
-
-				/* update our copy of the launch detection state */
-				_launch_detection_state = _launchDetector.getLaunchDetected();
+				_launchDetector.update(control_interval, _body_acceleration(0), &_mavlink_log_pub);
 			}
 
 		} else	{
 			/* no takeoff detection --> fly */
-			_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+			_launchDetector.forceSetFlyState();
 		}
 
-		if (!_launch_detected && _launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+		if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH
+		    && _launchDetector.launchDetectionEnabled()) {
 			_launch_detected = true;
 			_launch_global_position = global_position;
 			_takeoff_ground_alt = _current_altitude;
@@ -1568,7 +1560,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
 
 		/* Set control values depending on the detection state */
-		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+		if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH
+		    && _launchDetector.launchDetectionEnabled()) {
 			/* Launch has been detected, hence we have to control the plane. */
 
 			if (_param_fw_use_npfg.get()) {
@@ -1588,7 +1581,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 			 * full throttle, otherwise we use idle throttle */
-			const float max_takeoff_throttle = (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) ?
+			const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ?
 							   _param_fw_thr_idle.get() : _param_fw_thr_max.get();
 
 			tecs_update_pitch_throttle(control_interval,
@@ -1603,7 +1596,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 						   _param_sinkrate_target.get(),
 						   _param_fw_t_clmb_max.get());
 
-			if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+			if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
 				// explicitly set idle throttle until motors are enabled
 				_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
 
@@ -1626,6 +1619,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
+
+		launch_detection_status_s launch_detection_status;
+		launch_detection_status.timestamp = now;
+		launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected();
+		_launch_detection_status_pub.publish(launch_detection_status);
 	}
 
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);
@@ -2260,6 +2258,8 @@ FixedwingPositionControl::Run()
 
 			if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
 				_landed = vehicle_land_detected.landed;
+
+				if (_launch_detected) {_landed = false;}
 			}
 		}
 
@@ -2397,8 +2397,6 @@ FixedwingPositionControl::reset_takeoff_state()
 	_runway_takeoff.reset();
 
 	_launchDetector.reset();
-	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
-	_last_time_launch_detection_notified = 0;
 
 	_launch_detected = false;
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -72,6 +72,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/npfg_status.h>
 #include <uORB/topics/parameter_update.h>
@@ -216,6 +217,7 @@ private:
 	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
 	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
+	uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
 	manual_control_setpoint_s _manual_control_setpoint{};
@@ -307,11 +309,6 @@ private:
 	// class handling launch detection methods for fixed-wing takeoff
 	LaunchDetector _launchDetector;
 
-	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
-
-	// [us] logs the last time the launch detection notification was sent (used not to spam notifications during launch detection)
-	hrt_abstime _last_time_launch_detection_notified{0};
-
 	// true if a launch, specifically using the launch detector, has been detected
 	bool _launch_detected{false};
 

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.cpp

@@ -40,29 +40,44 @@
 #include "LaunchDetector.h"
 
 #include <px4_platform_common/log.h>
+#include <systemlib/mavlink_log.h>
+#include <px4_platform_common/events.h>
 
 namespace launchdetection
 {
 
-void LaunchDetector::update(const float dt, float accel_x)
+void LaunchDetector::update(const float dt, float accel_x,  orb_advert_t *mavlink_log_pub)
 {
-	switch (state) {
-	case LAUNCHDETECTION_RES_NONE:
+	switch (_state) {
+	case launch_detection_status_s::STATE_WAITING_FOR_LAUNCH:
+
+		_launchDetectionRunningInfoDelay += dt;
+
+		/* Inform user that launchdetection is running every 4s */
+		if (_launchDetectionRunningInfoDelay >= 4.f) {
+			mavlink_log_info(mavlink_log_pub, "Launch detection running\t");
+			events::send(events::ID("launch_detection_running_info"), events::Log::Info, "Launch detection running");
+			_launchDetectionRunningInfoDelay = 0.f; // reset counter
+		}
 
 		/* Detect a acceleration that is longer and stronger as the minimum given by the params */
 		if (accel_x > _param_laun_cat_a.get()) {
-			_integrator += dt;
+			_launchDetectionDelayCounter += dt;
 
-			if (_integrator > _param_laun_cat_t.get()) {
-				if (_param_laun_cat_mdel.get() > 0.0f) {
-					state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
-					PX4_WARN("Launch detected: enablecontrol, waiting %8.4fs until full throttle",
-						 double(_param_laun_cat_mdel.get()));
+			if (_launchDetectionDelayCounter > _param_laun_cat_t.get()) {
+				if (_param_laun_cat_mdel.get() > 0.f) {
+					_state = launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR;
+					mavlink_log_info(mavlink_log_pub, "Launch detected: enable control, waiting %8.1fs until full throttle\t",
+							 (double)_param_laun_cat_mdel.get());
+					events::send<float>(events::ID("launch_detection_wait_for_throttle"), {events::Log::Warning, events::LogInternal::Info},
+							    "Launch detected: enablecontrol, waiting {1:.1}s until full throttle", (double)_param_laun_cat_mdel.get());
 
 				} else {
 					/* No motor delay set: go directly to enablemotors state */
-					state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-					PX4_WARN("Launch detected: enablemotors (delay not activated)");
+					_state = launch_detection_status_s::STATE_FLYING;
+					mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors (no motor delay)\t");
+					events::send(events::ID("launch_detection_no_motor_delay"), {events::Log::Warning, events::LogInternal::Info},
+						     "Launch detected: enable motors (no motor delay)");
 				}
 			}
 
@@ -72,34 +87,39 @@ void LaunchDetector::update(const float dt, float accel_x)
 
 		break;
 
-	case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
-		/* Vehicle is currently controlling attitude but not with full throttle. Waiting until delay is
+	case launch_detection_status_s::STATE_LAUNCH_DETECTED_DISABLED_MOTOR:
+		/* Vehicle is currently controlling attitude but at idle throttle. Waiting until delay is
 		 * over to allow full throttle */
 		_motorDelayCounter += dt;
 
 		if (_motorDelayCounter > _param_laun_cat_mdel.get()) {
-			PX4_INFO("Launch detected: state enablemotors");
-			state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
+			mavlink_log_info(mavlink_log_pub, "Launch detected: enable motors\t");
+			events::send(events::ID("launch_detection_enable_motors"), {events::Log::Warning, events::LogInternal::Info},
+				     "Launch detected: enable motors");
+			_state = launch_detection_status_s::STATE_FLYING;
 		}
 
+		_launchDetectionRunningInfoDelay = 4.f; // reset counter
+
 		break;
 
 	default:
+		_launchDetectionRunningInfoDelay = 4.f; // reset counter
 		break;
 
 	}
 }
 
-LaunchDetectionResult LaunchDetector::getLaunchDetected() const
+uint LaunchDetector::getLaunchDetected() const
 {
-	return state;
+	return _state;
 }
 
 void LaunchDetector::reset()
 {
-	_integrator = 0.0f;
-	_motorDelayCounter = 0.0f;
-	state = LAUNCHDETECTION_RES_NONE;
+	_launchDetectionDelayCounter = 0.f;
+	_motorDelayCounter = 0.f;
+	_state = launch_detection_status_s::STATE_WAITING_FOR_LAUNCH;
 }
 
 

src/modules/fw_pos_control_l1/launchdetection/LaunchDetector.h

@@ -42,20 +42,11 @@
 #define LAUNCHDETECTOR_H
 
 #include <px4_platform_common/module_params.h>
+#include <uORB/topics/launch_detection_status.h>
 
 namespace launchdetection
 {
 
-enum LaunchDetectionResult {
-	LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
-	LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
-							  control the attitude. However any motors should not throttle
-							  up. For instance this is used to have a delay for the motor
-							  when launching a fixed wing aircraft from a bungee */
-	LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, the controller should control
-							attitude and also throttle up the motors. */
-};
-
 class __EXPORT LaunchDetector : public ModuleParams
 {
 public:
@@ -67,15 +58,49 @@ public:
 
 	void reset();
 
-	void update(const float dt, float accel_x);
-	LaunchDetectionResult getLaunchDetected() const;
+	/**
+	 * @brief Updates the state machine based on the current vehicle condition.
+	 *
+	 * @param dt Time step [us]
+	 * @param accel_x Measured acceleration in body x [m/s/s]
+	 * @param mavlink_log_pub
+	 */
+	void update(const float dt, float accel_x,  orb_advert_t *mavlink_log_pub);
+
+	/**
+	 * @brief Get the Launch Detected state
+	 *
+	 * @return uint (aligned with launch_detection_status_s::launch_detection_state)
+	 */
+	uint getLaunchDetected() const;
+
+	/**
+	 * @return Launch detection is enabled
+	 */
 	bool launchDetectionEnabled() { return _param_laun_all_on.get(); }
 
+	void forceSetFlyState() { _state = launch_detection_status_s::STATE_FLYING; }
+
 private:
-	float _integrator{0.f};
+	/**
+	 * Integrator [s]
+	 */
+	float _launchDetectionDelayCounter{0.f};
+
+	/**
+	 * Motor delay counter [s]
+	 */
 	float _motorDelayCounter{0.f};
 
-	LaunchDetectionResult state{LAUNCHDETECTION_RES_NONE};
+	float _launchDetectionRunningInfoDelay{4.f};
+
+	/**
+	 * Current state of the launch detection state machine [launch_detection_status_s::launch_detection_state]
+	 */
+	uint _state{launch_detection_status_s::STATE_WAITING_FOR_LAUNCH};
+
+	// [us] logs the last time the launch detection notification was sent (used not to spam notifications during launch detection)
+	hrt_abstime _last_time_launch_detection_notified{0};
 
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::LAUN_ALL_ON>) _param_laun_all_on,

src/modules/logger/logged_topics.cpp

@@ -77,6 +77,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("irlock_report", 1000);
 	add_topic("landing_gear_wheel", 10);
 	add_optional_topic("landing_target_pose", 1000);
+	add_optional_topic("launch_detection_status", 200);
 	add_optional_topic("magnetometer_bias_estimate", 200);
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");