ThrowLaunch: move into separate class

src/modules/commander/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,11 +31,12 @@
 #
 ############################################################################
 
+add_subdirectory(Arming)
+add_subdirectory(failsafe)
 add_subdirectory(failure_detector)
 add_subdirectory(HealthAndArmingChecks)
-add_subdirectory(failsafe)
-add_subdirectory(Arming)
 add_subdirectory(ModeUtil)
+add_subdirectory(MulticopterThrowLaunch)
 
 px4_add_module(
 	MODULE modules__commander
@@ -52,24 +53,25 @@ px4_add_module(
 		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		HomePosition.cpp
-		UserModeIntention.cpp
 		level_calibration.cpp
 		lm_fit.cpp
 		mag_calibration.cpp
 		rc_calibration.cpp
 		Safety.cpp
+		UserModeIntention.cpp
 		worker_thread.cpp
 	DEPENDS
+		ArmAuthorization
 		circuit_breaker
+		failsafe
 		failure_detector
 		geo
 		health_and_arming_checks
 		hysteresis
-		ArmAuthorization
+		mode_util
+		MulticopterThrowLaunch
 		sensor_calibration
 		world_magnetic_model
-		mode_util
-		failsafe
 	)
 
 px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)

src/modules/commander/Commander.cpp

@@ -1694,7 +1694,7 @@ void Commander::run()
 
 		safetyButtonUpdate();
 
-		throwLaunchUpdate();
+		_multicopter_throw_launch.update(isArmed());
 
 		vtolStatusUpdate();
 
@@ -1776,7 +1776,7 @@ void Commander::run()
 		_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)
-					    || isThrowLaunchInProgress());
+					    || _multicopter_throw_launch.isThrowLaunchInProgress());
 		// _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
@@ -2030,61 +2030,6 @@ void Commander::safetyButtonUpdate()
 	}
 }
 
-bool Commander::isThrowLaunchInProgress() const
-{
-	return !(_throw_launch_state == ThrowLaunchState::DISABLED || _throw_launch_state == ThrowLaunchState::FLYING);
-}
-
-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;
-		}
-
-		switch (_throw_launch_state) {
-		case ThrowLaunchState::IDLE:
-			if (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;
-			}
-
-			break;
-
-		case ThrowLaunchState::ARMED:
-			if (matrix::Vector3f(
-				    _vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz
-			    ).longerThan(_param_com_throw_min_speed.get())) {
-				PX4_INFO("Minimum throw speed exceeded; the motors will start when the vehicle starts falling down.");
-				_throw_launch_state = ThrowLaunchState::UNSAFE;
-			}
-
-			break;
-
-		case ThrowLaunchState::UNSAFE:
-			if (_vehicle_local_position.vz > 0) {
-				PX4_INFO("Throw successful, starting motors.");
-				_throw_launch_state = ThrowLaunchState::FLYING;
-			}
-
-			break;
-
-		default:
-
-			break;
-		}
-
-	} 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
@@ -2143,7 +2088,7 @@ void Commander::updateTunes()
 	} else if (_vehicle_status.failsafe && isArmed()) {
 		tune_failsafe(true);
 
-	} else if (_throw_launch_state == ThrowLaunchState::ARMED) {
+	} else if (_multicopter_throw_launch.isReadyToThrow()) {
 		set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
 
 	} else {
@@ -2202,7 +2147,7 @@ void Commander::handleAutoDisarm()
 				_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
 			}
 
-			if (_auto_disarm_landed.get_state() && !isThrowLaunchInProgress()) {
+			if (_auto_disarm_landed.get_state() && !_multicopter_throw_launch.isThrowLaunchInProgress()) {
 				if (_have_taken_off_since_arming) {
 					disarm(arm_disarm_reason_t::auto_disarm_land);
 
@@ -2222,7 +2167,7 @@ void Commander::handleAutoDisarm()
 		}
 
 		//don't disarm if throw launch is in progress
-		auto_disarm &= !isThrowLaunchInProgress();
+		auto_disarm &= !_multicopter_throw_launch.isThrowLaunchInProgress();
 
 		_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
 
@@ -2362,7 +2307,7 @@ 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) {
+		} else if (_multicopter_throw_launch.isReadyToThrow()) {
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_YELLOW;
 

src/modules/commander/Commander.hpp

@@ -34,13 +34,14 @@
 #pragma once
 
 /*   Helper classes  */
-#include "failure_detector/FailureDetector.hpp"
 #include "failsafe/failsafe.h"
-#include "Safety.hpp"
+#include "failure_detector/FailureDetector.hpp"
-#include "worker_thread.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
+#include "MulticopterThrowLaunch/MulticopterThrowLaunch.hpp"
+#include "Safety.hpp"
 #include "UserModeIntention.hpp"
+#include "worker_thread.hpp"
 
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
@@ -80,7 +81,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
 using math::constrain;
@@ -205,14 +205,6 @@ 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};
 
@@ -222,6 +214,7 @@ private:
 	FailsafeBase		&_failsafe{_failsafe_instance};
 	FailureDetector		_failure_detector{this};
 	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
+	MulticopterThrowLaunch  _multicopter_throw_launch{this};
 	Safety			_safety{};
 	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
 	WorkerThread 		_worker_thread{};
@@ -273,9 +266,7 @@ 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
@@ -291,7 +282,6 @@ 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};
@@ -343,8 +333,6 @@ 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/MulticopterThrowLaunch/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(MulticopterThrowLaunch
+	MulticopterThrowLaunch.cpp
+)

