Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: use ins singleton

Browse Source
This commit is contained in:
Peter Barker 2018-03-10 20:35:50 +11:00 committed by Lucas De Marchi
parent 9f556197a0
commit 40957ec430
4 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -583,8 +583,7 @@ void NavEKF2::check_log_write(void)
logging.log_baro = false;
}
if (logging.log_imu) {
const AP_InertialSensor &ins = _ahrs->get_ins();
DataFlash_Class::instance()->Log_Write_IMUDT(ins, imuSampleTime_us, _logging_mask.get());
DataFlash_Class::instance()->Log_Write_IMUDT(imuSampleTime_us, _logging_mask.get());
logging.log_imu = false;
}
@ -599,7 +598,7 @@ bool NavEKF2::InitialiseFilter(void)
if (_enable == 0) {
return false;
}
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
imuSampleTime_us = AP_HAL::micros64();
@ -688,7 +687,7 @@ void NavEKF2::UpdateFilter(void)
imuSampleTime_us = AP_HAL::micros64();
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
bool statePredictEnabled[num_cores];
for (uint8_t i=0; i<num_cores; i++) {

6
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -283,7 +283,7 @@ void NavEKF2_core::readMagData()
*/
void NavEKF2_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
// average IMU sampling rate
dtIMUavg = ins.get_loop_delta_t();
@ -391,7 +391,7 @@ void NavEKF2_core::readIMUData()
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt) {
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
if (ins_index < ins.get_accel_count()) {
ins.get_delta_velocity(ins_index,dVel);
@ -534,7 +534,7 @@ void NavEKF2_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) {
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
if (ins_index < ins.get_gyro_count()) {
ins.get_delta_angle(ins_index,dAng);

2
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -452,7 +452,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
{
if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
// we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
Vector3f angRateVec;
Vector3f gyroBias;
getGyroBias(gyroBias);

10
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -62,7 +62,7 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
than 100Hz is downsampled. For 50Hz main loop rate we need a
shorter buffer.
*/
if (_ahrs->get_ins().get_sample_rate() < 100) {
if (AP::ins().get_sample_rate() < 100) {
imu_buffer_length = 13;
} else {
// maximum 260 msec delay at 100 Hz fusion rate
@ -110,7 +110,7 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
void NavEKF2_core::InitialiseVariables()
{
// calculate the nominal filter update rate
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
@ -355,8 +355,10 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
// set re-used variables to zero
InitialiseVariables();
const AP_InertialSensor &ins = AP::ins();
// Initialise IMU data
dtIMUavg = _ahrs->get_ins().get_loop_delta_t();
dtIMUavg = ins.get_loop_delta_t();
readIMUData();
storedIMU.reset_history(imuDataNew);
imuDataDelayed = imuDataNew;
@ -365,7 +367,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
Vector3f initAccVec;
// TODO we should average accel readings over several cycles
initAccVec = _ahrs->get_ins().get_accel(imu_index);
initAccVec = ins.get_accel(imu_index);
// read the magnetometer data
readMagData();