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

When in complimentary filter mode pass the GPS information straight

Browse Source 
through to Position Actual
This commit is contained in:
James Cotton 2012-03-02 18:37:43 -06:00
parent 98b1a21921
commit a83fb019f6
1 changed files with 21 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -399,11 +399,27 @@ static int32_t updateAttitudeComplimentary(bool first_run)
AttitudeActualSet(&attitudeActual);
// Flush these queues for avoid errors
if ( xQueueReceive(baroQueue, &ev, 0) != pdTRUE )
{
}
if ( xQueueReceive(gpsQueue, &ev, 0) != pdTRUE )
{
xQueueReceive(baroQueue, &ev, 0);
if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
velocityActual.East = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
velocityActual.Down = 0;
VelocityActualSet(&velocityActual);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);