rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1900 have path_progress updated correctly for leg_remaining and error_below end conditions to work in fixedwingautotakeoff

Browse Source
This commit is contained in:
Corvus Corax 2015-06-04 12:39:17 +02:00
parent 23dab7e0b6
commit cc3b10060f
2 changed files with 26 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/modules/PathFollower/fixedwingautotakeoffcontroller.cpp
View File

@ -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);

8
flight/modules/PathFollower/fixedwinglandcontroller.cpp
View File

@ -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;
}
/**