staging, rebased again...

range_alt_notes.txt

@@ -0,0 +1,20 @@
+## FlightTaskManualAltitude
+- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
+- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
+- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
+- _respectMinAltitude ... weird, remove? No need for a function
+
+
+## FlightTask
+- _dist_to_bottom and _dist_to_ground ... confusing, unify
+-
+
+## FlightTaskAuto
+- reuse logic for range alt lock
+- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
+
+## EKF
+- Vz does not reflect true Vz due to noisy baro
+- Vz errors cause rangefinder kinematic consistency check to fail
+- Position setpoint error remains over long periods (might be related to Vz issues above)
+

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -67,6 +67,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 				_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
+				// TODO: this is a constant
 				const float dist_var = getRngVar();
 				_rng_consistency_check.current_posD_reset_count = get_posD_reset_count();
 
@@ -79,6 +80,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				_rng_consistency_check.horizontal_motion = updated_horizontal_motion;
 				const float z_var = P(State::pos.idx + 2, State::pos.idx + 2);
 				const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2);
+
+				// TODO: review -- variance
 				_rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(),
 							   dist_var, imu_sample.time_us);
 

src/modules/ekf2/EKF2.cpp

@@ -1601,8 +1601,18 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+
+	// TODO: this makes dist_bottom a function of the Terrain State estimate, which is not
+	// exactly correct. Terrain State can diverge from the distance_sensor.current_distance
+	// when fusion is disabled. This causes Terrain Hold to drift due to dist_bottom diverging.
+	// Ultimately the logic for rangefinder fusion needs to be improved, and when distance_sensor
+	// is not fused into the Terrain State estimate we should not use dist_bottom for Altitude Lock.
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
+	// TODO: review, we should use sensor variance not terrain
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
+	// Variance can be hardcoded to: EKF2_RNG_NOISE + (distance * EKF2_RNG_SFE)
+	// lpos.dist_bottom_var = _params->range_noise + (_params->range_noise_scaler);
+
 	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;

src/modules/ekf2/params_range_finder.yaml

@@ -48,7 +48,7 @@ parameters:
       description:
         short: Measurement noise for range finder fusion
       type: float
-      default: 0.1
+      default: 0.05
       min: 0.01
       unit: m
       decimal: 2
@@ -122,7 +122,7 @@ parameters:
         short: Range finder range dependent noise scaler
         long: Specifies the increase in range finder noise with range.
       type: float
-      default: 0.05
+      default: 0.01
       min: 0.0
       max: 0.2
       unit: m/m

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -95,6 +95,122 @@ void FlightTaskManualAltitude::_scaleSticks()
 	_velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2);
 }
 
