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

OP-1317 - project 3d airspeed vector unto fuselage vector to get scalar airspeed estimate

Browse Source
This commit is contained in:
Corvus Corax 2014-06-18 12:44:10 +02:00
parent 989b9f5c5e
commit 7933f2154c
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
flight/modules/Airspeed/imu_airspeed.c
View File

@ -267,11 +267,18 @@ void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
imu->Vw3 = FilterButterWorthDF2(ffV, wind[2], &(imu->Vw3n1), &(imu->Vw3n2));
} // else leave wind estimation unchanged
// airspeed = groundspeed - wind
airspeedData->CalibratedAirspeed = sqrtf(
(imu->v1Old - imu->Vw1) * (imu->v1Old - imu->Vw1)
+ (imu->v2Old - imu->Vw2) * (imu->v2Old - imu->Vw2)
+ (imu->v3Old - imu->Vw3) * (imu->v3Old - imu->Vw3));
{ // Scoping to save memory
// airspeed = groundspeed - wind
const float Vair[3] = {
imu->v1Old - imu->Vw1,
imu->v2Old - imu->Vw2,
imu->v3Old - imu->Vw3
};
// project airspeed into fuselage vector
airspeedData->CalibratedAirspeed = Vair[0] * xB[0] + Vair[1] * xB[1] + Vair[2] * xB[2];
}
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_AIRSPEED);
}