Added Throw Launch feature

More details in the PR: https://github.com/PX4/PX4-Autopilot/pull/21170

Signed-off-by: Michał Barciś <mbarcis@mbarcis.net>

src/modules/commander/Commander.cpp

@@ -1694,6 +1694,8 @@ void Commander::run()
 
 		safetyButtonUpdate();
 
+		throwLaunchUpdate();
+
 		vtolStatusUpdate();
 
 		_home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed);
@@ -1773,7 +1775,8 @@ void Commander::run()
 		_actuator_armed.prearmed = getPrearmState();
 		_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
 		_actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION)
-					    || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON));
+					    || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)
+					    || !(_throw_launch_state == ThrowLaunchState::DISABLED || _throw_launch_state == ThrowLaunchState::FLYING));
 		// _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL
 		_actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
 		// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
@@ -2027,6 +2030,47 @@ void Commander::safetyButtonUpdate()
 	}
 }
 
+void Commander::throwLaunchUpdate()
+{
+	if (_param_com_throw_en.get()) {
+		if (_vehicle_local_position_sub.updated()) {
+			_vehicle_local_position_sub.copy(&_vehicle_local_position);
+		}
+
+		if (!isArmed() && _throw_launch_state != ThrowLaunchState::IDLE) {
+			mavlink_log_info(&_mavlink_log_pub, "The vehicle is DISARMED with throw launch enabled. Do NOT throw it.\t");
+			_throw_launch_state = ThrowLaunchState::IDLE;
+		}
+
+		if (_throw_launch_state == ThrowLaunchState::IDLE && isArmed()) {
+			mavlink_log_info(&_mavlink_log_pub, "The vehicle is ARMED with throw launch enabled. Throw the vehicle now.\t");
+			_throw_launch_state = ThrowLaunchState::ARMED;
+		}
+
+		float vehicle_speed_squared = (
+						      _vehicle_local_position.vx * _vehicle_local_position.vx +
+						      _vehicle_local_position.vy * _vehicle_local_position.vy +
+						      _vehicle_local_position.vz * _vehicle_local_position.vz
+					      );
+		float min_launch_speed = _param_com_throw_min_speed.get();
+
+		if (_throw_launch_state == ThrowLaunchState::ARMED &&
+		    vehicle_speed_squared >= min_launch_speed * min_launch_speed) {
+			PX4_INFO("Minimum throw speed exceeded; the motors will start when the vehicle starts falling down.");
+			_throw_launch_state = ThrowLaunchState::UNSAFE;
+		}
+
+		if (_throw_launch_state == ThrowLaunchState::UNSAFE && _vehicle_local_position.vz > 0) {
+			PX4_INFO("Throw successful, starting motors.");
+			_throw_launch_state = ThrowLaunchState::FLYING;
+		}
+
+	} else if (_throw_launch_state != ThrowLaunchState::DISABLED) {
+		// make sure everything is reset when the throw launch is disabled
+		_throw_launch_state = ThrowLaunchState::DISABLED;
+	}
+}
+
 void Commander::vtolStatusUpdate()
 {
 	// Make sure that this is only adjusted if vehicle really is of type vtol
@@ -2085,6 +2129,9 @@ void Commander::updateTunes()
 	} else if (_vehicle_status.failsafe && isArmed()) {
 		tune_failsafe(true);
 
+	} else if (_throw_launch_state == ThrowLaunchState::ARMED) {
+		set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
+
 	} else {
 		set_tune(tune_control_s::TUNE_ID_STOP);
 	}
@@ -2141,7 +2188,10 @@ void Commander::handleAutoDisarm()
 				_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
 			}
 
