rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +01:00
Code Issues Releases Activity

Update the EKF code to use the GPSVelocity

Browse Source
This commit is contained in:
James Cotton 2012-04-09 19:03:21 -05:00
parent dbcb47e249
commit 136bbe3814
1 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,6 +58,7 @@
#include "baroaltitude.h" #include "baroaltitude.h"
#include "flightstatus.h" #include "flightstatus.h"
#include "gpsposition.h" #include "gpsposition.h"
#include "gpsvelocity.h"
#include "gyros.h" #include "gyros.h"
#include "gyrosbias.h" #include "gyrosbias.h"
#include "homelocation.h" #include "homelocation.h"
@ -86,6 +87,7 @@ static xQueueHandle accelQueue;
static xQueueHandle magQueue; static xQueueHandle magQueue;
static xQueueHandle baroQueue; static xQueueHandle baroQueue;
static xQueueHandle gpsQueue; static xQueueHandle gpsQueue;
static xQueueHandle gpsVelQueue;
static AttitudeSettingsData attitudeSettings; static AttitudeSettingsData attitudeSettings;
static HomeLocationData homeLocation; static HomeLocationData homeLocation;
@ -165,6 +167,7 @@ int32_t AttitudeStart(void)
magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); magQueue = xQueueCreate(1, sizeof(UAVObjEvent));
baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); baroQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent));
gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent));
// Start main task // Start main task
xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle); xTaskCreate(AttitudeTask, (signed char *)"Attitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &attitudeTaskHandle);
@ -176,6 +179,7 @@ int32_t AttitudeStart(void)
MagnetometerConnectQueue(magQueue); MagnetometerConnectQueue(magQueue);
BaroAltitudeConnectQueue(baroQueue); BaroAltitudeConnectQueue(baroQueue);
GPSPositionConnectQueue(gpsQueue); GPSPositionConnectQueue(gpsQueue);
GPSVelocityConnectQueue(gpsVelQueue);
return 0; return 0;
} }
@ -468,12 +472,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
MagnetometerData magData; MagnetometerData magData;
BaroAltitudeData baroData; BaroAltitudeData baroData;
GPSPositionData gpsData; GPSPositionData gpsData;
GPSVelocityData gpsVelData;
GyrosBiasData gyrosBias; GyrosBiasData gyrosBias;
HomeLocationData home; HomeLocationData home;
static bool mag_updated; static bool mag_updated;
static bool baro_updated; static bool baro_updated;
static bool gps_updated; static bool gps_updated;
static bool gps_vel_updated;
static uint32_t ins_last_time = 0; static uint32_t ins_last_time = 0;
static bool inited; static bool inited;
@ -508,6 +514,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && homeLocation.Set == HOMELOCATION_SET_TRUE; mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && homeLocation.Set == HOMELOCATION_SET_TRUE;
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
// Get most recent data // Get most recent data
GyrosGet(&gyrosData); GyrosGet(&gyrosData);
@ -515,6 +522,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
MagnetometerGet(&magData); MagnetometerGet(&magData);
BaroAltitudeGet(&baroData); BaroAltitudeGet(&baroData);
GPSPositionGet(&gpsData); GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
HomeLocationGet(&home); HomeLocationGet(&home);
// Have a minimum requirement for gps usage // Have a minimum requirement for gps usage
@ -658,10 +666,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GPSPositionData gpsPosition; GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition); GPSPositionGet(&gpsPosition);
if (0) {
vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f); vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f); vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
vel[2] = 0; vel[2] = 0;
} else {
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
// Transform the GPS position into NED coordinates // Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED); getNED(&gpsPosition, NED);