rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1156: add path_direction as a source for yaw as suggested by Werner :)

Browse Source
This commit is contained in:
Corvus Corax 2014-08-12 18:18:07 +02:00
parent ea4e7962a6
commit 56e4127bca
1 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
flight/modules/PathFollower/pathfollower.c
View File

@ -122,6 +122,7 @@ static uint8_t updateAutoPilotFixedWing();
static uint8_t updateAutoPilotVtol();
static float updateTailInBearing();
static float updateCourseBearing();
static float updatePathBearing();
static float updatePOIBearing();
static void processPOI();
static void updatePathVelocity(float kFF, float kH, float kV, bool limited);
@ -355,9 +356,12 @@ static uint8_t updateAutoPilotVtol()
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
yaw = updateTailInBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE:
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
yaw = updateCourseBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
yaw = updatePathBearing();
break;
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
yaw = updatePOIBearing();
break;
@ -412,6 +416,33 @@ static float updateCourseBearing()
return yaw;
}
/**
* Compute bearing of current path direction
*/
static float updatePathBearing()
{
PositionStateData positionState;
PositionStateGet(&positionState);
float cur[3] = { positionState.North,
positionState.East,
positionState.Down };
struct path_status progress;
path_progress(cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
cur, &progress, pathDesired.Mode);
// atan2f always returns in between + and - 180 degrees
float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]));
// result is in between 0 and 360 degrees
if (yaw < 0.0f) {
yaw += 360.0f;
}
return yaw;
}
/**
* Compute bearing between current position and POI
*/