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

Add cross coupling between roll and heading to make simulator fly like a plane

Browse Source 
:D
This commit is contained in:
James Cotton 2012-06-08 11:08:11 -05:00
parent 823538dbbb
commit 6605d861dd
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -547,6 +547,7 @@ static void simulateModelAirplane()
const float GPS_PERIOD = 0.1; const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0; const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0; const float BARO_PERIOD = 1.0 / 20.0;
const float ROLL_HEADING_COUPLING = 0.1; // (deg/s) heading change per deg of roll
static uint32_t last_time; static uint32_t last_time;
@ -582,10 +583,19 @@ static void simulateModelAirplane()
RateDesiredData rateDesired; RateDesiredData rateDesired;
RateDesiredGet(&rateDesired); RateDesiredGet(&rateDesired);
// Need to compute roll angle for easy cross coupling
//Quaternion2RPY(q,rpy);
//float roll = rpy[0];
rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA; rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA; rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
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;
{
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
rpy[2] += attitudeActual.Roll * ROLL_HEADING_COUPLING;
}
GyrosData gyrosData; // Skip get as we set all the fields GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss(); gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss(); gyrosData.y = rpy[1] + rand_gauss();