// auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce change of flips) if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); return false; } // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true; }else{ return false; } }
// auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, // as this will force the helicopter to descend. if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ return false; } #endif if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true; }else{ return false; } }
// auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw_mode == AUTO_YAW_ROI) { set_auto_yaw_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller wp_nav.wp_and_spline_init(); // clear guided limits guided_limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); return true; }else{ return false; } }