-			if (_auto_disarm_landed.get_state()) {
+			const bool throw_launch_in_progress = (_throw_launch_state == ThrowLaunchState::ARMED
+							       || _throw_launch_state == ThrowLaunchState::UNSAFE);
+
+			if (_auto_disarm_landed.get_state() && !throw_launch_in_progress) {
 				if (_have_taken_off_since_arming) {
 					disarm(arm_disarm_reason_t::auto_disarm_land);
 
@@ -2166,7 +2216,8 @@ void Commander::handleAutoDisarm()
 			if (_actuator_armed.manual_lockdown) {
 				disarm(arm_disarm_reason_t::kill_switch, true);
 
-			} else {
+			} else if (!_param_com_throw_en.get()) {  // don't disarm if throw
+				// launch is enabled
 				disarm(arm_disarm_reason_t::lockdown, true);
 			}
 		}
@@ -2298,6 +2349,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_PURPLE;
 
+		} else if (_throw_launch_state == ThrowLaunchState::ARMED) {
+			led_mode = led_control_s::MODE_BLINK_FAST;
+			led_color = led_control_s::COLOR_YELLOW;
+
 		} else if (isArmed()) {
 			led_mode = led_control_s::MODE_ON;
 			set_normal_color = true;

src/modules/commander/Commander.hpp

@@ -174,6 +174,8 @@ private:
 
 	void safetyButtonUpdate();
 
+	void throwLaunchUpdate();
+
 	void vtolStatusUpdate();
 
 	void updateTunes();
@@ -201,6 +203,14 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};
 
+	enum class ThrowLaunchState {
+		DISABLED = 0,
+		IDLE = 1,
+		ARMED = 2,
+		UNSAFE = 3,
+		FLYING = 4
+	};
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 
@@ -261,7 +271,9 @@ private:
 	bool _arm_tune_played{false};
 	bool _have_taken_off_since_arming{false};
 	bool _status_changed{true};
+	ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED};
 
+	vehicle_local_position_s	_vehicle_local_position{};
 	vehicle_land_detected_s	_vehicle_land_detected{};
 
 	// commander publications
@@ -277,6 +289,7 @@ private:
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription					_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 	uORB::SubscriptionInterval				_parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -328,6 +341,8 @@ private:
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
-		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
+		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max,
+		(ParamBool<px4::params::COM_THROW_EN>)      _param_com_throw_en,
+		(ParamFloat<px4::params::COM_THROW_SPEED>)  _param_com_throw_min_speed
 	)
 };

src/modules/commander/commander_params.c

@@ -1134,3 +1134,30 @@ PARAM_DEFINE_INT32(COM_ARMABLE, 1);
  * @group Commander
  */
 PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f);
+
+/**
+ * Enable throw-start
+ *
+ * Allows to start the vehicle by throwing it into the air.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_THROW_EN, 0);
+
+/**
+ * Minimum speed for the throw start
+ *
+ * When the throw launch is enabled, the drone will only arm after this speed is exceeded before detecting
+ * the freefall. This is a safety feature to ensure the drone does not turn on after accidental drop or
+ * a rapid movement before the throw.
+ *
+ * Set to 0 to disable.
+ *
+ * @group Commander
+ * @min 0
+ * @decimal 1
+ * @increment 0.1
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -39,6 +39,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 
+
 void TakeoffHandling::generateInitialRampValue(float velocity_p_gain)
 {
 	velocity_p_gain = math::max(velocity_p_gain, 0.01f);
@@ -74,6 +75,9 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co
 			_takeoff_state = TakeoffState::rampup;
 			_takeoff_ramp_progress = 0.f;
 
+		} else if (!landed) {
+			_takeoff_state = TakeoffState::flight;
+
 		} else {
 			break;
 		}
@@ -103,7 +107,6 @@ void TakeoffHandling::updateTakeoffState(const bool armed, const bool landed, co
 		_takeoff_state = TakeoffState::flight;
 	}
 
-	// TODO: need to consider free fall here
 	if (!armed) {
 		_takeoff_state = TakeoffState::disarmed;
 	}

src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp

@@ -53,15 +53,15 @@ TEST(TakeoffTest, RegularTakeoffRamp)
 	takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed);
 
-	// armed, not landed anymore, don't want takeoff
-	takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms);
+	// armed, landed, don't want takeoff
+	takeoff.updateTakeoffState(true, true, false, 1.f, false, 500_ms);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup);
 
-	// armed, not landed, don't want takeoff yet, spoolup time passed
-	takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s);
+	// armed, landed, don't want takeoff yet, spoolup time passed
+	takeoff.updateTakeoffState(true, true, false, 1.f, false, 2_s);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff);
 
-	// armed, not landed, want takeoff
+	// armed, not landed anymore, want takeoff
 	takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s);
 	EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup);