dr rtl flight task: clean up and commenting

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp

@@ -40,6 +40,7 @@
 bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	PX4_INFO("FlightTaskReturnDeadReckoning::activate");
+
 	if (!FlightTask::activate(last_setpoint)) {
 		PX4_ERR("Failed to activate task");
 		return false;
@@ -75,7 +76,7 @@ bool FlightTaskReturnDeadReckoning::update()
 	_updateSubscriptions();
 
 	if (_isGlobalPositionValid()) {
-		// Update the bearing to home
+		// Update the bearing to home if global position is valid
 		if (!_updateBearingToHome()) {
 			PX4_ERR("Failed to compute bearing to home");
 			return false;
@@ -91,84 +92,88 @@ bool FlightTaskReturnDeadReckoning::update()
 void FlightTaskReturnDeadReckoning::_updateState()
 {
 	switch (_state) {
-		case State::INIT:
+	case State::INIT:
-			if (_isAboveReturnAltitude()) {
+		if (_isAboveReturnAltitude()) {
-				_state = State::RETURN;
+			_state = State::RETURN;
-				PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
+			PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
-					 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
+				 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
-			} else {
-				_state = State::ASCENT;
-				PX4_INFO("Ascending to return altitude %.2fm (MSL) with bearing %.2f deg",
-					 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
-			}
-			break;
 
-		case State::ASCENT:
+		} else {
-			if (_isAboveReturnAltitude()) {
+			_state = State::ASCENT;
-				_state = State::RETURN;
+			PX4_INFO("Ascending to return altitude %.2fm (MSL) with bearing %.2f deg",
-				PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
+				 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
-					 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
+		}
-			}
-			break;
 
-		case State::RETURN:
+		break;
-			if (_isWithinHomePositionRadius()) {
-				_state = State::HOLD;
-				PX4_INFO("Holding altitude at %.2fm (MSL) over home position", (double) _rtl_alt);
-			}
-			break;
 
-		case State::HOLD:
+	case State::ASCENT:
-			break;
+		if (_isAboveReturnAltitude()) {
+			_state = State::RETURN;
+			PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
+				 (double) _rtl_alt, (double) math::degrees(_bearing_to_home));
+		}
 
-		default:
+		break;
-			PX4_ERR("Unknown state");
+
-			return;
+	case State::RETURN:
+		if (_isWithinHomePositionRadius()) {
+			_state = State::HOLD;
+			PX4_INFO("Holding altitude at %.2fm (MSL) over home position", (double) _rtl_alt);
+		}
+
+		break;
+
+	case State::HOLD:
+		break;
+
+	default:
+		PX4_ERR("Unknown state");
+		return;
 	}
 }
 
 void FlightTaskReturnDeadReckoning::_updateSetpoints()
 {
 	switch (_state) {
-		case State::INIT:
+	case State::INIT:
-			_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
-			_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
-			_slew_rate_velocity_z.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
 
-			// Hold current altitude
+		// Hold current altitude
-			_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
+		_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
-			break;
+		break;
 
-		case State::ASCENT:
+	case State::ASCENT:
-			_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
-			_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
-			_slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime);
+		_slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime);
 
-			// Ascent until reaching the return altitude
+		// Ascent until reaching the return altitude
-			_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
+		_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
-			break;
+		break;
 
-		case State::RETURN:
+	case State::RETURN:
-			_slew_rate_acceleration_x.update(_rtl_acc*cosf(_bearing_to_home), _deltatime);
+		_slew_rate_acceleration_x.update(_rtl_acc * cosf(_bearing_to_home), _deltatime);
-			_slew_rate_acceleration_y.update(_rtl_acc*sinf(_bearing_to_home), _deltatime);
+		_slew_rate_acceleration_y.update(_rtl_acc * sinf(_bearing_to_home), _deltatime);
-			_slew_rate_velocity_z.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
 
-			// Stay at the return altitude
+		// Stay at the return altitude
-			_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
+		_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
-			break;
+		break;
 
-		case State::HOLD:
+	case State::HOLD:
-			_slew_rate_acceleration_x.update(0.0f, _deltatime);
+		_slew_rate_acceleration_x.update(0.0f, _deltatime);
-			_slew_rate_acceleration_y.update(0.0f, _deltatime);
+		_slew_rate_acceleration_y.update(0.0f, _deltatime);
-			_slew_rate_velocity_z.update(0.0f, _deltatime);
+		_slew_rate_velocity_z.update(0.0f, _deltatime);
 
-			// Stay at the return altitude
+		// Stay at the return altitude
-			_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
+		_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
-			break;
+		break;
 
-		default:
+	default:
-			PX4_ERR("Unknown state");
+		PX4_ERR("Unknown state");
-			return;
+		return;
 
 	};
 
@@ -179,16 +184,20 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
 
 	// Acceleration setpoint
 	_acceleration_setpoint.xy() = matrix::Vector2f(
-		_slew_rate_acceleration_x.getState(),
+					      _slew_rate_acceleration_x.getState(),
-		_slew_rate_acceleration_y.getState()
+					      _slew_rate_acceleration_y.getState()
-	);
+				      );
 
 	return;
 }
 
-void FlightTaskReturnDeadReckoning::_updateSubscriptions()
+bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
 {
-	_sub_vehicle_global_position.update();
+	if (_readGlobalPosition(_start_vehicle_global_position) && _readHomePosition(_home_position)) {
+		_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
+	}
+
+	return !isnanf(_bearing_to_home);
 }
 
 bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_position)
