// 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(); }
// Main loop - 400hz void Copter::fast_loop() { // update INS immediately to get current gyro data populated ins.update(); // run low level rate controllers that only require IMU data attitude_control->rate_controller_run(); // send outputs to the motors library immediately motors_output(); // run EKF state estimator (expensive) // -------------------- read_AHRS(); #if FRAME_CONFIG == HELI_FRAME update_heli_control_dynamics(); #endif //HELI_FRAME // Inertial Nav // -------------------- read_inertia(); // check if ekf has reset target heading or position check_ekf_reset(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we've landed or crashed update_land_and_crash_detectors(); #if MOUNT == ENABLED // camera mount's fast update camera_mount.update_fast(); #endif // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); } }
// 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(); // check if ekf has reset target heading check_ekf_yaw_reset(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we've landed or crashed update_land_and_crash_detectors(); #if MOUNT == ENABLED // camera mount's fast update camera_mount.update_fast(); #endif // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); } }
// Main loop - 400hz void Sub::fast_loop() { // update INS immediately to get current gyro data populated ins.update(); if (control_mode != MANUAL) { //don't run rate controller in manual mode // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); } // send outputs to the motors library motors_output(); // run EKF state estimator (expensive) // -------------------- read_AHRS(); // Inertial Nav // -------------------- read_inertia(); // check if ekf has reset target heading check_ekf_yaw_reset(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we've reached the surface or bottom update_surface_and_bottom_detector(); #if MOUNT == ENABLED // camera mount's fast update camera_mount.update_fast(); #endif // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); } }
// Main loop - 400hz void Sub::fast_loop() { // IMU DCM Algorithm // -------------------- read_AHRS(); // run low level rate controllers that only require IMU data attitude_control.rate_controller_run(); // send outputs to the motors library motors_output(); // Inertial Nav // -------------------- read_inertia(); // check if ekf has reset target heading check_ekf_yaw_reset(); // run the attitude controllers update_flight_mode(); // update home from EKF if necessary update_home_from_EKF(); // check if we've landed or crashed update_land_and_crash_detectors(); // check if we've reached the surface or bottom update_surface_and_bottom_detector(); // camera mount's fast update camera_mount.update_fast(); // log sensor health if (should_log(MASK_LOG_ANY)) { Log_Sensor_Health(); } }
// update AHRS system void Rover::ahrs_update() { update_soft_armed(); #if HIL_MODE != HIL_MODE_DISABLED // update hil before AHRS update gcs_update(); #endif // AHRS may use movement to calculate heading update_ahrs_flyforward(); ahrs.update(); // update position have_position = ahrs.get_position(current_loc); // update home from EKF if necessary update_home_from_EKF(); // if using the EKF get a speed update now (from accelerometers) Vector3f velocity; if (ahrs.get_velocity_NED(velocity)) { ground_speed = norm(velocity.x, velocity.y); } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { ground_speed = ahrs.groundspeed(); } if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); } if (should_log(MASK_LOG_IMU)) { DataFlash.Log_Write_IMU(); } }