mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 20:48:33 -04:00
Sub: use ins singleton
This commit is contained in:
parent
a0b35cfa25
commit
78ca188dc3
@ -207,7 +207,7 @@ void Sub::ten_hz_logging_loop()
|
|||||||
pos_control.write_log();
|
pos_control.write_log();
|
||||||
}
|
}
|
||||||
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
|
if (should_log(MASK_LOG_IMU) || should_log(MASK_LOG_IMU_FAST) || should_log(MASK_LOG_IMU_RAW)) {
|
||||||
DataFlash.Log_Write_Vibration(ins);
|
DataFlash.Log_Write_Vibration();
|
||||||
}
|
}
|
||||||
if (should_log(MASK_LOG_CTUN)) {
|
if (should_log(MASK_LOG_CTUN)) {
|
||||||
attitude_control.control_monitor_log();
|
attitude_control.control_monitor_log();
|
||||||
@ -231,7 +231,7 @@ void Sub::twentyfive_hz_logging()
|
|||||||
|
|
||||||
// log IMU data if we're not already logging at the higher rate
|
// log IMU data if we're not already logging at the higher rate
|
||||||
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
|
if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
|
||||||
DataFlash.Log_Write_IMU(ins);
|
DataFlash.Log_Write_IMU();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -188,7 +188,7 @@ private:
|
|||||||
// Inertial Navigation EKF
|
// Inertial Navigation EKF
|
||||||
NavEKF2 EKF2{&ahrs, rangefinder};
|
NavEKF2 EKF2{&ahrs, rangefinder};
|
||||||
NavEKF3 EKF3{&ahrs, rangefinder};
|
NavEKF3 EKF3{&ahrs, rangefinder};
|
||||||
AP_AHRS_NavEKF ahrs{ins, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
SITL::SITL sitl;
|
SITL::SITL sitl;
|
||||||
|
Loading…
Reference in New Issue
Block a user