@@ -218,21 +227,14 @@ bool FlightTaskReturnDeadReckoning::_readGlobalPosition(matrix::Vector3d &global
 	return true;
 }
 
-bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
+float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start,
-{
+		const matrix::Vector3d &_global_position_end)
-	if (_readGlobalPosition(_start_vehicle_global_position) && _readHomePosition(_home_position)) {
-		_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
-	}
-
-	return !isnanf(_bearing_to_home);
-}
-
-float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start, const matrix::Vector3d &_global_position_end)
 {
 	float bearing = NAN;
+
 	if (_global_position_start.isAllFinite() && _global_position_end.isAllFinite()) {
 		bearing = get_bearing_to_next_waypoint(_global_position_start(0), _global_position_start(1),
-				     		       _global_position_end(0), _global_position_end(1));
+						       _global_position_end(0), _global_position_end(1));
 	}
 
 	return bearing;
@@ -248,6 +250,11 @@ bool FlightTaskReturnDeadReckoning::_computeReturnParameters()
 	return true;
 }
 
+void FlightTaskReturnDeadReckoning::_updateSubscriptions()
+{
+	_sub_vehicle_global_position.update();
+}
+
 bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
 {
 	// Initialize the heading smoother
@@ -268,9 +275,14 @@ bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
 	return true;
 }
 
+bool FlightTaskReturnDeadReckoning::_isGlobalPositionValid() const
+{
+	return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid;
+}
+
 bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
 {
-	float current_alt =_sub_vehicle_global_position.get().alt;
+	float current_alt = _sub_vehicle_global_position.get().alt;
 	float target_alt = _rtl_alt - _param_nav_mc_alt_rad.get();
 	return current_alt > target_alt;
 }
@@ -280,10 +292,9 @@ bool FlightTaskReturnDeadReckoning::_isWithinHomePositionRadius()
 	if (_isGlobalPositionValid()) {
 		_readGlobalPosition(_start_vehicle_global_position);
 		return get_distance_to_next_waypoint(
-			_start_vehicle_global_position(0), _start_vehicle_global_position(1),
+			       _start_vehicle_global_position(0), _start_vehicle_global_position(1),
-			_home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
+			       _home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
 	}
+
 	return false;
 }
