void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd);
}
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
// we set start_time_ms when we reach the waypoint
loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds
loiter_set_direction_wp(cmd);
}
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{
//set target alt
next_WP_loc.alt = cmd.content.location.alt;
// convert relative alt to absolute alt
if (cmd.content.location.flags.relative_alt) {
next_WP_loc.flags.relative_alt = false;
next_WP_loc.alt += home.alt;
}
//I know I'm storing this twice -- I'm doing that on purpose --
//see verify_loiter_alt() function
condition_value = next_WP_loc.alt;
//are lat and lon 0? if so, don't change the current wp lat/lon
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
set_next_WP(cmd.content.location);
}
//set loiter direction
loiter_set_direction_wp(cmd);
//must the plane be heading towards the next waypoint before breaking?
condition_value2 = LOWBYTE(cmd.p1);
}
void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_time_max = 100; // an arbitrary large loiter time
distance_past_wp = 0;
}
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
condition_value = 1; // used to signify primary turns goal not yet met
}
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{
//set target alt
Location loc = cmd.content.location;
location_sanitize(current_loc, loc);
set_next_WP(loc);
loiter_set_direction_wp(cmd);
// init to 0, set to 1 when altitude is reached
condition_value = 0;
}
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd);
// we set start_time_ms when we reach the waypoint
loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms
condition_value = 1; // used to signify primary time goal not yet met
}
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds
loiter_time_max = cmd.p1;
// this is the distance we travel past the waypoint - not there yet so 0 initially
distance_past_wp = 0;
set_next_WP(cmd.content.location);
}
void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
prev_WP_loc = current_loc;
set_next_WP(cmd.content.location);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100;
auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt;
next_WP_loc.lat = home.lat + 10;
next_WP_loc.lng = home.lng + 10;
auto_state.takeoff_speed_time_ms = 0;
auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable
// zero locked course
steer_state.locked_course_err = 0;
}
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
// just starting so we haven't previously reached the waypoint
previously_reached_wp = false;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_start_time = 0;
// this is the delay, stored in seconds
loiter_duration = cmd.p1;
// this is the distance we travel past the waypoint - not there yet so 0 initially
distance_past_wp = 0;
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
}
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
// configure abort altitude and pitch
// if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m
if (cmd.p1 > 0) {
auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
} else if (auto_state.takeoff_altitude_rel_cm <= 0) {
auto_state.takeoff_altitude_rel_cm = 3000;
}
if (auto_state.takeoff_pitch_cd <= 0) {
// If no takeoff command has ever been used, default to a conservative 10deg
auto_state.takeoff_pitch_cd = 1000;
}
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
landing.do_land(cmd, relative_altitude);
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// if we were in an abort we need to explicitly move out of the abort state, as it's sticky
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
}
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
{
auto_state.commanded_go_around = false;
set_next_WP(cmd.content.location);
// configure abort altitude and pitch
// if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 50m
if (cmd.p1 > 0) {
auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
} else if (auto_state.takeoff_altitude_rel_cm <= 0) {
auto_state.takeoff_altitude_rel_cm = 3000;
}
if (auto_state.takeoff_pitch_cd <= 0) {
// If no takeoff command has ever been used, default to a conservative 10deg
auto_state.takeoff_pitch_cd = 1000;
}
#if RANGEFINDER_ENABLED == ENABLED
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
#endif
}
void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
{
prev_WP_loc = current_loc;
set_next_WP(cmd.content.location);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100;
if (auto_state.takeoff_pitch_cd <= 0) {
// if the mission doesn't specify a pitch use 4 degrees
auto_state.takeoff_pitch_cd = 400;
}
auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt;
next_WP_loc.lat = home.lat + 10;
next_WP_loc.lng = home.lng + 10;
auto_state.takeoff_speed_time_ms = 0;
auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
auto_state.height_below_takeoff_to_level_off_cm = 0;
// Flag also used to override "on the ground" throttle disable
// zero locked course
steer_state.locked_course_err = 0;
steer_state.hold_course_cd = -1;
auto_state.baro_takeoff_alt = barometer.get_altitude();
}
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
loiter_set_direction_wp(cmd);
}
void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
loiter_set_direction_wp(cmd);
}
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
{
auto_state.commanded_go_around = false;
set_next_WP(cmd.content.location);
}
void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
}
