// Main loop - 400hz void Copter::fast_loop() { // IMU DCM Algorithm // -------------------- read_AHRS(); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); #if FRAME_CONFIG == HELI_FRAME update_heli_control_dynamics(); #endif //HELI_FRAME // send outputs to the motors library motors_output(); // Inertial Nav // -------------------- read_inertia(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we've landed update_land_detector(); }
// run land and crash detectors // called at MAIN_LOOP_RATE void Sub::update_land_and_crash_detectors() { // update 1hz filtered acceleration Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS); update_land_detector(); crash_check(); }
// run land and crash detectors // called at MAIN_LOOP_RATE void Copter::update_land_and_crash_detectors() { // update 1hz filtered acceleration Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS); update_land_detector(); #if PARACHUTE == ENABLED // check parachute parachute_check(); #endif crash_check(); }