+void FlightTaskManualAltitude::_updateAltitudeLock()
+{
+	// - check if sticks are released (braking)
+	// - check if no vertical motion (altitude lock) (noisy baro, increase default threshold)
+	// - check if horizontal motion is within limit (altitude lock)
+
+	switch ((AltitudeMode)_param_mpc_alt_mode.get()) {
+	case AltitudeMode::AltitudeFollow: {
+			// Altitude following - relative earth frame origin which may drift due to sensors
+			// TODO
+			break;
+		}
+
+	case AltitudeMode::TerrainFollow: {
+			// Terrain following - relative to ground (requires distance sensor) which changes with terrain variation
+			// TODO
+			break;
+		}
+
+	case AltitudeMode::TerrainHold: {
+			// Terrain hold - relative to ground when within thresholds (No Z vel, limited XY vel)
+			// - MPC_HOLD_MAX_Z
+			// - MPC_HOLD_MAX_XY
+			handle_terrain_hold_mode();
+			break;
+		}
+
+	case AltitudeMode::None: {
+			// Nothing to do
+			break;
+		}
+	}
+}
+
+void FlightTaskManualAltitude::handle_terrain_hold_mode()
+{
+	// TODO:
+	// - check dist_bottom_valid before using dist_bottom
+	// - use dist_bottom_var as heuristic for distance lock
+
+	if (!PX4_ISFINITE(_dist_to_bottom)) {
+		// Cannot perform terrain hold without distance sensor
+		// TODO: log error?
+		return;
+	}
+
+	// Check if user is adjusting throttle, velocity setpoint mode
+	if (fabsf(_sticks.getPositionExpo()(2)) > 0.001f) {
+		// PX4_INFO("USER IN CONTROL");
+		if (PX4_ISFINITE(_position_setpoint(2)) || PX4_ISFINITE(_dist_to_bottom_lock)) {
+			PX4_INFO("Setting position sp to NAN");
+			_current_mode = AltitudeMode::None;
+			_position_setpoint(2) = NAN;
+			_dist_to_bottom_lock = NAN;
+		}
+
+		return;
+	}
+
+	// Check if Z and XY velocities are within limit to active Terrain Hold
+	bool z_vel_okay = fabsf(_velocity(2)) < _param_mpc_hold_max_z.get();
+	bool xy_vel_okay = Vector2f(_velocity).length() < _param_mpc_hold_max_xy.get();
+
+	if (z_vel_okay && xy_vel_okay) {
+		// XYZ all within limit for Terrain Hold
+		// Update position setpoint to keep fixed distance from terrain
+		if (_current_mode != AltitudeMode::TerrainHold) {
+			_current_mode = AltitudeMode::TerrainHold;
+
+			// TODO: Estimate distance until stopped and use that distance as the alt lock distance
+
+			// Lock to dist_to_bottom
+			PX4_INFO("Locking to dist bottom: %f", (double)_dist_to_bottom);
+			_dist_to_bottom_lock = _dist_to_bottom;
+			_position_setpoint(2) = _position(2);
+
+		} else {
+			// TODO: we shouldn't need to check NAN of _dist_to_bottom_lock
+			if (PX4_ISFINITE(_dist_to_bottom_lock)) {
+				float delta_distance = _dist_to_bottom - _dist_to_bottom_lock;
+				// PX4_INFO("Updating setpoint: delta_distance %f", (double)delta_distance);
+				// PX4_INFO("delta_distance: %f", (double)delta_distance);
+				_position_setpoint(2) = _position(2) + delta_distance;
+				// PX4_INFO("_position_setpoint(2): %f", (double)_position_setpoint(2));
+			} else {
+				PX4_INFO("_dist_to_bottom_lock is NAN, this shouldn't happen");
+			}
+		}
+
+	} else {
+		// Either Z_vel or XY_vel are above threshold, just follow altitude setpoint
+		// Lock the position setpoint to the current Z position estimate
+		if (_current_mode != AltitudeMode::AltitudeFollow) {
+			_current_mode = AltitudeMode::AltitudeFollow;
+
+			PX4_INFO("Locking to Z estimate");
+			_position_setpoint(2) = _position(2);
+			_dist_to_bottom_lock = NAN;
+		} else {
+			// Nothing to do, we do not adjust the setpoint
+		}
+
+	}
+}
+
+void FlightTaskManualAltitude::handle_terrain_follow_mode()
+{
+
+}
+
+void FlightTaskManualAltitude::handle_altitude_follow_mode()
+{
+
+}
+
+#ifdef false
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
 	// Depending on stick inputs and velocity, position is locked.
@@ -124,12 +240,12 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 				_terrain_hold = false;
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
-				if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
-					_position_setpoint(2) = _position(2) - (_dist_to_ground_lock - _dist_to_bottom);
+				if (PX4_ISFINITE(_dist_to_bottom_lock) && PX4_ISFINITE(_dist_to_bottom)) {
+					_position_setpoint(2) = _position(2) - (_dist_to_bottom_lock - _dist_to_bottom);
 
 				} else {
 					_position_setpoint(2) = _position(2);
-					_dist_to_ground_lock = NAN;
+					_dist_to_bottom_lock = NAN;
 				}
 			}
 
@@ -142,7 +258,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 				// Adjust the setpoint to maintain the same height error to reduce control transients
 				if (PX4_ISFINITE(_position_setpoint(2))) {
-					_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+					_dist_to_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
 				}
 			}
 		}
@@ -166,7 +282,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 				_terrainFollowing(apply_brake, stopped);
 
 			} else {
-				_dist_to_ground_lock = NAN;
+				_dist_to_bottom_lock = NAN;
 			}
 
 		} else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
@@ -186,6 +302,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 
 	_respectMaxAltitude();
 }
