From cc3b10060f94b1061eb702aa67d8561e163f536a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 4 Jun 2015 12:39:17 +0200 Subject: [PATCH] OP-1900 have path_progress updated correctly for leg_remaining and error_below end conditions to work in fixedwingautotakeoff --- .../PathFollower/fixedwingautotakeoffcontroller.cpp | 18 ++++++++++++++++++ .../modules/PathFollower/fixedwinglandcontroller.cpp | 8 ++++++++ 2 files changed, 26 insertions(+) diff --git a/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp b/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp index 509805178..80b72d40d 100644 --- a/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp @@ -125,10 +125,22 @@ void FixedWingAutoTakeoffController::setAttitude(bool unsafe) StabilizationDesiredSet(&stabDesired); if (unsafe) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; } else { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); } + // calculate fractional progress based on altitude + float downPos; + PositionStateDownGet(&downPos); + if (fabsf(pathDesired->End.Down - pathDesired->Start.Down) < 1e-3f) { + pathStatus->fractional_progress = 1.0f; + pathStatus->error = 0.0f; + } else { + pathStatus->fractional_progress = (downPos - pathDesired->Start.Down) / (pathDesired->End.Down - pathDesired->Start.Down); + } + pathStatus->error = fabsf(downPos - pathDesired->End.Down); + PathStatusSet(pathStatus); } @@ -178,6 +190,12 @@ void FixedWingAutoTakeoffController::init_launch(void) { // find out vector direction of *runway* (if any) // and align, otherwise just stay straight ahead + pathStatus->path_direction_north = 0.0f; + pathStatus->path_direction_east = 0.0f; + pathStatus->path_direction_down = 0.0f; + pathStatus->correction_direction_north = 0.0f; + pathStatus->correction_direction_east = 0.0f; + pathStatus->correction_direction_down = 0.0f; if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f && fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) { AttitudeStateYawGet(&initYaw); diff --git a/flight/modules/PathFollower/fixedwinglandcontroller.cpp b/flight/modules/PathFollower/fixedwinglandcontroller.cpp index 52727eceb..d11d4c4fe 100644 --- a/flight/modules/PathFollower/fixedwinglandcontroller.cpp +++ b/flight/modules/PathFollower/fixedwinglandcontroller.cpp @@ -103,6 +103,14 @@ int32_t FixedWingLandController::Initialize(FixedWingPathFollowerSettingsData *p void FixedWingLandController::resetGlobals() { pathStatus->path_time = 0.0f; + pathStatus->path_direction_north = 0.0f; + pathStatus->path_direction_east = 0.0f; + pathStatus->path_direction_down = 0.0f; + pathStatus->correction_direction_north = 0.0f; + pathStatus->correction_direction_east = 0.0f; + pathStatus->correction_direction_down = 0.0f; + pathStatus->error = 0.0f; + pathStatus->fractional_progress = 0.0f; } /** -- 2.11.4.GIT