mc_pos_control: use correct altitude as limit

fadfdasf

src/modules/land_detector/land_detector_params.c

@@ -206,7 +206,7 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0);
  * hard altitude limit. This setting will
  * be consolidated with the GF_MAX_VER_DIST
  * parameter.
- * A negative value indicates that there is no limit
+ * A negative value indicates no altitude limitation.
  *
  * @unit m
  * @min -1

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -919,28 +919,30 @@ MulticopterPositionControl::reset_alt_sp()
 void
 MulticopterPositionControl::limit_altitude()
 {
-	if (_vehicle_land_detected.alt_max > 0.0f) {
+	if (_vehicle_land_detected.alt_max < 0.0f) {
-		/* in altitude control, limit setpoint */
+		// there is no altitude limitation present
-		if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
+		return;
-			_pos_sp(2) = -_vehicle_land_detected.alt_max;
+	}
-			return;
-		}
 
-		/* in velocity control mode and want to fly upwards */
+	float altitude_above_home = -(_pos(2) - _home_pos.z);
-		if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
 
-			/* time to travel to reach zero velocity */
+	if (_run_alt_control && (altitude_above_home > _vehicle_land_detected.alt_max)) {
-			float delta_t = -_vel(2) / _acceleration_z_max_down.get();
+		// we are above maximum altitude
+		_pos_sp(2) = -_vehicle_land_detected.alt_max +  _home_pos.z;
 
-			/* predicted position */
+	} else if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
-			float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
+		// we want to fly upwards: check if vehicle does not exceed altitude
-					   _acceleration_z_max_down.get() * delta_t *delta_t;
 
-			if (pos_z_next <= -_vehicle_land_detected.alt_max) {
+		// time to reach zero velocity
-				_pos_sp(2) = -_vehicle_land_detected.alt_max;
+		float delta_t = -_vel(2) / _acceleration_z_max_down.get();
-				_run_alt_control = true;
+
-				return;
+		// predict next position based on current position, velocity, max acceleration downwards and time to reach zero velocity
-			}
+		float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t;
+
+		if (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max) {
+			// prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint
+			_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
+			_run_alt_control = true;
 		}
 	}
 }

src/modules/navigator/enginefailure.cpp

@@ -126,9 +126,8 @@ EngineFailure::set_ef_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-
 	pos_sp_triplet->next.valid = false;
 
 	_navigator->set_position_setpoint_triplet_updated();

src/modules/navigator/follow_target.cpp

@@ -361,7 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
 
 	pos_sp_triplet->previous.valid = use_position;
 	pos_sp_triplet->previous = pos_sp_triplet->current;
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
 	pos_sp_triplet->current.position_valid = use_position;

src/modules/navigator/land.cpp

@@ -81,12 +81,13 @@ Land::on_activation()
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
 	pos_sp_triplet->previous.valid = false;
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
+
 	_navigator->set_can_loiter_at_sp(false);
+
 	_navigator->set_position_setpoint_triplet_updated();
 }
 

src/modules/navigator/loiter.cpp

@@ -129,7 +129,7 @@ Loiter::set_loiter_position()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	pos_sp_triplet->current.velocity_valid = false;
 	pos_sp_triplet->previous.valid = false;
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/mission.cpp

@@ -508,7 +508,7 @@ Mission::set_mission_items()
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
-		mission_apply_limitation(&_mission_item);
+		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
@@ -765,7 +765,7 @@ Mission::set_mission_items()
 			set_align_mission_item(&_mission_item, &mission_item_next_position);
 
 			/* set position setpoint to target during the transition */
-			mission_apply_limitation(&_mission_item);
+			mission_apply_limitation(_mission_item);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 		}
 
@@ -808,7 +808,7 @@ Mission::set_mission_items()
 	/*********************************** set setpoints and check next *********************************************/
 
 	/* set current position setpoint from mission item (is protected against non-position items) */
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 
 	/* issue command if ready (will do nothing for position mission items) */
