Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use ins singleton

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

7
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -637,8 +637,7 @@ void NavEKF3::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;
}
@ -653,7 +652,7 @@ bool NavEKF3::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();
@ -756,7 +755,7 @@ void NavEKF3::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_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -329,7 +329,7 @@ void NavEKF3_core::readMagData()
*/
void NavEKF3_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
const AP_InertialSensor &ins = AP::ins();
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
@ -437,7 +437,7 @@ void NavEKF3_core::readIMUData()
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_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);
@ -583,7 +583,7 @@ void NavEKF3_core::readGpsData()
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
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_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -451,7 +451,7 @@ void NavEKF3_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);

8
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -53,8 +53,8 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
*/
// Calculate the expected EKF time step
if (_ahrs->get_ins().get_sample_rate() > 0) {
dtEkfAvg = 1.0f / _ahrs->get_ins().get_sample_rate();
if (AP::ins().get_sample_rate() > 0) {
dtEkfAvg = 1.0f / AP::ins().get_sample_rate();
dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT);
} else {
return false;
@ -155,7 +155,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
void NavEKF3_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, (uint8_t)EKF_TARGET_DT_MS);
@ -430,7 +430,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
Vector3f initAccVec;
// TODO we should average accel readings over several cycles
initAccVec = _ahrs->get_ins().get_accel(imu_index);
initAccVec = AP::ins().get_accel(imu_index);
// normalise the acceleration vector
float pitch=0, roll=0;