rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

re-enabled wind estimation absed on groundspeed

Browse Source
This commit is contained in:
Corvus Corax 2013-07-18 20:01:02 +02:00
parent b195ffda2f
commit ea8373b0f7
4 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/modules/Airspeed/revolution/airspeed.c
View File

@ -43,6 +43,7 @@
#include "airspeedsensor.h" // object that will be updated by the module
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "taskinfo.h"
// Private constants
@ -164,6 +165,9 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
baro_airspeedGetETASV3(&airspeedData, &airspeedSettings);
break;
#endif
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_GROUNDSPEEDBASEDWINDESTIMATION:
gps_airspeedGet(&airspeedData, &airspeedSettings);
break;
default:
airspeedData.SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
}

26
flight/modules/Airspeed/revolution/gps_airspeed.c
View File

@ -31,9 +31,11 @@
#include "openpilot.h"
#include "gps_airspeed.h"
#include "gpsvelocitysensor.h"
#include "velocitystate.h"
#include "attitudestate.h"
#include "airspeedsensor.h"
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include <pios_math.h>
@ -66,8 +68,9 @@ void gps_airspeedInitialize()
gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals));
// GPS airspeed calculation variables
GPSVelocitySensorData gpsVelData;
GPSVelocitySensorGet(&gpsVelData);
VelocityStateInitialize();
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
@ -96,7 +99,7 @@ void gps_airspeedInitialize()
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
void gps_airspeedGet(float *v_air_GPS)
void gps_airspeedGet(AirspeedSensorData *airspeedData, __attribute__((unused)) AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
@ -115,8 +118,8 @@ void gps_airspeedGet(float *v_air_GPS)
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
GPSVelocitySensorData gpsVelData;
GPSVelocitySensorGet(&gpsVelData);
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
@ -125,7 +128,14 @@ void gps_airspeedGet(float *v_air_GPS)
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
*v_air_GPS = sqrtf(normDiffGPS2 / normDiffAttitude2);
airspeedData->CalibratedAirspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
} else {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
}
// Save old variables for next pass
gps->gpsVelOld_N = gpsVelData.North;

2
flight/modules/Airspeed/revolution/inc/gps_airspeed.h
View File

@ -32,7 +32,7 @@
#define GPS_AIRSPEED_H
void gps_airspeedInitialize();
void gps_airspeedGet(float *v_air_GPS);
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings);
#endif // GPS_AIRSPEED_H

2
shared/uavobjectdefinition/airspeedsettings.xml
View File

@ -4,7 +4,7 @@
<field name="SamplePeriod" units="ms" type="uint8" elements="1" defaultvalue="100"/>
<field name="ZeroPoint" units="raw" type="uint16" elements="1" defaultvalue="0"/>
<field name="Scale" units="raw" type="float" elements="1" defaultvalue="1.0"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002" defaultvalue="DIYDronesMPXV7002"/>
<field name="AirspeedSensorType" units="" type="enum" elements="1" options="EagleTreeAirspeedV3,DIYDronesMPXV5004,DIYDronesMPXV7002,GroundSpeedBasedWindEstimation" defaultvalue="DIYDronesMPXV7002"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>