mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-02 19:53:57 -04:00
AP_Airspeed: remove AP_Vehicle::FixedWing dependency
This commit is contained in:
parent
f85fc7c1dc
commit
ce7b766d8b
@ -139,9 +139,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
|
||||
};
|
||||
|
||||
|
||||
AP_Airspeed::AP_Airspeed(const AP_Vehicle::FixedWing &parms)
|
||||
AP_Airspeed::AP_Airspeed()
|
||||
: _EAS2TAS(1.0f)
|
||||
, _calibration(parms)
|
||||
, _calibration()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
@ -16,7 +16,7 @@ class Airspeed_Calibration {
|
||||
public:
|
||||
friend class AP_Airspeed;
|
||||
// constructor
|
||||
Airspeed_Calibration(const AP_Vehicle::FixedWing &parms);
|
||||
Airspeed_Calibration();
|
||||
|
||||
// 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);
|
||||
|
||||
|
@ -15,7 +15,7 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// 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 +23,6 @@ Airspeed_Calibration::Airspeed_Calibration(const AP_Vehicle::FixedWing &parms)
|
||||
, Q1(0.0000005f)
|
||||
, state(0, 0, 0)
|
||||
, DT(1)
|
||||
, aparm(parms)
|
||||
{
|
||||
}
|
||||
|
||||
@ -70,7 +69,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m
|
||||
-(state.z*SH2*(2*vg.y - 2*state.y))/2,
|
||||
1/SH2);
|
||||
|
||||
// Calculate the fusion innovaton covariance assuming a TAS measurement
|
||||
// Calculate the fusion innovation covariance assuming a TAS measurement
|
||||
// noise of 1.0 m/s
|
||||
// S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]
|
||||
Vector3f PH = P * H_TAS;
|
||||
|
@ -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()
|
||||
{
|
||||
|
Loading…
Reference in New Issue
Block a user