// wait for an INS sample

if (!ins.wait_for_sample(1000)) {

Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY); return; }

uint32_t timer = micros();

// check loop time

perf_info_check_loop_time(timer - fast_loopTimer);

// used by PI Loops

G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; fast_loopTimer = timer;

// for mainloop failure monitoring mainLoop_count++;

// Execute the fast loop // --------------------- fast_loop();

// tell the scheduler one tick has passed scheduler.tick();

// run all the tasks that are due to run. Note that we only // have to call this once per loop, as the tasks are scheduled // in multiples of the main loop tick. So if they don't run on // the first call to the scheduler they won't run on a later // call until scheduler.tick() is called again

uint32_t time_available = (timer + 10000) - micros(); scheduler.run(time_available - 300); }

// Main loop - 100hz static void fast_loop() {

// IMU DCM Algorithm // -------------------- read_AHRS();

// reads all of the necessary trig functions for cameras, throttle, etc. // -------------------------------------------------------------------- update_trig(); // Acrobatic control if (ap.do_flip) {

if(abs(g.rc_1.control_in) < 4000) {

// calling roll_flip will override the desired roll rate and throttle output roll_flip(); }else{

// force an exit from the loop if we are not hands off sticks. ap.do_flip = false;

Log_Write_Event(DATA_EXIT_FLIP); } }

// run low level rate controllers that only require IMU data run_rate_controllers();

// write out the servo PWM values // ------------------------------

set_servos_4();

// Inertial Nav

// -------------------- read_inertia();

// optical flow

// -------------------- #if OPTFLOW == ENABLED

if(g.optflow_enabled) { update_optical_flow(); }

#endif // OPTFLOW == ENABLED

// Read radio and 3-position switch on radio // ----------------------------------------- read_radio();

read_control_switch();

// custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode();

update_roll_pitch_mode();

// update targets to rate controllers update_rate_contoller_targets(); }

// throttle_loop - should be run at 50 hz // --------------------------- static void throttle_loop() {

// get altitude and climb rate from inertial lib read_inertial_altitude();

// Update the throttle ouput // ------------------------- update_throttle_mode();

// check if we've landed update_land_detector();

#if TOY_EDF == ENABLED edf_toy(); #endif

// check auto_armed status update_auto_armed(); }

// update_mount - update camera mount position // should be run at 50hz static void update_mount() {

#if MOUNT == ENABLED

// update camera mount's position

camera_mount.update_mount_position(); #endif

#if MOUNT2 == ENABLED

// update camera mount's position

camera_mount2.update_mount_position();

#endif

#if CAMERA == ENABLED

camera.trigger_pic_cleanup(); #endif }

// update_batt_compass - read battery and compass // should be called at 10hz

static void update_batt_compass(void) {

// read battery before compass because it may be used for motor interference compensation read_battery();

#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled) { if (compass.read()) {

compass.null_offsets(); }

// log compass information

if (motors.armed() && (g.log_bitmask & MASK_LOG_COMPASS)) { Log_Write_Compass(); } } #endif

// record throttle output

throttle_integrator += g.rc_3.servo_out; }

// ten_hz_logging_loop // should be run at 10hz

static void ten_hz_logging_loop() {

if(motors.armed()) {

if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) { Log_Write_Attitude(); }

if (g.log_bitmask & MASK_LOG_MOTORS) { Log_Write_Motors(); } } }

// fifty_hz_logging_loop // should be run at 50hz

static void fifty_hz_logging_loop() {

#if HIL_MODE != HIL_MODE_DISABLED

// HIL for a copter needs very fast update of the servo values gcs_send_message(MSG_RADIO_OUT); #endif

# if HIL_MODE == HIL_MODE_DISABLED

if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motors.armed()) { Log_Write_Attitude(); }

if (g.log_bitmask & MASK_LOG_IMU && motors.armed()) { DataFlash.Log_Write_IMU(&ins); } #endif

}

// three_hz_loop - 3.3hz loop static void three_hz_loop() {

// check if we've lost contact with the ground station failsafe_gcs_check();

#if AC_FENCE == ENABLED

// check if we have breached a fence fence_check();

#endif // AC_FENCE_ENABLED

#if SPRAYER == ENABLED sprayer.update(); #endif

update_events();

if(g.radio_tuning > 0) tuning(); }

// one_hz_loop - runs at 1Hz static void one_hz_loop() {

if (g.log_bitmask != 0) {

Log_Write_Data(DATA_AP_STATE, ap.value); }

// pass latest alt hold kP value to navigation controller wp_nav.set_althold_kP(g.pi_alt_hold.kP());

// update latest lean angle to navigation controller wp_nav.set_lean_angle_max(g.angle_max);

// log battery info to the dataflash

if ((g.log_bitmask & MASK_LOG_CURRENT) && motors.armed()) Log_Write_Current();

// perform pre-arm checks pre_arm_checks(false);

// auto disarm checks auto_disarm_check();

if (!motors.armed()) {

// make it possible to change ahrs orientation at runtime during initial config ahrs.set_orientation();

// check the user hasn't updated the frame orientation motors.set_frame_orientation(g.frame_orientation); }

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4

update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12); #elif MOUNT == ENABLED

update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11); #endif

enable_aux_servos();