@@ -835,7 +835,7 @@ Mission::set_mission_items()
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
-			mission_apply_limitation(&_mission_item);
+			mission_apply_limitation(_mission_item);
 			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
 
 		} else {
@@ -1187,7 +1187,7 @@ Mission::do_abort_landing()
 	_mission_item.origin = ORIGIN_ONBOARD;
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	_navigator->set_position_setpoint_triplet_updated();
 

src/modules/navigator/mission_block.cpp

@@ -751,22 +751,30 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
 }
 
 void
-MissionBlock::mission_apply_limitation(struct mission_item_s *item)
+MissionBlock::mission_apply_limitation(mission_item_s &item)
 {
+	/*
+	 * Limit altitude
+	 */
 
 	/* do nothing if altitude max is negative */
 	if (_navigator->get_land_detected()->alt_max > 0.0f) {
 
-		/* get absolut altitude */
+		/* absolute altitude */
-		float altitude_abs = item->altitude_is_relative
+		float altitude_abs = item.altitude_is_relative
-				     ? item->altitude + _navigator->get_home_position()->alt
+				     ? item.altitude + _navigator->get_home_position()->alt
-				     : item->altitude;
+				     : item.altitude;
 
+		/* limit altitude to maximum allowed altitude */
 		if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
-			item->altitude = item->altitude_is_relative ?
+			item.altitude = item.altitude_is_relative ?
-					 _navigator->get_land_detected()->alt_max :
+					_navigator->get_land_detected()->alt_max :
-					 _navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
+					_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
 
 		}
 	}
+
+	/*
+	 * Add other limitations here
+	 */
 }

src/modules/navigator/mission_block.h

@@ -121,10 +121,9 @@ protected:
 	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 
 	/**
-	 * This function limits the setpoint dependent
+	 * General function used to adjust the mission item based on vehicle specific limitations
-	 * on vehicle model
+	 */
-	*/
+	void	mission_apply_limitation(mission_item_s &item);
-	void	mission_apply_limitation(struct mission_item_s *item);
 
 	void issue_command(const mission_item_s &item);
 

src/modules/navigator/navigator.h

@@ -36,7 +36,6 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Thomas Gubler <thomasgubler@gmail.com>
- * @author Dennis Mannhart <dennis@px4.io>
  */
 
 #ifndef NAVIGATOR_H

src/modules/navigator/rcloss.cpp

@@ -140,7 +140,7 @@ RCLoss::set_rcl_item()
 	reset_mission_item_reached();
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/rtl.cpp

@@ -84,7 +84,7 @@ RTL::on_activation()
 {
 	set_current_position_item(&_mission_item);
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->next.valid = false;
@@ -292,7 +292,7 @@ RTL::set_rtl_item()
 	}
 
 	/* convert mission item to current position setpoint and make it valid */
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->next.valid = false;
 

src/modules/navigator/rtl.h

@@ -79,6 +79,7 @@ private:
 	 */
 	float 		get_rtl_altitude();
 
+
 	enum RTLState {
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,

src/modules/navigator/takeoff.cpp

@@ -92,7 +92,7 @@ Takeoff::on_active()
 		// set loiter item so position controllers stop doing takeoff logic
 		set_loiter_item(&_mission_item);
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		mission_apply_limitation(&_mission_item);
+		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		_navigator->set_position_setpoint_triplet_updated();
 	}
@@ -149,7 +149,7 @@ Takeoff::set_takeoff_position()
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	mission_apply_limitation(&_mission_item);
+	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.yaw_valid = true;

src/modules/systemlib/battery.cpp

@@ -225,7 +225,7 @@ void
 Battery::determineWarning(bool connected)
 {
 	if (connected) {
-		// propagate warning state only if the state is higher, otherwise remain in current waringin state
+		// propagate warning state only if the state is higher, otherwise remain in current warning state
 		if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
 			_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;