Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: changed to AP_Vehicle.h 

also allow updates to ARSPD_RATIO from user while autocal is running
This commit is contained in:
Andrew Tridgell 2013-09-13 11:45:57 +10:00
parent 23fc6f8aed
commit 2e742582e4
3 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -7,13 +7,13 @@
#include <AP_HAL.h>
#include <AP_Param.h>
#include <GCS_MAVLink.h>
#include <AP_SpdHgtControl.h>
#include <AP_Vehicle.h>
class Airspeed_Calibration {
public:
friend class AP_Airspeed;
// constructor
Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms);
Airspeed_Calibration(const AP_Vehicle::FixedWing &parms);
// initialise the calibration
void init(float initial_ratio);
@ -29,14 +29,14 @@ private:
const float Q1; // process noise matrix bottom right element
Vector3f state; // state vector
const float DT; // time delta
const AP_SpdHgtControl::AircraftParameters &aparm;
const AP_Vehicle::FixedWing &aparm;
};
class AP_Airspeed
{
public:
// constructor
AP_Airspeed(const AP_SpdHgtControl::AircraftParameters &parms) :
AP_Airspeed(const AP_Vehicle::FixedWing &parms) :
_ets_fd(-1),
_EAS2TAS(1.0f),
_calibration(parms)

19
libraries/AP_Airspeed/Airspeed_Calibration.cpp
View File

@ -14,7 +14,7 @@
extern const AP_HAL::HAL& hal;
// constructor - fill in all the initial values
Airspeed_Calibration::Airspeed_Calibration(const AP_SpdHgtControl::AircraftParameters &parms) :
Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms) :
P(100, 0, 0,
0, 100, 0,
0, 0, 0.000001f),
@ -119,18 +119,27 @@ void AP_Airspeed::update_calibration(const Vector3f &vground)
// auto-calibration not enabled
return;
}
// set state.z based on current ratio, this allows the operator to
// override the current ratio in flight with autocal, which is
// very useful both for testing and to force a reasonable value.
float ratio = constrain_float(_ratio, 1.0f, 4.0f);
_calibration.state.z = 1.0 / sqrtf(ratio);
// calculate true airspeed, assuming a airspeed ratio of 1.0
float dpress = get_differential_pressure();
float true_airspeed = sqrtf(dpress) * _EAS2TAS;
float ratio = _calibration.update(true_airspeed, vground);
if (isnan(ratio) || isinf(ratio)) {
float zratio = _calibration.update(true_airspeed, vground);
if (isnan(zratio) || isinf(zratio)) {
return;
}
// this constrains the resulting ratio to between 1.0 and 4.0
ratio = constrain_float(ratio, 0.5f, 1.0f);
_ratio.set(1/sq(ratio));
zratio = constrain_float(zratio, 0.5f, 1.0f);
_ratio.set(1/sq(zratio));
if (_counter > 60) {
if (_last_saved_ratio > 1.05f*_ratio ||
_last_saved_ratio < 0.95f*_ratio) {

4
libraries/AP_Airspeed/examples/Airspeed/Airspeed.pde
View File

@ -30,7 +30,7 @@
#include <Filter.h>
#include <AP_Buffer.h>
#include <AP_Airspeed.h>
#include <AP_SpdHgtControl.h>
#include <AP_Vehicle.h>
#include <AP_Notify.h>
#include <DataFlash.h>
#include <GCS_MAVLink.h>
@ -43,7 +43,7 @@ AP_ADC_ADS7844 apm1_adc;
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
static AP_SpdHgtControl::AircraftParameters aparm;
static AP_Vehicle::FixedWing aparm;
AP_Airspeed airspeed(aparm);