diff --git a/range_alt_notes.txt b/range_alt_notes.txt new file mode 100644 index 0000000000..cae965ac8c --- /dev/null +++ b/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) + diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 422c594e29..7507abd93e 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/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); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 319dbfd926..b4fc5f2458 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1601,8 +1601,18 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) #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; diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 90525178af..68f8a9107f 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/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 diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 2f27fd1aed..61d16c8078 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/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() diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 8386dfedad..83a349b7fc 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -45,6 +45,13 @@ #include "StickTiltXY.hpp" #include +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; }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 351daa86bf..cbb4e7c234 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/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; } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index faaf26dace..1f7ab5cdac 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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()