rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

Allow guidance to run purely off raw GPS data

Browse Source
This commit is contained in:
James Cotton 2012-04-15 16:23:34 -05:00
parent 050c756d54
commit a7ba6d96c1
2 changed files with 31 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
flight/Modules/Attitude/revolution/attitude.c
View File

@ -432,6 +432,13 @@ static int32_t updateAttitudeComplimentary(bool first_run)
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
NEDPositionData nedPosition;
NEDPositionGet(&nedPosition);
nedPosition.North = NED[0];
nedPosition.East = NED[1];
nedPosition.Down = NED[2];
NEDPositionSet(&nedPosition);
PositionActualData positionActual;
PositionActualGet(&positionActual);

27
flight/Modules/Guidance/guidance.c
View File

@ -58,6 +58,7 @@
#include "gpsposition.h"
#include "guidancesettings.h"
#include "nedaccel.h"
#include "nedposition.h"
#include "stabilizationdesired.h"
#include "stabilizationsettings.h"
#include "systemsettings.h"
@ -306,16 +307,36 @@ void updateVtolDesiredVelocity()
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
float northPos = 0, eastPos = 0, downPos = 0;
switch (guidanceSettings.PositionSource) {
case GUIDANCESETTINGS_POSITIONSOURCE_EKF:
northPos = positionActual.North;
eastPos = positionActual.East;
downPos = positionActual.Down;
break;
case GUIDANCESETTINGS_POSITIONSOURCE_GPSPOS:
{
NEDPositionData nedPosition;
NEDPositionGet(&nedPosition);
northPos = nedPosition.North;
eastPos = nedPosition.East;
downPos = nedPosition.Down;
}
default:
PIOS_Assert(0);
break;
}
// Note all distances in cm
// Compute desired north command
northError = positionDesired.North - positionActual.North;
northError = positionDesired.North - northPos;
northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral);
eastError = positionDesired.East - positionActual.East;
eastError = positionDesired.East - eastPos;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
@ -331,7 +352,7 @@ void updateVtolDesiredVelocity()
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = positionDesired.Down - positionActual.Down;
downError = positionDesired.Down - downPos;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);