void
Navigator::set_rtl_item()
{
reset_reached();
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
float climb_alt = _home_pos.alt + _parameters.rtl_alt;
if (_vstatus.condition_landed) {
climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
}
_mission_item_valid = true;
_mission_item.lat = _global_pos.lat;
_mission_item.lon = _global_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
break;
}
case RTL_STATE_RETURN: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
// don't change altitude
if (_pos_sp_triplet.previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
case RTL_STATE_DESCEND: {
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_mission_item_valid = true;
_mission_item.lat = _home_pos.lat;
_mission_item.lon = _home_pos.lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
_mission_item.origin = ORIGIN_ONBOARD;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
break;
}
default: {
mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
start_loiter();
break;
}
}
_pos_sp_triplet_updated = true;
}
void ModeGuided::update()
{
switch (_guided_mode) {
case Guided_WP:
{
_distance_to_destination = get_distance(rover.current_loc, _destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) {
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// determine if we should keep navigating
if (!_reached_destination) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
}
break;
}
case Guided_HeadingAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(_desired_speed, true, true);
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
}
break;
}
case Guided_TurnRateAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
calc_throttle(_desired_speed, true, true);
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
stop_vehicle();
}
} else {
stop_vehicle();
}
}
break;
}
case Guided_Loiter:
{
rover.mode_loiter.update();
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
break;
}
}
