bool
Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
struct mission_item_s *next_position_mission_item, bool *has_next_position_item)
{
bool first_res = false;
int offset = 1;
if (read_mission_item(onboard, 0, mission_item)) {
first_res = true;
/* trying to find next position mission item */
while (read_mission_item(onboard, offset, next_position_mission_item)) {
if (item_contains_position(next_position_mission_item)) {
*has_next_position_item = true;
break;
}
offset++;
}
}
return first_res;
}
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return;
}
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :
_navigator->get_loiter_radius();
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
sp->acceptance_radius = item->acceptance_radius;
sp->disable_mc_yaw_control = false;
sp->cruising_speed = _navigator->get_cruising_speed();
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()){
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
sp->disable_mc_yaw_control = true;
}
break;
default:
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
}
void
MissionBlock::issue_command(const mission_item_s &item)
{
if (item_contains_position(item)) {
return;
}
// NAV_CMD_DO_LAND_START is only a marker
if (item.nav_cmd == NAV_CMD_DO_LAND_START) {
return;
}
if (item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_INFO("DO_SET_SERVO command");
// XXX: we should issue a vehicle command and handle this somewhere else
actuator_controls_s actuators = {};
actuators.timestamp = hrt_absolute_time();
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
// params[1] new value for selected actuator in ms 900...2000
actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1];
if (_actuator_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
}
} else {
_action_start = hrt_absolute_time();
// mission_item -> vehicle_command
// we're expecting a mission command item here so assign the "raw" inputs to the command
// (MAV_FRAME_MISSION mission item)
vehicle_command_s vcmd = {};
vcmd.command = item.nav_cmd;
vcmd.param1 = item.params[0];
vcmd.param2 = item.params[1];
vcmd.param3 = item.params[2];
vcmd.param4 = item.params[3];
vcmd.param5 = item.params[4];
vcmd.param6 = item.params[5];
vcmd.param7 = item.params[6];
_navigator->publish_vehicle_cmd(&vcmd);
}
}
void
MissionBlock::issue_command(const struct mission_item_s *item)
{
if (item_contains_position(item)) {
return;
}
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_WARN("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else
memset(&actuators, 0, sizeof(actuators));
// params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6
// params[1] new value for selected actuator in ms 900...2000
actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
actuators.timestamp = hrt_absolute_time();
if (_actuator_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
}
} else {
PX4_WARN("forwarding command %d\n", item->nav_cmd);
struct vehicle_command_s cmd = {};
mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time();
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
}
}
}
void
Mission::set_mission_items()
{
/* make sure param is up to date */
updateParams();
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
altitude_sp_foh_reset();
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
/* mission item that comes after current if available */
struct mission_item_s mission_item_next_position;
bool has_next_position_item = false;
work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* copy information about the previous mission item */
if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
/* Copy previous mission item altitude */
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* try setting onboard mission item */
if (_param_onboard_enabled.get()
&& prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
if (_navigator->get_land_detected()->landed) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, landed");
} else {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
}
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_NONE;
/* set loiter mission item and ensure that there is a minimum clearance from home */
set_loiter_item(&_mission_item, _param_takeoff_alt.get());
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
set_mission_finished();
if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
if (_navigator->get_land_detected()->landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
return;
}
/*********************************** handle mission item *********************************************/
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
/* force vtol land */
if (_mission_item.nav_cmd == NAV_CMD_LAND && _navigator->get_vstatus()->is_vtol
&& _param_force_vtol.get() && !_navigator->get_vstatus()->is_rotary_wing) {
_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
}
/* we have a new position item so set previous position setpoint to current */
set_previous_pos_setpoint();
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
has_next_position_item = true;
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home",
(double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
/* ignore yaw for takeoff items */
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* if we just did a takeoff navigate to the actual waypoint now */
if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) {
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
/* check if the vtol_takeoff command is on top of us */
if (do_need_move_to_takeoff()) {
new_work_item_type = WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
_mission_item.yaw = _navigator->get_global_position()->yaw;
} else {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
_mission_item.yaw = NAN;
}
}
/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
&& _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0.0f;
}
/* move to land wp as fixed wing */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_DEFAULT
&& !_navigator->get_land_detected()->landed) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
has_next_position_item = true;
float altitude = _navigator->get_global_position()->alt;
if (pos_sp_triplet->current.valid) {
altitude = pos_sp_triplet->current.alt;
}
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* transition to MC */
if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
&& _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed) {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
_mission_item.autocontinue = true;
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
}
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() &&
(_work_item_type == WORK_ITEM_TYPE_DEFAULT ||
_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
/* use current mission item as next position item */
memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
has_next_position_item = true;
/*
* Ignoring waypoint altitude:
* Set altitude to the same as we have now to prevent descending too fast into
* the ground. Actual landing will descend anyway until it touches down.
* XXX: We might want to change that at some point if it is clear to the user
* what the altitude means on this waypoint type.
*/
float altitude = _navigator->get_global_position()->alt;
_mission_item.altitude = altitude;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
}
/* we just moved to the landing waypoint, now descend */
if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
&& _navigator->get_vstatus()->is_rotary_wing) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
}
/* ignore yaw for landing items */
/* XXX: if specified heading for landing is desired we could add another step before the descent
* that aligns the vehicle first */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
_mission_item.yaw = NAN;
}
/* handle non-position mission items such as commands */
} else {
/* turn towards next waypoint before MC to FW transition */
if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
&& _work_item_type != WORK_ITEM_TYPE_ALIGN
&& _navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_land_detected()->landed
&& has_next_position_item) {
new_work_item_type = WORK_ITEM_TYPE_ALIGN;
set_align_mission_item(&_mission_item, &mission_item_next_position);
}
/* yaw is aligned now */
if (_work_item_type == WORK_ITEM_TYPE_ALIGN) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
set_previous_pos_setpoint();
}
}
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected against non-position items) */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
issue_command(&_mission_item);
/* set current work item type */
_work_item_type = new_work_item_type;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|| pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
_distance_current_previous = get_distance_to_next_waypoint(
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon,
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon);
}
_navigator->set_position_setpoint_triplet_updated();
}
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
/* set the correct setpoint for vtol transition */
if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw)
&& item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) {
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
waypoint_from_heading_and_distance(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
item->yaw,
1000000.0f,
&sp->lat,
&sp->lon);
sp->alt = _navigator->get_global_position()->alt;
}
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return;
}
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius :
_navigator->get_loiter_radius();
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
sp->acceptance_radius = item->acceptance_radius;
sp->disable_mc_yaw_control = false;
sp->cruising_speed = _navigator->get_cruising_speed();
sp->cruising_throttle = _navigator->get_cruising_throttle();
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
case NAV_CMD_VTOL_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
case NAV_CMD_VTOL_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
sp->disable_mc_yaw_control = true;
}
break;
default:
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
}
void
RTL::set_rtl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* make sure we have the latest params */
updateParams();
if (!_rtl_start_lock) {
set_previous_pos_setpoint();
}
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
// use home yaw if close to home
/* check if we are pretty close to home already */
float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
if (home_dist < _param_rtl_min_dist.get()) {
_mission_item.yaw = _navigator->get_home_position()->yaw;
} else {
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(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
_rtl_start_lock = true;
break;
}
case RTL_STATE_TRANSITION_TO_MC: {
_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
break;
}
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
// check if we are already lower - then we will just stay there
if (_mission_item.altitude > _navigator->get_global_position()->alt) {
_mission_item.altitude = _navigator->get_global_position()->alt;
}
_mission_item.yaw = _navigator->get_home_position()->yaw;
// except for vtol which might be still off here and should point towards this location
float d_current = get_distance_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
/* disable previous setpoint to prevent drift */
pos_sp_triplet->previous.valid = false;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
_mission_item.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)Navigator::get_time_inside(_mission_item));
} else {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
}
break;
}
case RTL_STATE_LAND: {
_mission_item.yaw = _navigator->get_home_position()->yaw;
set_land_item(&_mission_item, false);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
set_idle_item(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
break;
}
default:
break;
}
reset_mission_item_reached();
/* execute command if set */
if (!item_contains_position(&_mission_item)) {
issue_command(&_mission_item);
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
bool
MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
{
/* don't change the setpoint for non-position items */
if (!item_contains_position(item)) {
return false;
}
sp->lat = item.lat;
sp->lon = item.lon;
sp->alt = item.altitude_is_relative ? item.altitude + _navigator->get_home_position()->alt : item.altitude;
sp->yaw = item.yaw;
sp->yaw_valid = PX4_ISFINITE(item.yaw);
sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
_navigator->get_loiter_radius();
sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
sp->acceptance_radius = item.acceptance_radius;
sp->disable_mc_yaw_control = item.disable_mc_yaw;
sp->cruising_speed = _navigator->get_cruising_speed();
sp->cruising_throttle = _navigator->get_cruising_throttle();
switch (item.nav_cmd) {
case NAV_CMD_IDLE:
sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
// if already flying (armed and !landed) treat TAKEOFF like regular POSITION
if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !_navigator->get_land_detected()->landed) {
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
} else {
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
// set pitch and ensure that the hold time is zero
sp->pitch_min = item.pitch_min;
}
break;
case NAV_CMD_VTOL_TAKEOFF:
sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_takeoff.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LAND:
case NAV_CMD_VTOL_LAND:
sp->type = position_setpoint_s::SETPOINT_TYPE_LAND;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) {
sp->disable_mc_yaw_control = true;
}
break;
case NAV_CMD_LOITER_TO_ALT:
// initially use current altitude, and switch to mission item altitude once in loiter position
if (_param_loiter_min_alt.get() > 0.0f) { // ignore _param_loiter_min_alt if smaller then 0 (-1)
sp->alt = math::max(_navigator->get_global_position()->alt,
_navigator->get_home_position()->alt + _param_loiter_min_alt.get());
} else {
sp->alt = _navigator->get_global_position()->alt;
}
// fall through
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) {
sp->disable_mc_yaw_control = true;
}
break;
default:
sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
break;
}
sp->valid = true;
return sp->valid;
}