src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.cpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "MulticopterThrowLaunch.hpp"
+#include <px4_platform_common/events.h>
+
+MulticopterThrowLaunch::MulticopterThrowLaunch(ModuleParams *parent) :
+	ModuleParams(parent)
+{}
+
+void MulticopterThrowLaunch::update(const bool armed)
+{
+	if (_param_com_throw_en.get()) {
+		if (_vehicle_local_position_sub.updated()) {
+			vehicle_local_position_s vehicle_local_position{};
+			if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+				_last_velocity = matrix::Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+			}
+		}
+
+		if (!armed && _throw_launch_state != ThrowLaunchState::IDLE) {
+			events::send(events::ID("mc_throw_launch_not_ready"), events::Log::Critical, "Disarmed, don't throw");
+			_throw_launch_state = ThrowLaunchState::IDLE;
+		}
+
+		switch (_throw_launch_state) {
+		case ThrowLaunchState::IDLE:
+			if (armed) {
+				events::send(events::ID("mc_throw_launch_ready"), events::Log::Critical, "Ready for throw launch");
+				_throw_launch_state = ThrowLaunchState::ARMED;
+			}
+
+			break;
+
+		case ThrowLaunchState::ARMED:
+			if (_last_velocity.longerThan(_param_com_throw_min_speed.get())) {
+				PX4_INFO("Throw detected, motors will start once falling");
+				_throw_launch_state = ThrowLaunchState::UNSAFE;
+			}
+
+			break;
+
+		case ThrowLaunchState::UNSAFE:
+			if (_last_velocity(2) > 0.f) {
+				PX4_INFO("Throw and fall detected, starting motors");
+				_throw_launch_state = ThrowLaunchState::FLYING;
+			}
+
+			break;
+
+		case ThrowLaunchState::DISABLED:
+		case ThrowLaunchState::FLYING:
+			// Nothing to do
+			break;
+		}
+
+	} else if (_throw_launch_state != ThrowLaunchState::DISABLED) {
+		// make sure everything is reset when the throw launch is disabled
+		_throw_launch_state = ThrowLaunchState::DISABLED;
+	}
+}

src/modules/commander/MulticopterThrowLaunch/MulticopterThrowLaunch.hpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file MulticopterThrowLaunch.hpp
+ *
+ * Changes to manage a takeoff of a multicopter by manually throwing it into the air.
+ *
+ * @author Michał Barciś <mbarcis@mbarcis.net>
+ */
+
+#pragma once
+
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+
+class MulticopterThrowLaunch : public ModuleParams
+{
+public:
+	explicit MulticopterThrowLaunch(ModuleParams *parent);
+	~MulticopterThrowLaunch() override = default;
+
+	/**
+	 * @return false if feature disabled or already flying
+	 */
+	bool isThrowLaunchInProgress() const {
+		return _throw_launch_state != ThrowLaunchState::DISABLED
+			&& _throw_launch_state != ThrowLaunchState::FLYING;
+	}
+
+	bool isReadyToThrow() const { return _throw_launch_state == ThrowLaunchState::ARMED; }
+
+	/**
+	 * Main update of the state
+	 * @param armed true if vehicle is armed
+	 */
+	void update(const bool armed);
+
+	enum class ThrowLaunchState {
+		DISABLED = 0,
+		IDLE = 1,
+		ARMED = 2,
+		UNSAFE = 3,
+		FLYING = 4
+	};
+
+private:
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED};
+	matrix::Vector3f _last_velocity{};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::COM_THROW_EN>) _param_com_throw_en,
+		(ParamFloat<px4::params::COM_THROW_SPEED>) _param_com_throw_min_speed
+	);
+};

src/modules/mc_pos_control/Takeoff/Takeoff.cpp

@@ -39,7 +39,6 @@
 #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);