rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

Simulate GPS noise drift

Browse Source
This commit is contained in:
James Cotton 2012-03-15 04:25:58 -05:00
parent f491c924d6
commit c988685293
1 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/Modules/Sensors/simulated/sensors.c
View File

@ -446,13 +446,23 @@ static void simulateModelQuadcopter()
T[1] = cos(homeLocation.Latitude / 10e6 * M_PI / 180.0f)*(homeLocation.Altitude+6.378137E6) * M_PI / 180.0;
T[2] = -1.0;
static float gps_drift[3] = {0,0,0};
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
static float gps_vel_drift[3] = {0,0,0};
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + (pos[0] / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + (pos[1] / T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + (pos[2] / T[2]);
gpsPosition.Groundspeed = sqrt(vel[0] * vel[0] + vel[1] * vel[1]);
gpsPosition.Heading = 180 / M_PI * atan2(vel[1],vel[0]);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
gpsPosition.Groundspeed = sqrt(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
gpsPosition.Heading = 180 / M_PI * atan2(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition);