// 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();