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; } }