diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 9333dd3db5..78bb6c2470 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -113,33 +113,34 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(!storedBaro.init(obs_buffer_length)) { return false; } - if(!storedTAS.init(obs_buffer_length)) { + if(dal.airspeed() && !storedTAS.init(obs_buffer_length)) { return false; } - if (!storedOF.init(flow_buffer_length)) { + if(dal.opticalflow_enabled() && !storedOF.init(flow_buffer_length)) { return false; } - if(!storedBodyOdm.init(obs_buffer_length)) { + if(frontend->sources.ext_nav_enabled() && !storedBodyOdm.init(obs_buffer_length)) { return false; } - if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors + if(frontend->sources.wheel_encoder_enabled() && !storedWheelOdm.init(imu_buffer_length)) { + // initialise to same length of IMU to allow for multiple wheel sensors return false; } - if(!storedYawAng.init(yaw_angle_buffer_length)) { + if(frontend->sources.ext_yaw_enabled() && !storedYawAng.init(yaw_angle_buffer_length)) { return false; } // Note: the use of dual range finders potentially doubles the amount of data to be stored - if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) { + if(dal.rangefinder() && !storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) { return false; } // Note: range beacon data is read one beacon at a time and can arrive at a high rate - if(!storedRangeBeacon.init(imu_buffer_length+1)) { + if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) { return false; } - if (!storedExtNav.init(extnav_buffer_length)) { + if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) { return false; } - if (!storedExtNavVel.init(extnav_buffer_length)) { + if (frontend->sources.ext_nav_enabled() && !storedExtNavVel.init(extnav_buffer_length)) { return false; } if(!storedIMU.init(imu_buffer_length)) {