5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-12 10:53:59 -04:00

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

This reverts commit 5439257236.
This commit is contained in:
Tom Pittenger 2016-08-07 17:48:54 -07:00
parent fc50f145ce
commit 1586abab8d
4 changed files with 16 additions and 45 deletions

View File

@ -134,37 +134,17 @@ 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 // @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 // @User: Advanced
AP_GROUPINFO("PSI_RANGE", 8, AP_Airspeed, _psi_range, PSI_RANGE_DEFAULT), 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_GROUPEND
}; };
AP_Airspeed::AP_Airspeed() AP_Airspeed::AP_Airspeed(const AP_Vehicle::FixedWing &parms)
: _EAS2TAS(1.0f) : _EAS2TAS(1.0f)
, _calibration() , _calibration(parms)
, analog(_pin, _psi_range)
, digital(_psi_range)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
}; }
/* /*

View File

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

View File

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

View File

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