split logic into simpler functions

src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.cpp

@@ -44,14 +44,11 @@ using namespace matrix;
 bool FlightTaskPrecisionLanding::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
-	PX4_INFO("precland activated");
-
 	_precland_state = PRECLAND_STATE::AUTORTL_CLIMB;
 
 	_position_setpoint = _position;
 	_search_count = 0;
 
-	// TODO: Use target orientation instead
 	_initial_yaw = _yaw;
 	_initial_position = _position;
 
@@ -72,45 +69,118 @@ bool FlightTaskPrecisionLanding::inside_acceptance_radius()
 
 bool FlightTaskPrecisionLanding::precision_target_available()
 {
-	// TODO: Add timeout
 	const bool ever_received = _landing_target_pose.timestamp != 0;
 	const bool timed_out =  hrt_absolute_time() - _landing_target_pose.timestamp > _param_pld_btout.get() * SEC2USEC;
 	return ever_received && !timed_out;
 }
 
-bool FlightTaskPrecisionLanding::update()
+void FlightTaskPrecisionLanding::generate_pos_xy_setpoints()
 {
-	// Get setpoints from FlightTask and later override if necessary
+	switch (_precland_state) {
-	bool ret = FlightTask::update();
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+		_position_setpoint(0) = _initial_position(0);
+		_position_setpoint(1) = _initial_position(1);
+		break;
 
-	// Fetch uorb
+	case PRECLAND_STATE::AUTORTL_APPROACH:
-	if (_landing_target_pose_sub.updated()) {
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
-		_landing_target_pose_sub.copy(&_landing_target_pose);
 
-		_precision_target_ned(0) = _landing_target_pose.x_abs;
+		// Horizontal approach to home or precision target
-		_precision_target_ned(1) = _landing_target_pose.y_abs;
+		if (precision_target_available()) {
+			_position_setpoint(0) = _landing_target_pose.x_abs;
+			_position_setpoint(1) = _landing_target_pose.y_abs;
 
-		// Supplement precision target altitude with home postion's altitude if necessary
+		} else {
-		_precision_target_ned(2) = PX4_ISFINITE(_landing_target_pose.z_rel) ? _landing_target_pose.z_rel :
+			_position_setpoint(0) = _sub_home_position.get().x;
-					   _sub_home_position.get().z;
+			_position_setpoint(1) = _sub_home_position.get().y;
+		}
+
+		break;
+
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+	case PRECLAND_STATE::LANDING:
+		_position_setpoint(0) = _landing_target_pose.x_abs;
+		_position_setpoint(1) = _landing_target_pose.y_abs;
+		break;
 	}
+}
 
-	// Fetch runtime parameters
+void FlightTaskPrecisionLanding::generate_pos_z_setpoints()
-	const float max_search_duration = _param_pld_srch_tout.get();
+{
 	const float search_rel_altitude = _param_pld_srch_alt.get();
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+		// Horizontal approach to home or precision target
+		_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
+		break;
 
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+		// TODO: Should use current altitude when state was entered as the altitude setpoint
+		if (PX4_ISFINITE(_landing_target_pose.z_abs)){
+			// For safety reasons take whichever reference altitude is higher up (lower in NED)
+			_position_setpoint(2) = math::min(_sub_home_position.get().z, _landing_target_pose.z_abs) - search_rel_altitude;
+		} else {
+			_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
+		}
+		break;
+
+	case PRECLAND_STATE::LANDING:
+		_position_setpoint(2) = NAN;
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::generate_vel_setpoints()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+		_velocity_setpoint.setNaN();
+		break;
+
+	case PRECLAND_STATE::LANDING:
+		_velocity_setpoint(0) = 0;
+		_velocity_setpoint(1) = 0;
+		_velocity_setpoint(2) = _param_mpc_land_speed.get();
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::generate_acc_setpoints()
+{
+	_acceleration_setpoint.setNaN();
+}
+
+void FlightTaskPrecisionLanding::generate_yaw_setpoint()
+{
+	switch (_precland_state) {
+	case PRECLAND_STATE::AUTORTL_CLIMB:
+		_yaw_setpoint = _initial_yaw;
+		break;
+
+	case PRECLAND_STATE::AUTORTL_APPROACH:
+		// TODO: Point in direction of flight
+	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
+	case PRECLAND_STATE::SEARCHING_TARGET:
+	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
+	case PRECLAND_STATE::LANDING:
+		_yaw_setpoint = _target_yaw;
+		break;
+	}
+}
+
+void FlightTaskPrecisionLanding::check_state_transitions()
+{
 	switch (_precland_state) {
 	case PRECLAND_STATE::AUTORTL_CLIMB:
 		PX4_INFO("AUTORTL_CLIMB");
-		_position_setpoint(0) = _initial_position(0);
-		_position_setpoint(1) = _initial_position(1);
-		_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
-
-		_velocity_setpoint.setNaN(); // TODO: Is there a RTL climb speed?
-		_acceleration_setpoint.setNaN();
-
-		_yaw_setpoint = _initial_yaw;
-
 		// transition condition
 		// Wait for drone to reach z position setpoint or be higher
 		if (_position(2) <= _position_setpoint(2) + _param_nav_mc_alt_rad.get()) {
@@ -123,21 +193,6 @@ bool FlightTaskPrecisionLanding::update()
 	case PRECLAND_STATE::AUTORTL_APPROACH: {
 			PX4_INFO("AUTORTL_APPROACH");
 
-			// Horizontal approach to home or precision target
-			if (precision_target_available()) {
-				_position_setpoint(0) = _precision_target_ned(0);
-				_position_setpoint(1) = _precision_target_ned(1);
-				_position_setpoint(2) = _precision_target_ned(2);
-
-			} else {
-				PX4_INFO("reverting to HOME...");
-				_position_setpoint(0) = _sub_home_position.get().x;
-				_position_setpoint(1) = _sub_home_position.get().y;
-				_position_setpoint(2) = _sub_home_position.get().z - _param_rtl_return_alt.get();
-			}
-
-			_velocity_setpoint.setNaN();
-
 			// transition condition
 			// Wait for drone to reach position setpoint
 			if (inside_acceptance_radius()) {
@@ -159,16 +214,6 @@ bool FlightTaskPrecisionLanding::update()
 	case PRECLAND_STATE::MOVE_TO_SEARCH_ALTITUDE:
 		PX4_INFO("MOVE_TO_SEARCH_ALTITUDE");
 
-		// Ascend/Descend to search altitude
-		_position_setpoint(0) = _sub_home_position.get().x;
-		_position_setpoint(1) = _sub_home_position.get().y;
-		_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
-
-		_velocity_setpoint.setNaN();
-		_acceleration_setpoint.setNaN();
-
-		_yaw_setpoint = _target_yaw;
-
 		// transition condition
 		if (inside_acceptance_radius()) {
 			mavlink_log_info(&_mavlink_log_pub, "Starting search");
@@ -178,44 +223,36 @@ bool FlightTaskPrecisionLanding::update()
 		break;
 
 	case PRECLAND_STATE::SEARCHING_TARGET:
-		PX4_INFO("SEARCHING_TARGET");
+		{
+			PX4_INFO("SEARCHING_TARGET");
+			const float max_search_duration = _param_pld_srch_tout.get();
 
-		_yaw_setpoint = _target_yaw;
+			// Currently no search pattern is implemented. Just wait for target...
+			// Workaround for Orion app making unwanted changes to my parameters
+			if ((hrt_absolute_time() - _state_start_time) > max_search_duration * SEC2USEC) {
+				mavlink_log_info(&_mavlink_log_pub, "Search timed out");
 
-		// Currently no search pattern is implemented. Just wait for target...
+				if (++_search_count >= _param_pld_max_srch.get()) {
-		// Workaround for Orion app making unwanted changes to my parameters
+					mavlink_log_info(&_mavlink_log_pub, "Abandonning search and landing immediately");
-		if ((hrt_absolute_time() - _state_start_time) > max_search_duration * SEC2USEC) {
+					do_state_transition(PRECLAND_STATE::LANDING);
-			mavlink_log_info(&_mavlink_log_pub, "Search timed out");
 
-			if (++_search_count >= _param_pld_max_srch.get()) {
+				} else {
-				mavlink_log_info(&_mavlink_log_pub, "Abandonning search and landing immediately");
+					mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
-				do_state_transition(PRECLAND_STATE::LANDING);
+					do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
-
+				}
-			} else {
-				mavlink_log_info(&_mavlink_log_pub, "Moving to search center");
-				do_state_transition(PRECLAND_STATE::SEARCHING_TARGET);
 			}
-		}
 
-		// transition condition
+			// transition condition
-		if (precision_target_available()) {
+			if (precision_target_available()) {
-			mavlink_log_info(&_mavlink_log_pub, "Moving above target");
+				mavlink_log_info(&_mavlink_log_pub, "Moving above target");
-			do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
+				do_state_transition(PRECLAND_STATE::MOVING_ABOVE_TARGET);
-		}
+			}
 
-		break;
+			break;
+		}
 
 	case PRECLAND_STATE::MOVING_ABOVE_TARGET:
 		PX4_INFO("MOVING_ABOVE_TARGET");
-		// TODO: Should use current altitude when state was entered as the altitude setpoint
-		_position_setpoint(0) = _precision_target_ned(0);
-		_position_setpoint(1) = _precision_target_ned(1);
-		_position_setpoint(2) = _sub_home_position.get().z - search_rel_altitude;
-
-		_velocity_setpoint.setNaN();
-		_acceleration_setpoint.setNaN();
-
-		_yaw_setpoint = _target_yaw;
 
 		// Transition condition
 		// - Check acceptance radius and
@@ -237,23 +274,28 @@ bool FlightTaskPrecisionLanding::update()
 
 		break;
 
-
 	case PRECLAND_STATE::LANDING:
 		PX4_INFO("LANDING");
-		// Control XY position and Z velocity
-		_position_setpoint(0) = _precision_target_ned(0);
-		_position_setpoint(1) = _precision_target_ned(1);
-		_position_setpoint(2) = NAN;
-
-		_velocity_setpoint(0) = 0;
-		_velocity_setpoint(1) = 0;
-		_velocity_setpoint(2) = _param_mpc_land_speed.get();
-		_acceleration_setpoint(2) = NAN;
-
-		_yaw_setpoint = _target_yaw;
 		break;
-
 	}
+}
+
+bool FlightTaskPrecisionLanding::update()
+{
+	// Get setpoints from FlightTask and later override if necessary
+	bool ret = FlightTask::update();
+
+	// Fetch uorb
+	if (_landing_target_pose_sub.updated()) {
+		_landing_target_pose_sub.copy(&_landing_target_pose);
+	}
+
+	generate_pos_xy_setpoints();
+	generate_pos_z_setpoints();
+	generate_vel_setpoints();
+	generate_acc_setpoints();
+	generate_yaw_setpoint();
+	check_state_transitions();
 
 	precision_landing_status_s precision_landing_status{};
 	precision_landing_status.timestamp = hrt_absolute_time();

src/modules/flight_mode_manager/tasks/PrecisionLanding/FlightTaskPrecisionLanding.hpp

@@ -92,6 +92,15 @@ private:
 
 	bool precision_target_available();
 
+	void generate_setpoints();
+	void generate_pos_xy_setpoints();
+	void generate_pos_z_setpoints();
+	void generate_vel_setpoints();
+	void generate_acc_setpoints();
+	void generate_yaw_setpoint();
+
+	void check_state_transitions();
+
 	PRECLAND_STATE _precland_state;
 
 	uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
@@ -106,6 +115,4 @@ private:
 	matrix::Vector3f _initial_position;
 
 	orb_advert_t	_mavlink_log_pub{nullptr};
-	matrix::Vector3f _precision_target_ned = {NAN, NAN, NAN};
-
 };