void
Navigator::set_mission_item()
{
reset_reached();
/* copy current mission to previous item */
memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
_reset_loiter_pos = true;
_do_takeoff = false;
int ret;
bool onboard;
unsigned index;
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
_mission.report_current_offboard_mission_item();
_mission_item_valid = true;
position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
_mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
_mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
/* don't reset RTL state on RTL or LOITER items */
_rtl_state = RTL_STATE_NONE;
}
if (_vstatus.is_rotary_wing) {
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
)) {
/* do special TAKEOFF handling for VTOL */
_need_takeoff = false;
/* calculate desired takeoff altitude AMSL */
float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
if (_vstatus.condition_landed) {
/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt);
}
/* check if we really need takeoff */
if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) {
/* force TAKEOFF if landed or waypoint altitude is more than current */
_do_takeoff = true;
/* move current position setpoint to next */
memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
/* set current setpoint to takeoff */
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.alt = takeoff_alt_amsl;
_pos_sp_triplet.current.yaw = NAN;
_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
}
} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
/* will need takeoff after landing */
_need_takeoff = true;
}
}
if (_do_takeoff) {
mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
} else {
if (onboard) {
mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
} else {
mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
}
}
} else {
/* since a mission is not advanced without WPs available, this is not supposed to happen */
_mission_item_valid = false;
_pos_sp_triplet.current.valid = false;
warnx("ERROR: current WP can't be set");
}
if (!_do_takeoff) {
mission_item_s item_next;
ret = _mission.get_next_mission_item(&item_next);
if (ret == OK) {
position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
} else {
/* this will fail for the last WP */
_pos_sp_triplet.next.valid = false;
}
}
_pos_sp_triplet_updated = true;
}
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;
}