-
-

src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp

@@ -50,16 +50,27 @@ public:
 	FlightTaskReturnDeadReckoning() = default;
 	virtual ~FlightTaskReturnDeadReckoning() = default;
 
-	bool update() override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool update() override;
 
 private:
 	uORB::SubscriptionData<vehicle_global_position_s> _sub_vehicle_global_position{ORB_ID(vehicle_global_position)};
 
+	/**
+	 * Update the flight task state machine
+	 */
 	void _updateState();
 
+	/**
+	 * Update the trajectory setpoints according to the current state
+	 */
 	void _updateSetpoints();
 
+	/**
+	 * Update bearing to home position with the last available GNSS position
+	 * @return true on success, false on error
+	 */
+	bool _updateBearingToHome();
 
 	/**
 	 * Initialize home position
@@ -73,15 +84,12 @@ private:
 	 */
 	bool _readGlobalPosition(matrix::Vector3d &global_position);
 
-
-	bool _updateBearingToHome();
-
 	/**
 	 * Compute bearing to home position
 	 * @return true on success, false on error
 	 */
 	float _computeBearing(const matrix::Vector3d &_global_position_start,
-				const matrix::Vector3d &_global_position_end);
+			      const matrix::Vector3d &_global_position_end);
 
 	/**
 	 * Computes return altitude velocity
@@ -89,25 +97,36 @@ private:
 	 */
 	bool _computeReturnParameters();
 
+	/**
+	 * Update uORB subscriptions
+	 */
+	void _updateSubscriptions();
+
 	/**
 	 * Initialize the trajectory setpoint smoothers
 	 * @return true on success, false on error
 	 */
 	bool _initializeSmoothers();
 
-	void _updateSubscriptions();
+	/**
-
+	 * Check if the global position is valid
-	bool _isGlobalPositionValid() const
+	 * @return true if valid, false otherwise
-	{
+	 */
-		return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid;
+	bool _isGlobalPositionValid() const;
-	}
 
+	/**
+	 * Check if the vehicle is above the return altitude
+	 * @return true if above, false otherwise
+	 */
 	bool _isAboveReturnAltitude() const;
 
+	/**
+	 * Check if the vehicle is within the home position waypoint acceptance radius
+	 * @return true if within, false otherwise
+	 */
 	bool _isWithinHomePositionRadius();
 
 
-	// Variable storing the home position
 	matrix::Vector3d _home_position;			/**< Stores home position */
 	matrix::Vector3d _start_vehicle_global_position;	/**< Stores vehicle last known GNSS position */
 	float _bearing_to_home{0.0f};				/**< Stores bearing between home and last GNSS position */
@@ -115,12 +134,13 @@ private:
 	HeadingSmoothing _heading_smoothing;			/**< Smoother for heading */
 	SlewRate<float> _slew_rate_acceleration_x{0.0f};	/**< Slew rate for x-acceleration setpoint */
 	SlewRate<float> _slew_rate_acceleration_y{0.0f};	/**< Slew rate for y-acceleration setpoint */
-	SlewRate<float> _slew_rate_velocity_z{0.0f};		/**< Smoother for vertical velocity */
+	SlewRate<float> _slew_rate_velocity_z{0.0f};		/**< Slew rate for vertical velocity */
 
 
-	float _rtl_alt{0.0f};			/**< Return altitude */
+	float _rtl_alt{0.0f};					/**< Return altitude */
-	float _rtl_acc{0.0f};			/**< Return acceleration */
+	float _rtl_acc{0.0f};					/**< Return acceleration */
 
+	/**< State machine for flight task */
 	enum class State {
 		UNKNOWN = 0,
 		INIT,
@@ -128,7 +148,7 @@ private:
 		RETURN,
 		HOLD
 	};
-	State _state{State::UNKNOWN};		/**< State machine for flight task */
+	State _state{State::UNKNOWN};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,