void
RCLoss::set_rcl_item()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous = pos_sp_triplet->current;
_navigator->set_can_loiter_at_sp(false);
switch (_rcl_state) {
case RCL_STATE_LOITER: {
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = _navigator->get_global_position()->alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
break;
}
case RCL_STATE_TERMINATE: {
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
warnx("rc not recovered: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
break;
}
default:
break;
}
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();
}
void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
{
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
// activate line following in pos control if position is valid
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.position_valid = use_position;
pos_sp_triplet->current.velocity_valid = use_velocity;
pos_sp_triplet->current.vx = _current_vel(0);
pos_sp_triplet->current.vy = _current_vel(1);
pos_sp_triplet->next.valid = false;
pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
pos_sp_triplet->current.yawspeed = yaw_rate;
_navigator->set_position_setpoint_triplet_updated();
}
void
Loiter::set_loiter_position()
{
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
_navigator->get_land_detected()->landed) {
// Not setting loiter position if disarmed and landed, instead mark the current
// setpoint as invalid and idle (both, just to be sure).
_navigator->set_can_loiter_at_sp(false);
_navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
_navigator->set_position_setpoint_triplet_updated();
_loiter_pos_set = false;
return;
} else if (_loiter_pos_set) {
// Already set, nothing to do.
return;
}
_loiter_pos_set = true;
// set current mission item to loiter
set_loiter_item(&_mission_item, _param_min_alt.get());
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
}
