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

Allow simulated quadcopter to include GyroBias in the artificial sensor data.

Browse Source
This commit is contained in:
James Cotton 2012-04-14 14:52:56 -05:00
parent af02eb6517
commit 212a10bcd4
1 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -134,17 +134,6 @@ static void SensorsTask(void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
// HomeLocationData homeLocation;
// HomeLocationGet(&homeLocation);
// homeLocation.Latitude = 0;
// homeLocation.Longitude = 0;
// homeLocation.Altitude = 0;
// homeLocation.Be[0] = 26000;
// homeLocation.Be[1] = 400;
// homeLocation.Be[2] = 40000;
// homeLocation.Set = HOMELOCATION_SET_TRUE;
// HomeLocationSet(&homeLocation);
sensor_sim_type = MODEL_QUADCOPTER;
// Main task loop
@ -340,9 +329,17 @@ static void simulateModelQuadcopter()
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss();
gyrosData.x = rpy[1] + rand_gauss() + 3;
gyrosData.y = rpy[1] + rand_gauss() - 5;
gyrosData.z = rpy[2] + rand_gauss() + 1;
// Add in estimate of bias from sensors
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
GyrosSet(&gyrosData);
// Predict the attitude forward in time