// 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();
}
