// run the zigzag controller // should be called at 100hz or more void Copter::ModeZigZag::run() { // initialize vertical speed and acceleration's range pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { zero_throttle_and_relax_ac(); return; } // auto control if (stage == AUTO) { // if vehicle has reached destination switch to manual control if (reached_destination()) { AP_Notify::events.waypoint_complete = 1; stage = MANUAL_REGAIN; loiter_nav->init_target(wp_nav->get_wp_destination()); } else { auto_control(); } } // manual control if (stage == STORING_POINTS || stage == MANUAL_REGAIN) { // receive pilot's inputs, do position and attitude control manual_control(); } }
bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { // exit immediately if we haven't reached the destination if (!reached_destination()) { return false; } // Check if this is the first time we have noticed reaching the waypoint if (!previously_reached_wp) { previously_reached_wp = true; // check if we are loitering at this waypoint - the message sent to the GCS is different if (loiter_duration > 0) { // send message including loiter time gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds", static_cast<uint32_t>(cmd.index), static_cast<uint32_t>(loiter_duration)); // record the current time i.e. start timer loiter_start_time = millis(); } else { // send simpler message to GCS gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u", static_cast<uint32_t>(cmd.index)); } } // Check if we have loitered long enough if (loiter_duration == 0) { return true; } else { return (((millis() - loiter_start_time) / 1000) >= loiter_duration); } }
bool ModeAuto::verify_RTL() { return reached_destination(); }