keep in sync with master

follow changes in master
Fixing of errors caused by #19495
autogyro takeoff updats
Fix build of Autogyro take off

Add motor rampup state, killswitch handling
update throttle

Keep upstream

Author: roman-dvorak

src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.cpp

@@ -80,19 +80,28 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 	_climbout = true;
 	takeoff_status_s takeoff_status = {};
 
+	actuator_armed_s actuator_armed;
+	_actuator_armed_sub.update(&actuator_armed);
+
+	if (actuator_armed.manual_lockdown && _state <= AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP) {
+		_state = AutogyroTakeoffState::TAKEOFF_ERROR;
+	}
+
 	switch (_state) {
 	/*
 	    Hangling error states of takeoff mode. Should lead in allerting operator and/or
 	    abrod takeoff process
 
 	    IN: error state
 	*/
-	case AutogyroTakeoffState::TAKEOFF_ERROR:
-		PX4_INFO("ERR STATE");
-		mavlink_log_info(mavlink_log_pub, "#Takeoff: Error state");
+	case AutogyroTakeoffState::TAKEOFF_ERROR: {
+			if (_state != _state_last) {
+				PX4_INFO("ERR STATE");
+				mavlink_log_info(mavlink_log_pub, "#Takeoff: Error state");
+			}
+		}
 		break;
 
-
 	/*
 	    Initial state of regulator, wait for manual prerotate of rotor.
 
@@ -118,7 +127,8 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				_time_in_state = now;
 			}
 
-			mavlink_log_info(mavlink_log_pub, "#Takeoff: minimal RPM for prerotator reached");
+			mavlink_log_info(mavlink_log_pub, "Takeoff: minimal RPM for prerotator reached");
+			//PX4_INFO("Takeoff: minimal RPM for prerotator reached");
 		}
 
 		break;
@@ -140,13 +150,14 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			_time_in_state = now;
 			play_next_tone();
 			mavlink_log_info(mavlink_log_pub, "Takeoff, prerotator RPM reached");
+			//PX4_INFO("Takeoff: prerotator RPM reached");
 		}
 
 		break;
 
 	/*
-	    All required takeoff conditions are satisfied. Now it is prepared to start.
-	    Try to start main motor. Slowly rump up and check it.
+	    All required takeoff conditions are satisfied. Now it is prepared to
+	    try to start main motor.
 
 	    IN: rotor prepared;
 	    OUT: rotor prepared; minimal airspeed; motor max power,
@@ -156,31 +167,75 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 
 			if (rotor_rpm < _param_ag_rotor_flight_rpm.get()) {
 				ready_for_release = false;
-
+				//PX4_INFO("Takeofff, waiting for flight rpm.");
 				// Some histesis needs to be applied for the start interrupt procedure.
 				// Currently, this does not allow the start to be interrupted.
 				//_state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE;
-				_time_in_state = now;
+				//_time_in_state = now;
 			}
 
 			// check minimal airspeed
 			if (airspeed < (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get())) {
 				ready_for_release = false;
+				//PX4_INFO("Takeofff, waiting for min airspeed.");
 			}
 
 			if (ready_for_release) {
 				_init_yaw = get_bearing_to_next_waypoint(_initial_wp(0), _initial_wp(1), current_lat, current_lon);
 
-				_state = AutogyroTakeoffState::TAKEOFF_RELEASE;
+				_state = AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP;
 				_time_in_state = now;
-				mavlink_log_info(mavlink_log_pub, "Takeoff, Please release.");
-
+				mavlink_log_info(mavlink_log_pub, "Ready to start motor");
+				//PX4_INFO("Takeoff, Please release.");
 				play_next_tone();
 			}
 
 		}
 		break;
 
+	/*
+	    Slowly rampup motor. Keep trying to check other flight parameters.
+	    If I can still fly
+	*/
+	case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: {
+			bool ready_for_release = true;
+
+			if (rotor_rpm < _param_ag_rotor_flight_rpm.get()) {
+				ready_for_release = false;
+				//PX4_INFO("Takeofff, waiting for flight rpm.");
+				// Some histesis needs to be applied for the start interrupt procedure.
+				// Currently, this does not allow the start to be interrupted.
+				//_state = AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE;
+				//_time_in_state = now;
+			}
+
+			// check minimal airspeed
+			if (airspeed < (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get())) {
+				ready_for_release = false;
+				//PX4_INFO("Takeofff, waiting for min airspeed.");
+			}
+
+			// ramup time elapsed
+			if (hrt_elapsed_time(&_time_in_state) < (_param_rwto_ramp_time.get() * 1_s * 1.5)) {
+				ready_for_release = false;
+			}
+
+			// Check if motor/esc power (properller RPM) is suffiscient
+			// TODO
+			if (false) {
+				ready_for_release = false;
+			}
+
+			if (ready_for_release) {
+
+				_state = AutogyroTakeoffState::TAKEOFF_RELEASE;
+				_time_in_state = now;
+				PX4_INFO("Takeoff, Please release.");
+				play_next_tone();
+			}
+		}
+		break;
+
 	/*
 	    Command for release. Sound signal for release from hand or release from
 	    some takeoff platform with mavlink command. This step ends on release ACK.
@@ -198,8 +253,9 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 				play_release_tone();
 			}
 
-			if (alt_agl > _param_rwto_nav_alt.get()) {
+			if (alt_agl > _param_ag_nav_alt.get()) {
 				mavlink_log_info(mavlink_log_pub, "Climbout");
+				PX4_INFO("Takeoff: Climbout.");
 				_state = AutogyroTakeoffState::TAKEOFF_CLIMBOUT;
 				play_next_tone();
 				_time_in_state = now;
@@ -224,6 +280,7 @@ void AutogyroTakeoff::update(const hrt_abstime &now, float airspeed, float rotor
 			_climbout = false;
 			_state = AutogyroTakeoffState::FLY;
 			_time_in_state = now;
+			PX4_INFO("Takeoff:FLY.");
 		}
 
 		//_climbout = false;
@@ -278,6 +335,7 @@ float AutogyroTakeoff::getRequestedAirspeed()
 	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START:
 	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE:
 	case AutogyroTakeoffState::PRE_TAKEOFF_DONE:
+	case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP:
 		return _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get();
 
 	default:
@@ -305,12 +363,14 @@ float AutogyroTakeoff::getPitch(float tecsPitch)
 	switch (_state) {
 
 	case AutogyroTakeoffState::TAKEOFF_ERROR:
-	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: // 0 Null pitch
 		return 0;
 
+	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE_START: // 0 Null pitch
 	case AutogyroTakeoffState::PRE_TAKEOFF_PREROTATE:   // 1 maximal pitch
 	case AutogyroTakeoffState::PRE_TAKEOFF_DONE: // 2
-		return math::radians(_param_rwto_max_pitch.get());
+	case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP:
+		//return math::radians(_param_rwto_max_pitch.get());
+		return math::radians(_param_rwto_psp.get());
 
 	// FLy
 	default:
@@ -331,8 +391,8 @@ float AutogyroTakeoff::getRoll(float navigatorRoll)
 	// allow some limited roll during RELEASE and CLIMBOUT
 	else if (_state < AutogyroTakeoffState::FLY) {
 		return math::constrain(navigatorRoll,
-				       math::radians(-_param_rwto_max_roll.get()),
-				       math::radians(_param_rwto_max_roll.get()));
+				       math::radians(-_param_ag_tko_max_roll.get()),
+				       math::radians(_param_ag_tko_max_roll.get()));
 	}
 
 	return navigatorRoll;
@@ -367,6 +427,8 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 
 	float idle = (double)_param_fw_thr_idle.get();
 
+	//PX4_INFO("GET THROTTLE %f, state: %f, time: %f", (double)idle, (double)_state, (double)(now - _time_in_state));
+
 	switch (_state) {
 
 	case AutogyroTakeoffState::TAKEOFF_ERROR:
@@ -377,15 +439,15 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 	case AutogyroTakeoffState::PRE_TAKEOFF_DONE: {
 			if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) {
 				float throttle = ((now - _time_in_state) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
+				//PX4_INFO("Thortle: %f",(double)throttle);
 				return math::min(throttle, _param_rwto_max_thr.get());
 
 			} else {
 				return idle;
 			}
 		}
 
-
-	case AutogyroTakeoffState::TAKEOFF_RELEASE: {
+	case AutogyroTakeoffState::PRE_TAKEOFF_RAMPUP: {
 			float throttle = idle;
 
 			if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) {
@@ -396,6 +458,20 @@ float AutogyroTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
 				throttle = math::min(throttle, _param_rwto_max_thr.get());
 			}
 
+			return throttle;
+		}
+
+
+	case AutogyroTakeoffState::TAKEOFF_RELEASE: {
+			float throttle = idle;
+
+			if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_RUNWAY) {
+				throttle = _param_rwto_max_thr.get();
+
+			} else if (_param_ag_prerotator_type.get() == AutogyroTakeoffType::WOPREROT_PLATFORM) {
+				throttle = _param_rwto_max_thr.get();
+			}
+
 			return math::min(throttle, _param_rwto_max_thr.get());
 		}
 

src/modules/fw_pos_control_l1/autogyro_takeoff/AutogyroTakeoff.h

@@ -50,10 +50,12 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 
+#include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/takeoff_status.h>
 #include <uORB/topics/debug_value.h>
 #include <uORB/topics/tune_control.h>
+#include <uORB/topics/actuator_armed.h>
 
 namespace autogyrotakeoff
 {
@@ -63,9 +65,10 @@ enum AutogyroTakeoffState {
 	TAKEOFF_ERROR = -1,
 	PRE_TAKEOFF_PREROTATE_START = 0, /**< Wait for manual rotor prerotation or for some other trigger */
 	PRE_TAKEOFF_PREROTATE = 1, /**< Start prerotation of rotor controlled from AP or prerotation with some movement */
-	PRE_TAKEOFF_DONE = 2, /**< autogyro conditions are OK for takeoff, rampup motor */
-	TAKEOFF_RELEASE = 3, /**< command for release */
-	TAKEOFF_CLIMBOUT = 4, /**< Climbout for minimal altitude */
+	PRE_TAKEOFF_DONE = 2, /**< autogyro conditions are OK for takeoff*/
+	PRE_TAKEOFF_RAMPUP = 3, /**< Get ready to takeoff, rampup motor */
+	TAKEOFF_RELEASE = 4, /**< command for release */
+	TAKEOFF_CLIMBOUT = 5, /**< Climbout for minimal altitude */
 	FLY /**< fly to next waypoint */
 };
 