+#endif
 
 void FlightTaskManualAltitude::_respectMinAltitude()
 {
@@ -198,7 +315,7 @@ void FlightTaskManualAltitude::_respectMinAltitude()
 
 void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 {
-	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
+	if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_bottom_lock)) {
 		// User wants to break and vehicle reached zero velocity. Lock height to ground.
 
 		// lock position
@@ -206,19 +323,19 @@ void FlightTaskManualAltitude::_terrainFollowing(bool apply_brake, bool stopped)
 		// ensure that minimum altitude is respected
 		_respectMinAltitude();
 		// lock distance to ground but adjust first for minimum altitude
-		_dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
+		_dist_to_bottom_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2));
 
-	} else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) {
+	} else if (apply_brake && PX4_ISFINITE(_dist_to_bottom_lock)) {
 		// vehicle needs to follow terrain
 
 		// difference between the current distance to ground and the desired distance to ground
-		const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom;
+		const float delta_distance_to_ground = _dist_to_bottom_lock - _dist_to_bottom;
 		// adjust position setpoint for the delta (note: NED frame)
 		_position_setpoint(2) = _position(2) - delta_distance_to_ground;
 
 	} else {
 		// user demands velocity change in D-direction
-		_dist_to_ground_lock = _position_setpoint(2) = NAN;
+		_dist_to_bottom_lock = _position_setpoint(2) = NAN;
 	}
 }
 
@@ -269,7 +386,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
 {
-	_dist_to_ground_lock = NAN;
+	PX4_INFO("_ekfResetHandlerHagl");
+	_dist_to_bottom_lock = NAN;
 }
 
 void FlightTaskManualAltitude::_updateSetpoints()

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -45,6 +45,13 @@
 #include "StickTiltXY.hpp"
 #include <uORB/Subscription.hpp>
 
+enum class AltitudeMode : int {
+	AltitudeFollow,
+	TerrainFollow,
+	TerrainHold,
+	None,
+};
+
 class FlightTaskManualAltitude : public FlightTask
 {
 public:
@@ -76,7 +83,8 @@ protected:
 	StickYaw _stick_yaw{this};
 
 	bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
-	bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+	// bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
+	AltitudeMode _current_mode = AltitudeMode::AltitudeFollow;
 
 	float _velocity_constraint_up{INFINITY};
 	float _velocity_constraint_down{INFINITY};
@@ -94,6 +102,10 @@ protected:
 					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
 				       )
 private:
+
+	void handle_terrain_hold_mode();
+	void handle_terrain_follow_mode();
+	void handle_altitude_follow_mode();
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
@@ -132,7 +144,7 @@ private:
 	 * Distance to ground during terrain following.
 	 * If user does not demand velocity change in D-direction and the vehcile
 	 * is in terrain-following mode, then height to ground will be locked to
-	 * _dist_to_ground_lock.
+	 * _dist_to_bottom_lock.
 	 */
-	float _dist_to_ground_lock = NAN;
+	float _dist_to_bottom_lock = NAN;
 };

src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -107,7 +107,7 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 	_acceleration_setpoint(2) = _smoothing.getCurrentAcceleration();
 	_velocity_setpoint(2) = _smoothing.getCurrentVelocity();
 
-	if (!_terrain_hold) {
+	if (_current_mode != AltitudeMode::TerrainHold) {
 		if (_terrain_hold_previous) {
 			// Reset position setpoint to current position when switching from terrain hold to non-terrain hold
 			_smoothing.setCurrentPosition(_position(2));
@@ -116,5 +116,5 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState()
 		_position_setpoint(2) = _smoothing.getCurrentPosition();
 	}
 
-	_terrain_hold_previous = _terrain_hold;
+	_terrain_hold_previous = _current_mode == AltitudeMode::TerrainHold;
 }

src/modules/logger/logged_topics.cpp

@@ -315,6 +315,10 @@ void LoggedTopics::add_estimator_replay_topics()
 	add_topic("vehicle_visual_odometry");
 	add_topic("aux_global_position");
 	add_topic_multi("distance_sensor");
+	add_topic("vehicle_local_position");
+	add_topic("estimator_states");
+	add_topic("estimator_status");
+	add_topic("estimator_aid_src_rng_hgt");
 }
 
 void LoggedTopics::add_thermal_calibration_topics()