AP_Airspeed: Converted library to be stand-alone from APM:Plane.

Additional changes:
   ARSPD_FBW_MIN and ARSPD_FBW_MAX renamed to ARSPD_MIN and ARSPD_MAX
   ARSPD_MIN and ARSPD_MAX changed to floats
This commit is contained in:
AndersonRayner 2016-05-29 01:01:38 +10:00 committed by Tom Pittenger
parent 63317e9430
commit 5439257236
4 changed files with 45 additions and 16 deletions

View File

@ -132,17 +132,37 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @Description: This parameter allows you to to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
// @User: Advanced
AP_GROUPINFO("PSI_RANGE", 8, AP_Airspeed, _psi_range, PSI_RANGE_DEFAULT),
// @Param: ARSPD_FBW_MIN
// @DisplayName: Minimum Airspeed
// @Description: This is the minimum airspeed you want to fly at in modes where the autopilot controls the airspeed. This should be set to a value around 20% higher than the level flight stall speed for the airframe. This value is also used in the STALL_PREVENTION code.
// @Units: m/s
// @Range: 5 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("FBW_MIN", 9, AP_Airspeed, _airspeed_min, 9),
// @Param: ARSPD_FBW_MAX
// @DisplayName: Maximum Airspeed
// @Description: This is the maximum airspeed that you want to allow for your airframe in auto-throttle modes. You should ensure that this value is sufficiently above the ARSPD_MIN value to allow for a sufficient flight envelope to accurately control altitude using airspeed. A value at least 50% above ARSPD_MIN is recommended.
// @Units: m/s
// @Range: 5 100
// @Increment: 1
// @User: Standard
AP_GROUPINFO("FBW_MAX", 10, AP_Airspeed, _airspeed_max, 22),
AP_GROUPEND
};
AP_Airspeed::AP_Airspeed(const AP_Vehicle::FixedWing &parms)
AP_Airspeed::AP_Airspeed()
: _EAS2TAS(1.0f)
, _calibration(parms)
, _calibration()
, analog(_pin, _psi_range)
, digital(_psi_range)
{
AP_Param::setup_object_defaults(this, var_info);
}
};
/*

View File

@ -16,7 +16,7 @@ class Airspeed_Calibration {
public:
friend class AP_Airspeed;
// constructor
Airspeed_Calibration(const AP_Vehicle::FixedWing &parms);
Airspeed_Calibration(void);
// initialise the calibration
void init(float initial_ratio);
@ -32,14 +32,13 @@ private:
const float Q1; // process noise matrix bottom right element
Vector3f state; // state vector
const float DT; // time delta
const AP_Vehicle::FixedWing &aparm;
};
class AP_Airspeed
{
public:
// constructor
AP_Airspeed(const AP_Vehicle::FixedWing &parms);
AP_Airspeed();
void init(void);
@ -124,6 +123,16 @@ public:
return _EAS2TAS;
}
// get the min set airspeed
float get_airspeed_min(void) const {
return (float)(_airspeed_min);
}
// get the max set airspeed
float get_airspeed_max(void) const {
return (float)(_airspeed_max);
}
// update airspeed ratio calibration
void update_calibration(const Vector3f &vground);
@ -156,6 +165,8 @@ private:
AP_Int8 _autocal;
AP_Int8 _tube_order;
AP_Int8 _skip_cal;
AP_Int16 _airspeed_min;
AP_Int16 _airspeed_max;
float _raw_airspeed;
float _airspeed;
float _last_pressure;

View File

@ -13,9 +13,10 @@
#include "AP_Airspeed.h"
extern const AP_HAL::HAL& hal;
AP_Airspeed arspd;
// constructor - fill in all the initial values
Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms)
Airspeed_Calibration::Airspeed_Calibration()
: P(100, 0, 0,
0, 100, 0,
0, 0, 0.000001f)
@ -23,7 +24,6 @@ Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms)
, Q1(0.0000005f)
, state(0, 0, 0)
, DT(1)
, aparm(parms)
{
}
@ -39,7 +39,7 @@ void Airspeed_Calibration::init(float initial_ratio)
update the state of the airspeed calibration - needs to be called
once a second
*/
float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
float Airspeed_Calibration::update(float aspeed, const Vector3f &vg)
{
// Perform the covariance prediction
// Q is a diagonal matrix so only need to add three terms in
@ -54,7 +54,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
// invariant plus process noise
// Ignore vertical wind component
float TAS_pred = state.z * norm(vg.x - state.x, vg.y - state.y, vg.z);
float TAS_mea = airspeed;
float TAS_mea = aspeed;
// Calculate the observation Jacobian H_TAS
float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x);
@ -101,8 +101,8 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
P.b.y = MAX(P.b.y, 0.0f);
P.c.z = MAX(P.c.z, 0.0f);
state.x = constrain_float(state.x, -aparm.airspeed_max, aparm.airspeed_max);
state.y = constrain_float(state.y, -aparm.airspeed_max, aparm.airspeed_max);
state.x = constrain_float(state.x, -arspd.get_airspeed_max(), arspd.get_airspeed_max());
state.y = constrain_float(state.y, -arspd.get_airspeed_max(), arspd.get_airspeed_max());
state.z = constrain_float(state.z, 0.5f, 1.0f);
return state.z;

View File

@ -25,11 +25,9 @@
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static AP_Vehicle::FixedWing aparm;
float temperature;
AP_Airspeed airspeed(aparm);
AP_Airspeed airspeed;
void setup()
{