@@ -115,6 +118,7 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 
 	void reset();
 
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 
 	void play_next_tone();
 	void play_release_tone();
@@ -147,11 +151,9 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 		(ParamBool<px4::params::AG_TKOFF>) _param_ag_tkoff,
 		(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
 		(ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
-		(ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
 		(ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
 		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
 		(ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
-		(ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
 		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
 		(ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
 
@@ -164,7 +166,10 @@ class __EXPORT AutogyroTakeoff : public ModuleParams
 		(ParamFloat<px4::params::AG_PROT_MIN_RPM>) _param_ag_prerotator_minimal_rpm,
 		(ParamFloat<px4::params::AG_PROT_TRG_RPM>) _param_ag_prerotator_target_rpm,
 		(ParamFloat<px4::params::AG_ROTOR_RPM>) _param_ag_rotor_flight_rpm,
-		(ParamInt<px4::params::AG_PROT_TYPE>) _param_ag_prerotator_type
+		(ParamInt<px4::params::AG_PROT_TYPE>) _param_ag_prerotator_type,
+
+		(ParamFloat<px4::params::AG_NAV_ALT>) _param_ag_nav_alt,
+		(ParamFloat<px4::params::AG_TKO_MAX_ROLL>)  _param_ag_tko_max_roll
 
 
 	)

src/modules/fw_pos_control_l1/autogyro_takeoff/autogyro_takeoff_parameters.c

@@ -99,3 +99,33 @@ PARAM_DEFINE_FLOAT(AG_ROTOR_RPM, 1000.0);
  * @value 10 Not implemented: SITL in flightgear
  */
 PARAM_DEFINE_INT32(AG_PROT_TYPE, 0);
+
+
+/**
+ * Altitude AGL at which we have enough ground clearance to allow some roll.
+ *
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 1
+ * @group Autogyro
+ */
+PARAM_DEFINE_FLOAT(AG_NAV_ALT, 5.0);
+
+/**
+ * Max roll during climbout.
+ *
+ * Roll is limited during climbout to ensure enough lift and prevents aggressive
+ * navigation before we're on a safe height.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @decimal 1
+ * @increment 0.5
+ * @group Autogyro
+ */
+
+PARAM_DEFINE_FLOAT(AG_TKO_MAX_ROLL, 25.0);

src/modules/logger/logged_topics.cpp

@@ -50,7 +50,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
-	add_optional_topic("autogyro_takeoff_status", 100);
 	add_optional_topic("autotune_attitude_control_status", 100);
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
@@ -105,7 +104,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("sensors_status_imu", 200);
 	add_optional_topic("spoilers_setpoint", 1000);
 	add_topic("system_power", 500);
-	add_optional_topic("takeoff_status", 1000);
+	add_topic("takeoff_status");
 	add_optional_topic("tecs_status", 200);
 	add_optional_topic("tiltrotor_extra_controls", 100);
 	add_topic("trajectory_setpoint", 200);