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:
parent
fc50f145ce
commit
1586abab8d
libraries/AP_Airspeed
@ -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);
|
||||||
};
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -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;
|
||||||
|
@ -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;
|
||||||
|
@ -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()
|
||||||
{
|
{
|
||||||
|
Loading…
Reference in New Issue
Block a user