global: use static method to construct AP_AHRS_NavEKF
This commit is contained in:
parent
e2a9d9c001
commit
beabae6a98
@ -165,7 +165,7 @@ private:
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rangefinder);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rangefinder);
|
||||
AP_AHRS_NavEKF ahrs {ins, barometer, gps, EKF2, EKF3};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3);
|
||||
#else
|
||||
AP_AHRS_DCM ahrs {ins, barometer, gps};
|
||||
#endif
|
||||
|
@ -118,7 +118,7 @@ private:
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rng);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rng);
|
||||
AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3);
|
||||
#else
|
||||
AP_AHRS_DCM ahrs{ins, barometer, gps};
|
||||
#endif
|
||||
|
@ -216,7 +216,7 @@ private:
|
||||
// Inertial Navigation EKF
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rangefinder);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rangefinder);
|
||||
AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
SITL::SITL sitl;
|
||||
|
@ -215,7 +215,7 @@ private:
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rangefinder);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rangefinder);
|
||||
AP_AHRS_NavEKF ahrs {ins, barometer, gps, EKF2, EKF3};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3);
|
||||
#else
|
||||
AP_AHRS_DCM ahrs {ins, barometer, gps};
|
||||
#endif
|
||||
|
@ -186,7 +186,7 @@ private:
|
||||
// Inertial Navigation EKF
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rangefinder);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rangefinder);
|
||||
AP_AHRS_NavEKF ahrs {ins, barometer, gps, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF);
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
SITL::SITL sitl;
|
||||
|
@ -66,7 +66,7 @@ public:
|
||||
RangeFinder rng = RangeFinder::create(serial_manager, ROTATION_PITCH_270);
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, rng);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, rng);
|
||||
AP_AHRS_NavEKF ahrs {ins, barometer, gps, EKF2, EKF3};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3);
|
||||
AP_InertialNav_NavEKF inertial_nav{ahrs};
|
||||
AP_Vehicle::FixedWing aparm;
|
||||
AP_Airspeed airspeed;
|
||||
|
@ -28,15 +28,15 @@ public:
|
||||
RangeFinder sonar = RangeFinder::create(serial_manager, ROTATION_PITCH_270);
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, sonar);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, sonar);
|
||||
AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3,
|
||||
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3,
|
||||
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF);
|
||||
};
|
||||
|
||||
static DummyVehicle vehicle;
|
||||
|
||||
// choose which AHRS system to use
|
||||
// AP_AHRS_DCM ahrs(ins, baro, gps);
|
||||
AP_AHRS_NavEKF ahrs(vehicle.ahrs);
|
||||
AP_AHRS_NavEKF &ahrs = vehicle.ahrs;
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
|
@ -27,8 +27,8 @@ public:
|
||||
AP_InertialSensor ins = AP_InertialSensor::create();
|
||||
AP_SerialManager serial_manager = AP_SerialManager::create();
|
||||
RangeFinder sonar = RangeFinder::create(serial_manager, ROTATION_PITCH_270);
|
||||
AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3,
|
||||
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
||||
AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3,
|
||||
AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF);
|
||||
NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, sonar);
|
||||
NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, sonar);
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user