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 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)
{
// 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 Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_at_dest)
{
// 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;
// get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
// in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN;
if (loiter_duration == 0) {
next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN);
}
// retrieve and sanitize target location
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd, stay_active_at_dest);
}
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch(packet.command) {
case MAV_CMD_DO_REPOSITION: {
// sanity check location
if (!check_latlng(packet.x, packet.y)) {
return MAV_RESULT_FAILED;
}
Location requested_position {};
requested_position.lat = packet.x;
requested_position.lng = packet.y;
// check the floating representation for overflow of altitude
if (fabsf(packet.z * 100.0f) >= 0x7fffff) {
return MAV_RESULT_FAILED;
}
requested_position.alt = (int32_t)(packet.z * 100.0f);
// load option flags
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
requested_position.relative_alt = 1;
}
else if (packet.frame == MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
requested_position.terrain_alt = 1;
}
else if (packet.frame != MAV_FRAME_GLOBAL_INT) {
// not a supported frame
return MAV_RESULT_FAILED;
}
if (is_zero(packet.param4)) {
requested_position.loiter_ccw = 0;
} else {
requested_position.loiter_ccw = 1;
}
if (location_sanitize(plane.current_loc, requested_position)) {
// if the location wasn't already sane don't load it
return MAV_RESULT_FAILED; // failed as the location is not valid
}
// location is valid load and set
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
(plane.control_mode == GUIDED)) {
plane.set_mode(GUIDED, MODE_REASON_GCS_COMMAND);
plane.guided_WP_loc = requested_position;
// add home alt if needed
if (plane.guided_WP_loc.relative_alt) {
plane.guided_WP_loc.alt += plane.home.alt;
plane.guided_WP_loc.relative_alt = 0;
}
plane.set_guided_WP();
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;
}
default:
return GCS_MAVLINK::handle_command_int_packet(packet);
}
}
