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

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

Browse Source 
This reverts commit 212a10bcd4.
This commit is contained in:
James Cotton 2012-04-14 17:20:01 -05:00
parent 212a10bcd4
commit 9a2e0b071c
1 changed files with 14 additions and 11 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,6 +134,17 @@ static void SensorsTask(void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); 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; sensor_sim_type = MODEL_QUADCOPTER;
// Main task loop // Main task loop
@ -329,17 +340,9 @@ static void simulateModelQuadcopter()
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA; 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 gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[1] + rand_gauss() + 3; gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss() - 5; gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss() + 1; gyrosData.z = rpy[2] + rand_gauss();
// 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); GyrosSet(&gyrosData);
// Predict the attitude forward in time // Predict the attitude forward in time