// motors_output - send output to motors library which will adjust and send to ESCs and servos void Copter::motors_output() { #if ADVANCED_FAILSAFE == ENABLED // this is to allow the failsafe module to deliberately crash // the vehicle. Only used in extreme circumstances to meet the // OBC rules if (g2.afs.should_crash_vehicle()) { g2.afs.terminate_vehicle(); return; } #endif // Update arming delay state if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) { ap.in_arming_delay = false; } // check if we are performing the motor test if (ap.motor_test) { motor_test_output(); } else { bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop; if (!motors.get_interlock() && interlock) { motors.set_interlock(true); Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); } else if (motors.get_interlock() && !interlock) { motors.set_interlock(false); Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); } // send output signals to motors motors.output(); } }
// Run landing gear controller at 10Hz void Copter::landinggear_update() { // exit immediately if no landing gear output has been enabled if (!SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) { return; } // last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear static bool last_deploy_status = landinggear.deployed(); // if we are doing an automatic landing procedure, force the landing gear to deploy. // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || (control_mode == AUTO && auto_mode == Auto_Land) || (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); } // send event message to datalog if status has changed if (landinggear.deployed() != last_deploy_status) { if (landinggear.deployed()) { Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); } else { Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); } } last_deploy_status = landinggear.deployed(); }
// Run landing gear controller at 10Hz void Sub::landinggear_update(){ // If landing gear control is active, run update function. if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ // last status (deployed or retracted) used to check for changes static bool last_deploy_status; // if we are doing an automatic landing procedure, force the landing gear to deploy. // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || (control_mode == AUTO && auto_mode == Auto_Land) || (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { landinggear.force_deploy(true); } landinggear.update(); // send event message to datalog if status has changed if (landinggear.deployed() != last_deploy_status){ if (landinggear.deployed()) { Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); } else { Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); } } last_deploy_status = landinggear.deployed(); } }
void Copter::set_motor_emergency_stop(bool b) { if(ap.motor_emergency_stop != b) { ap.motor_emergency_stop = b; } // Log new status if (ap.motor_emergency_stop){ Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED); } else { Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED); } }
// motors_output - send output to motors library which will adjust and send to ESCs and servos void Copter::motors_output() { #if ADVANCED_FAILSAFE == ENABLED // this is to allow the failsafe module to deliberately crash // the vehicle. Only used in extreme circumstances to meet the // OBC rules if (g2.afs.should_crash_vehicle()) { g2.afs.terminate_vehicle(); if (!g2.afs.terminating_vehicle_via_landing()) { return; } // landing must continue to run the motors output } #endif // Update arming delay state if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) { ap.in_arming_delay = false; } // output any servo channels SRV_Channels::calc_pwm(); // cork now, so that all channel outputs happen at once SRV_Channels::cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); // check if we are performing the motor test if (ap.motor_test) { motor_test_output(); } else { bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); if (!motors->get_interlock() && interlock) { motors->set_interlock(true); Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); } else if (motors->get_interlock() && !interlock) { motors->set_interlock(false); Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); } // send output signals to motors motors->output(); } // push all channels SRV_Channels::push(); }
void Sub::set_surfaced(bool at_surface) { if(ap.at_surface == at_surface) { // do nothing if state unchanged return; } ap.at_surface = at_surface; if(!ap.at_surface) { surface_detector_count = 0; Log_Write_Event(DATA_SURFACED); gcs_send_text(MAV_SEVERITY_INFO, "Off Surface"); } else { Log_Write_Event(DATA_NOT_SURFACED); gcs_send_text(MAV_SEVERITY_INFO, "Surfaced"); } }
void Sub::set_bottomed(bool at_bottom) { if(ap.at_bottom == at_bottom) { // do nothing if state unchanged return; } ap.at_bottom = at_bottom; if(!ap.at_bottom) { bottom_detector_count = 0; Log_Write_Event(DATA_BOTTOMED); gcs_send_text(MAV_SEVERITY_INFO, "Off Bottom"); } else { Log_Write_Event(DATA_NOT_BOTTOMED); gcs_send_text(MAV_SEVERITY_INFO, "Bottomed"); } }
void Sub::set_bottomed(bool at_bottom) { if (ap.at_bottom == at_bottom) { // do nothing if state unchanged return; } ap.at_bottom = at_bottom; bottom_detector_count = 0; if (ap.at_bottom) { Log_Write_Event(DATA_BOTTOMED); } else { Log_Write_Event(DATA_NOT_BOTTOMED); } }
// do_gripper - control gripper void Copter::do_gripper(const AP_Mission::Mission_Command& cmd) { // Note: we ignore the gripper num parameter because we only support one gripper switch (cmd.content.gripper.action) { case GRIPPER_ACTION_RELEASE: g2.gripper.release(); Log_Write_Event(DATA_GRIPPER_RELEASE); break; case GRIPPER_ACTION_GRAB: g2.gripper.grab(); Log_Write_Event(DATA_GRIPPER_GRAB); break; default: // do nothing break; } }
// flip_init - initialise flip controller bool Copter::flip_init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // Flip mode not available for helis as it is untested. return false; #endif // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) { return false; } // if in acro or stabilize ensure throttle is above zero if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) { return false; } // ensure roll input is less than 40deg if (abs(channel_roll->control_in) >= 4000) { return false; } // only allow flip when flying if (!motors.armed() || ap.land_complete) { return false; } // capture original flight mode so that we can return to it after completion flip_orig_control_mode = control_mode; // initialise state flip_state = Flip_Start; flip_start_time = millis(); flip_roll_dir = flip_pitch_dir = 0; // choose direction based on pilot's roll and pitch sticks if (channel_pitch->control_in > 300) { flip_pitch_dir = FLIP_PITCH_BACK; }else if(channel_pitch->control_in < -300) { flip_pitch_dir = FLIP_PITCH_FORWARD; }else if (channel_roll->control_in >= 0) { flip_roll_dir = FLIP_ROLL_RIGHT; }else{ flip_roll_dir = FLIP_ROLL_LEFT; } // log start of flip Log_Write_Event(DATA_FLIP_START); // capture current attitude which will be used during the Flip_Recovery stage flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max); flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max); flip_orig_attitude.z = ahrs.yaw_sensor; return true; }
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object void Copter::heli_update_rotor_speed_targets() { static bool rotor_runup_complete_last = false; // get rotor control method uint8_t rsc_control_mode = motors.get_rsc_mode(); rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in); switch (rsc_control_mode) { case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH: // pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom if (rsc_control_deglitched > 10) { motors.set_interlock(true); motors.set_desired_rotor_speed(rsc_control_deglitched); } else { motors.set_interlock(false); motors.set_desired_rotor_speed(0); } break; case ROTOR_CONTROL_MODE_SPEED_SETPOINT: case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT: case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT: // pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode // other than being used to create a crude estimate of rotor speed if (rsc_control_deglitched > 0) { motors.set_interlock(true); motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); }else{ motors.set_interlock(false); motors.set_desired_rotor_speed(0); } break; } // when rotor_runup_complete changes to true, log event if (!rotor_runup_complete_last && motors.rotor_runup_complete()){ Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE); } else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){ Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL); } rotor_runup_complete_last = motors.rotor_runup_complete(); }
// land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more void Copter::ModeLand::nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; // process pilot inputs if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max()); } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); attitude_control->set_throttle_out(0,false,g.throttle_filt); #else motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // disarm when the landing detector says we've landed if (ap.land_complete) { copter.init_disarm_motors(); } return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // pause before beginning land descent if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; } land_run_vertical_control(land_pause); }
// save_trim - adds roll and pitch trims from the radio to ahrs void Copter::save_trim() { // save roll and pitch trim float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); Log_Write_Event(DATA_SAVE_TRIM); gcs_send_text(MAV_SEVERITY_INFO, "Trim saved"); }
// save_trim - adds roll and pitch trims from the radio to ahrs void Copter::save_trim() { // save roll and pitch trim float roll_trim = ToRad((float)channel_roll->control_in/100.0f); float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f); ahrs.add_trim(roll_trim, pitch_trim); Log_Write_Event(DATA_SAVE_TRIM); gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Trim saved")); }
// --------------------------------------------- void Copter::set_simple_mode(uint8_t b) { if(ap.simple_mode != b) { if(b == 0) { Log_Write_Event(DATA_SET_SIMPLE_OFF); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off"); } else if(b == 1) { Log_Write_Event(DATA_SET_SIMPLE_ON); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on"); } else { // initialise super simple heading update_super_simple_bearing(true); Log_Write_Event(DATA_SET_SUPERSIMPLE_ON); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); } ap.simple_mode = b; } }
void Sub::set_surfaced(bool at_surface) { if (ap.at_surface == at_surface) { // do nothing if state unchanged return; } ap.at_surface = at_surface; surface_detector_count = 0; if (ap.at_surface) { Log_Write_Event(DATA_SURFACED); } else { Log_Write_Event(DATA_NOT_SURFACED); } }
// set_home - sets ahrs home (used for RTL) to specified location // initialises inertial nav and compass on first call // returns true if home location set successfully bool Copter::set_home(const Location& loc, bool lock) { // check location is valid if (loc.lat == 0 && loc.lng == 0) { return false; } // check EKF origin has been set Location ekf_origin; if (!ahrs.get_origin(ekf_origin)) { return false; } // check home is close to EKF origin if (far_from_EKF_origin(loc)) { return false; } const bool home_was_set = ahrs.home_is_set(); // set ahrs home (used for RTL) ahrs.set_home(loc); // init inav and compass declination if (!home_was_set) { // update navigation scalers. used to offset the shrinking longitude as we go towards the poles scaleLongDown = longitude_scale(loc); // record home is set Log_Write_Event(DATA_SET_HOME); #if MODE_AUTO_ENABLED == ENABLED // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { AP_Mission::Mission_Command temp_cmd; if (mission.read_cmd_from_storage(0, temp_cmd)) { DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd); } } #endif } // lock home position if (lock) { ahrs.lock_home(); } // log ahrs home and ekf origin dataflash ahrs.Log_Write_Home_And_Origin(); // send new home and ekf origin to GCS gcs().send_home(); gcs().send_ekf_origin(); // return success return true; }
// flip_init - initialise flip controller bool Copter::ModeFlip::init(bool ignore_checks) { // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (copter.control_mode != ACRO && copter.control_mode != STABILIZE && copter.control_mode != ALT_HOLD && copter.control_mode != FLOWHOLD) { return false; } // if in acro or stabilize ensure throttle is above zero if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) { return false; } // ensure roll input is less than 40deg if (abs(channel_roll->get_control_in()) >= 4000) { return false; } // only allow flip when flying if (!motors->armed() || ap.land_complete) { return false; } // capture original flight mode so that we can return to it after completion orig_control_mode = copter.control_mode; // initialise state _state = FlipState::Start; start_time_ms = millis(); roll_dir = pitch_dir = 0; // choose direction based on pilot's roll and pitch sticks if (channel_pitch->get_control_in() > 300) { pitch_dir = FLIP_PITCH_BACK; } else if (channel_pitch->get_control_in() < -300) { pitch_dir = FLIP_PITCH_FORWARD; } else if (channel_roll->get_control_in() >= 0) { roll_dir = FLIP_ROLL_RIGHT; } else { roll_dir = FLIP_ROLL_LEFT; } // log start of flip Log_Write_Event(DATA_FLIP_START); // capture current attitude which will be used during the FlipState::Recovery stage const float angle_max = copter.aparm.angle_max; orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max); orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max); orig_attitude.z = ahrs.yaw_sensor; return true; }
// check for ekf yaw reset and adjust target heading void Copter::check_ekf_yaw_reset() { float yaw_angle_change_rad = 0.0f; uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); if (new_ekfYawReset_ms != ekfYawReset_ms) { attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f); ekfYawReset_ms = new_ekfYawReset_ms; Log_Write_Event(DATA_EKF_YAW_RESET); } }
// do_parachute - configure or release parachute void Copter::do_parachute(const AP_Mission::Mission_Command& cmd) { switch (cmd.p1) { case PARACHUTE_DISABLE: parachute.enabled(false); Log_Write_Event(DATA_PARACHUTE_DISABLED); break; case PARACHUTE_ENABLE: parachute.enabled(true); Log_Write_Event(DATA_PARACHUTE_ENABLED); break; case PARACHUTE_RELEASE: parachute_release(); break; default: // do nothing break; } }
// --------------------------------------------- void Copter::set_auto_armed(bool b) { // if no change, exit immediately if( ap.auto_armed == b ) return; ap.auto_armed = b; if(b){ Log_Write_Event(DATA_AUTO_ARMED); } }
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user void Copter::parachute_release() { // send message to gcs and dataflash gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released"); Log_Write_Event(DATA_PARACHUTE_RELEASED); // disarm motors init_disarm_motors(); // release parachute parachute.release(); }
// control winch based on mission command void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd) { // Note: we ignore the gripper num parameter because we only support one gripper switch (cmd.content.winch.action) { case WINCH_RELAXED: g2.winch.relax(); Log_Write_Event(DATA_WINCH_RELAXED); break; case WINCH_RELATIVE_LENGTH_CONTROL: g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate); Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); break; case WINCH_RATE_CONTROL: g2.winch.set_desired_rate(cmd.content.winch.release_rate); Log_Write_Event(DATA_WINCH_RATE_CONTROL); break; default: // do nothing break; } }
// --------------------------------------------- void Copter::set_simple_mode(uint8_t b) { if(ap.simple_mode != b){ switch (b) { case 0: Log_Write_Event(DATA_SET_SIMPLE_OFF); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off"); break; case 1: Log_Write_Event(DATA_SET_SIMPLE_ON); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on"); break; default: // initialise super simple heading update_super_simple_bearing(true); Log_Write_Event(DATA_SET_SUPERSIMPLE_ON); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); break; } ap.simple_mode = b; } }
// init_disarm_motors - disarm motors void Copter::init_disarm_motors() { // return immediately if we are already disarmed if (!motors.armed()) { return; } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors"); #endif // save compass offsets learned by the EKF if enabled if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) { for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { Vector3f magOffsets; if (ahrs.getMagOffsets(i, magOffsets)) { compass.set_and_save_offsets(i, magOffsets); } } } #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters autotune_save_tuning_gains(); #endif // we are not in the air set_land_complete(true); set_land_complete_maybe(true); // log disarm to the dataflash Log_Write_Event(DATA_DISARMED); // send disarm command to motors motors.armed(false); // reset the mission mission.reset(); // suspend logging if (!DataFlash.log_while_disarmed()) { DataFlash.EnableWrites(false); } DataFlash_Class::instance()->set_vehicle_armed(false); // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); ap.in_arming_delay = false; }
void Sub::set_land_complete(bool b) { // if no change, exit immediately if( ap.land_complete == b ) return; land_detector_count = 0; if(b){ Log_Write_Event(DATA_LAND_COMPLETE); } else { Log_Write_Event(DATA_NOT_LANDED); } ap.land_complete = b; // trigger disarm-on-land if configured bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0; bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode); if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) { init_disarm_motors(); } }
// set_home_state - update home state void Copter::set_home_state(enum HomeState new_home_state) { // if no change, exit immediately if (ap.home_state == new_home_state) return; // update state ap.home_state = new_home_state; // log if home has been set if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) { Log_Write_Event(DATA_SET_HOME); } }
// checks if we should update ahrs/RTL home position from GPS void Copter::set_system_time_from_GPS() { // exit immediately if system time already set if (ap.system_time_set) { return; } // if we have a 3d lock and valid location if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { // set system clock for log timestamps hal.util->set_system_clock(gps.time_epoch_usec()); ap.system_time_set = true; Log_Write_Event(DATA_SYSTEM_TIME_SET); } }
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user void Sub::parachute_release() { // send message to gcs and dataflash gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released"); Log_Write_Event(DATA_PARACHUTE_RELEASED); // disarm motors init_disarm_motors(); // release parachute parachute.release(); // deploy landing gear landinggear.set_cmd_mode(LandingGear_Deploy); }
// init_disarm_motors - disarm motors void Copter::init_disarm_motors() { // return immediately if we are already disarmed if (!motors.armed()) { return; } #if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL gcs_send_text(MAV_SEVERITY_INFO, "DISARMING MOTORS"); #endif // save compass offsets learned by the EKF Vector3f magOffsets; if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) { compass.set_and_save_offsets(compass.get_primary(), magOffsets); } #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters autotune_save_tuning_gains(); #endif // we are not in the air set_land_complete(true); set_land_complete_maybe(true); // log disarm to the dataflash Log_Write_Event(DATA_DISARMED); // send disarm command to motors motors.armed(false); // reset the mission mission.reset(); // suspend logging if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) { DataFlash.EnableWrites(false); } // disable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(false); hal.util->set_soft_armed(false); }