// rtl_returnhome_run - return home // called by rtl_run at 100hz or more void Copter::ModeRTL::land_run(bool disarm_on_land) { // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); // disarm when the landing detector says we've landed if (ap.land_complete && disarm_on_land) { copter.init_disarm_motors(); } // check if we've completed this stage of RTL _state_complete = ap.land_complete; return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); // check if we've completed this stage of RTL _state_complete = ap.land_complete; }
// rtl_returnhome_run - return home // called by rtl_run at 100hz or more void Copter::rtl_land_run() { // if not auto armed or landing completed 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(0, 0, 0, get_smoothing_gain()); 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 // set target to current position wp_nav.init_loiter_target(); // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; return; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; }
// land_gps_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more void Copter::ModeLand::gps_run() { // 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()) { zero_throttle_and_relax_ac(); loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); // 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); // pause before beginning land descent if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; } land_run_horizontal_control(); land_run_vertical_control(land_pause); }
// land_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more void Copter::land_gps_run() { // 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(0, 0, 0, get_smoothing_gain()); 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 wp_nav.init_loiter_target(); // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } return; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; } land_run_horizontal_control(); land_run_vertical_control(land_pause); }
// 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); }
// rtl_returnhome_run - return home // called by rtl_run at 100hz or more void Copter::ModeRTL::land_run(bool disarm_on_land) { // check if we've completed this stage of RTL _state_complete = ap.land_complete; // disarm when the landing detector says we've landed if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) { copter.init_disarm_motors(); } // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); loiter_nav->init_target(); return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); }