Skip to content

Commit

Permalink
flight_mode_manager: invalidate dist to bottom/ground on terrain reset
Browse files Browse the repository at this point in the history
 - this allows the terrain hold to simply reset on the next valid update
   rather than trying to adjust the setpoints
  • Loading branch information
dagar committed Oct 4, 2024
1 parent 34ee097 commit ad28c58
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ void FlightTask::_checkEkfResetCounters()
if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) {
_ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom);
_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;

_dist_to_bottom = NAN;
}

if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -279,6 +279,11 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
_stick_yaw.ekfResetHandler(delta_psi);
}

void FlightTaskManualAltitude::_ekfResetHandlerHagl(float delta_hagl)
{
_dist_to_ground_lock = NAN;
}

void FlightTaskManualAltitude::_updateSetpoints()
{
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ class FlightTaskManualAltitude : public FlightTask

protected:
void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
void _ekfResetHandlerHagl(float delta_hagl) override;

virtual void _updateSetpoints(); /**< updates all setpoints */
virtual void _scaleSticks(); /**< scales sticks to velocity in z */
bool _checkTakeoff() override;
Expand Down

0 comments on commit ad28c58

Please sign in to comment.