void init_devices( void ) { // set the Arduino's PWM timers to 3.921 KHz, above the acoustic range TCCR3B = (TCCR3B & 0xF8) | 0x02; // pins 2,3,5 | timer 3 TCCR4B = (TCCR4B & 0xF8) | 0x02; // pins 6,7,8 | timer 4 accumulator_init( ); master_cylinder_init( ); brake_init( ); accumulator_turn_pump_off( ); master_cylinder_open( ); set_release_solenoid_duty_cycle( SOLENOID_PWM_OFF ); set_accumulator_solenoid_duty_cycle( SOLENOID_PWM_OFF ); }
// set_mode - change flight mode and perform any necessary initialisation // optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was successfully set // ACRO, STABILIZE, ALTHOLD, LAND, DRIFT and SPORT can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) { // boolean to record if flight mode could be set bool success = false; bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform // return immediately if we are already in the desired mode if (mode == control_mode) { prev_control_mode = control_mode; prev_control_mode_reason = control_mode_reason; control_mode_reason = reason; return true; } switch(mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME success = heli_acro_init(ignore_checks); #else success = acro_init(ignore_checks); #endif break; case STABILIZE: #if FRAME_CONFIG == HELI_FRAME success = heli_stabilize_init(ignore_checks); #else success = stabilize_init(ignore_checks); #endif break; case ALT_HOLD: success = althold_init(ignore_checks); break; case AUTO: success = auto_init(ignore_checks); break; case CIRCLE: success = circle_init(ignore_checks); break; case LOITER: success = loiter_init(ignore_checks); break; case GUIDED: success = guided_init(ignore_checks); break; case LAND: success = land_init(ignore_checks); break; case RTL: success = rtl_init(ignore_checks); break; case DRIFT: success = drift_init(ignore_checks); break; case SPORT: success = sport_init(ignore_checks); break; case ALT_POS: success = altpos_init(ignore_checks); break; case FLIP: success = flip_init(ignore_checks); break; #if AUTOTUNE_ENABLED == ENABLED case AUTOTUNE: success = autotune_init(ignore_checks); break; #endif #if POSHOLD_ENABLED == ENABLED case POSHOLD: success = poshold_init(ignore_checks); break; #endif case BRAKE: success = brake_init(ignore_checks); break; case THROW: success = throw_init(ignore_checks); break; case AVOID_ADSB: success = avoid_adsb_init(ignore_checks); break; case GUIDED_NOGPS: success = guided_nogps_init(ignore_checks); break; default: success = false; break; } // update flight mode if (success) { // perform any cleanup required by previous flight mode exit_mode(control_mode, mode); prev_control_mode = control_mode; prev_control_mode_reason = control_mode_reason; control_mode = mode; control_mode_reason = reason; DataFlash.Log_Write_Mode(control_mode, control_mode_reason); adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED)); #if AC_FENCE == ENABLED // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe) // but it should be harmless to disable the fence temporarily in these situations as well fence.manual_recovery_start(); #endif }else{ // Log error that we failed to enter desired flight mode Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); } // update notify object if (success) { notify_flight_mode(control_mode); } // return success or failure return success; }