void
Mission::altitude_sp_foh_update()
{
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
!PX4_ISFINITE(_mission_item_previous_alt)) {
return;
}
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < FLT_EPSILON) {
return;
}
/* Don't do FOH for non-missions, landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic
*
* NAV_CMD_LOITER_TO_ALT doesn't change altitude until reaching desired lat/lon
* */
if (_mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT
|| !_navigator->is_planned_mission()) {
return;
}
/* Calculate distance to current waypoint */
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
/* Save distance to waypoint if it is the smallest ever achieved, however make sure that
* _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */
_min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy),
_distance_current_previous);
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
} else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
* of the mission item as it is used to check if the mission item is reached
* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
* radius around the current waypoint
**/
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
_mission_item.acceptance_radius));
float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
// we set altitude directly so we can run this in parallel to the heading update
_navigator->set_position_setpoint_triplet_updated();
}
/*
* Calculate distance in global frame.
*
* The distance calculation is based on the WGS84 geoid (GPS)
*/
float wpm_distance_to_point_global_wgs84(float sp_alt, float sp_lat, float sp_lon, float alt, float lat, float lon, float *dist_xy, float *dist_z)
{
float dxy = get_distance_to_next_waypoint (lat, lon, sp_lat, sp_lon);
float dz = alt - sp_alt;
*dist_xy = fabsf(dxy);
*dist_z = fabsf(dz);
return sqrtf(dxy * dxy + dz * dz);
}
bool
MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
return true;
}
/* find first waypoint (with lat/lon) item in datamanager */
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
}
/* check only items with valid lat/lon */
if (!MissionBlock::item_contains_position(mission_item)) {
continue;
}
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
if (dist_to_1wp < max_distance) {
if (dist_to_1wp > ((max_distance * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"First waypoint far away: %d meters.", (int)dist_to_1wp);
_navigator->get_mission_result()->warning = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"First waypoint too far away: %d meters, %d max.",
(int)dist_to_1wp, (int)max_distance);
_navigator->get_mission_result()->warning = true;
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
return true;
}
bool
MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
return true;
}
double last_lat = (double)NAN;
double last_lon = (double)NAN;
/* Go through all waypoints */
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
}
/* check only items with valid lat/lon */
if (!MissionBlock::item_contains_position(mission_item)) {
continue;
}
/* Compare it to last waypoint if already available. */
if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {
/* check distance from current position to item */
const float dist_between_waypoints = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
last_lat, last_lon);
if (dist_between_waypoints > max_distance) {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Distance between waypoints too far: %d meters, %d max.",
(int)dist_between_waypoints, (int)max_distance);
_navigator->get_mission_result()->warning = true;
return false;
}
}
last_lat = mission_item.lat;
last_lon = mission_item.lon;
}
/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
return true;
}
void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
yaw_vector.normalize();
float ground_speed_body = yaw_vector * ground_speed;
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
float delta_altitude = 0.0f;
if (pos_sp_triplet.previous.valid) {
distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt;
} else {
distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt;
}
float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance));
/*
* Ground speed undershoot is the amount of ground velocity not reached
* by the plane. Consequently it is zero if airspeed is >= min ground speed
* and positive if airspeed < min ground speed.
*
* This error value ensures that a plane (as long as its throttle capability is
* not exceeded) travels towards a waypoint (and is not pushed more and more away
* by wind). Not countering this would lead to a fly-away.
*/
_groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f);
} else {
_groundspeed_undershoot = 0;
}
}
bool
Mission::do_need_move_to_takeoff()
{
if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
return d_current > _navigator->get_acceptance_radius();
}
return false;
}
__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
// headed towards the end point.
float dist_to_end;
float bearing_end;
float bearing_track;
float bearing_diff;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (dist_to_end < 0.1f) {
return ERROR;
}
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
bearing_diff = bearing_track - bearing_end;
bearing_diff = _wrap_pi(bearing_diff);
// Return past_end = true if past end point of line
if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
crosstrack_error->past_end = true;
return_value = OK;
return return_value;
}
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
} else {
crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
}
return_value = OK;
return return_value;
}
void
Loiter::reposition()
{
// we can't reposition if we are not armed yet
if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
return;
}
struct position_setpoint_triplet_s *rep = _navigator->get_reposition_triplet();
if (rep->current.valid) {
// set loiter position based on reposition command
// 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.yaw = _navigator->get_global_position()->yaw;
pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
pos_sp_triplet->next.valid = false;
// set yaw (depends on the value of parameter MIS_YAWMODE):
// MISSION_YAWMODE_NONE: do not change yaw setpoint
// MISSION_YAWMODE_FRONT_TO_WAYPOINT: point to next waypoint
if (_param_yawmode.get() != MISSION_YAWMODE_NONE) {
float travel_dist = get_distance_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
if (travel_dist > 1.0f) {
// calculate direction the vehicle should point to.
pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
}
}
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
_navigator->set_position_setpoint_triplet_updated();
// mark this as done
memset(rep, 0, sizeof(*rep));
}
}
void
PrecLand::run_state_start()
{
// check if target visible and go to horizontal approach
if (switch_to_state_horizontal_approach()) {
return;
}
if (_mode == PrecLandMode::Opportunistic) {
// could not see the target immediately, so just fall back to normal landing
if (!switch_to_state_fallback()) {
PX4_ERR("Can't switch to search or fallback landing");
}
}
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
// check if we've reached the start point
if (dist < _navigator->get_acceptance_radius()) {
if (!_point_reached_time) {
_point_reached_time = hrt_absolute_time();
}
// if we don't see the target after 1 second, search for it
if (_param_search_timeout.get() > 0) {
if (hrt_absolute_time() - _point_reached_time > 2000000) {
if (!switch_to_state_search()) {
if (!switch_to_state_fallback()) {
PX4_ERR("Can't switch to search or fallback landing");
}
}
}
} else {
if (!switch_to_state_fallback()) {
PX4_ERR("Can't switch to search or fallback landing");
}
}
}
}
void
RTL::on_activation()
{
/* reset starting point so we override what the triplet contained from the previous navigation state */
_rtl_start_lock = false;
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* 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);
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_land_detected()->landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");
/* if lower than return altitude, climb up first */
} else if (home_dist > _param_rtl_min_dist.get()
&& _navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item();
}
bool
MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems,
fw_pos_ctrl_status_s *fw_pos_ctrl_status, bool land_start_req)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
bool landing_valid = false;
bool land_start_found = false;
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
// if DO_LAND_START found then require valid landing AFTER
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
if (land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
return false;
} else {
land_start_found = true;
do_land_start_index = i;
}
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
mission_item_s missionitem_previous {};
if (i > 0) {
landing_approach_index = i - 1;
if (dm_read(dm_current, landing_approach_index, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (MissionBlock::item_contains_position(missionitem_previous)) {
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);
if (wp_distance > fw_pos_ctrl_status->landing_flare_length) {
/* Last wp is before flare region */
const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
if (delta_altitude < 0) {
if (missionitem_previous.altitude > slope_alt_req) {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %.1fm or move further away by %.1fm.",
(double)(slope_alt_req - missionitem_previous.altitude),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.");
return false;
}
} else {
/* Last wp is in flare region */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.");
return false;
}
landing_valid = true;
} else {
// mission item before land doesn't have a position
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.");
return false;
}
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
return false;
}
}
}
if (land_start_req && !land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start required.");
return false;
}
if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.");
return false;
}
/* No landing waypoints or no waypoints */
return true;
}
void
BottleDrop::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
bool updated = false;
float z_0; // ground properties
float turn_radius; // turn radius of the UAV
float precision; // Expected precision of the UAV
float ground_distance = _alt_clearance; // Replace by closer estimate in loop
// constant
float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2]
float m = 0.5f; // mass of bottle [kg]
float rho = 1.2f; // air density [kg/m^3]
float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s]
// Has to be estimated by experiment
float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 []
float t_signal =
0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
float t_door =
0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
// Definition
float h_0; // height over target
float az; // acceleration in z direction[m/s^2]
float vz; // velocity in z direction [m/s]
float z; // fallen distance [m]
float h; // height over target [m]
float ax; // acceleration in x direction [m/s^2]
float vx; // ground speed in x direction [m/s]
float x; // traveled distance in x direction [m]
float vw; // wind speed [m/s]
float vrx; // relative velocity in x direction [m/s]
float v; // relative speed vector [m/s]
float Fd; // Drag force [N]
float Fdx; // Drag force in x direction [N]
float Fdz; // Drag force in z direction [N]
float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED)
float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
float x_l, y_l; // local position in projected coordinates
float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates
double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED
float distance_open_door; // The distance the UAV travels during its doors open [m]
float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector
float distance_real = 0; // The distance between the UAVs position and the drop point [m]
float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m]
unsigned counter = 0;
param_t param_gproperties = param_find("BD_GPROPERTIES");
param_t param_turn_radius = param_find("BD_TURNRADIUS");
param_t param_precision = param_find("BD_PRECISION");
param_t param_cd = param_find("BD_OBJ_CD");
param_t param_mass = param_find("BD_OBJ_MASS");
param_t param_surface = param_find("BD_OBJ_SURFACE");
param_get(param_precision, &precision);
param_get(param_turn_radius, &turn_radius);
param_get(param_gproperties, &z_0);
param_get(param_cd, &cd);
param_get(param_mass, &m);
param_get(param_surface, &A);
int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct parameter_update_s update;
memset(&update, 0, sizeof(update));
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
struct mission_item_s flight_vector_s {};
struct mission_item_s flight_vector_e {};
flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_s.acceptance_radius = 50; // TODO: make parameter
flight_vector_s.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_e.acceptance_radius = 50; // TODO: make parameter
flight_vector_e.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
struct wind_estimate_s wind;
// wakeup source(s)
struct pollfd fds[1];
// Setup of loop
fds[0].fd = _command_sub;
fds[0].events = POLLIN;
// Whatever state the bay is in, we want it closed on startup
lock_release();
close_bay();
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* vehicle commands updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
/* copy global position */
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
if (_global_pos.timestamp == 0) {
continue;
}
const unsigned sleeptime_us = 9500;
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = sleeptime_us / 1e6f;
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate
orb_check(_wind_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
}
// Get vehicle position
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
// Get parameter updates
orb_check(parameter_update_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
// update all parameters
param_get(param_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision);
}
orb_check(_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
ground_distance = _global_pos.alt - _target_position.alt;
// Distance to drop position and angle error to approach vector
// are relevant in all states greater than target valid (which calculates these positions)
if (_drop_state > DROP_STATE_TARGET_VALID) {
distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
}
}
switch (_drop_state) {
case DROP_STATE_INIT:
// do nothing
break;
case DROP_STATE_TARGET_VALID:
{
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;
float wind_direction_n, wind_direction_e;
if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen
wind_direction_n = 1.0f;
wind_direction_e = 0.0f;
} else {
wind_direction_n = wind.windspeed_north / windspeed_norm;
wind_direction_e = wind.windspeed_east / windspeed_norm;
}
x_drop = x_t + x * wind_direction_n;
y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance;
// Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
&(flight_vector_s.lat), &(flight_vector_s.lon));
flight_vector_s.altitude = _drop_position.alt;
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
&flight_vector_e.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt;
// Save WPs in datamanager
const ssize_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
warnx("ERROR: could not save onboard WP");
}
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
warnx("ERROR: could not save onboard WP");
}
_onboard_mission.count = 2;
_onboard_mission.current_seq = 0;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
}
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
}
break;
case DROP_STATE_TARGET_SET:
{
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
// We're close enough - open the bay
distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
if (isfinite(distance_real) && distance_real < distance_open_door &&
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
}
}
}
break;
case DROP_STATE_BAY_OPEN:
{
if (_drop_approval) {
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
if (isfinite(distance_real) &&
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
} else {
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
}
}
}
break;
case DROP_STATE_DROPPED:
/* 2s after drop, reset and close everything again */
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
_drop_state = DROP_STATE_INIT;
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
_onboard_mission.count = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
break;
case DROP_STATE_BAY_CLOSED:
// do nothing
break;
}
counter++;
// update_actuators();
// run at roughly 100 Hz
usleep(sleeptime_us);
dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
last_run = hrt_absolute_time();
}
}
warnx("exiting.");
_main_task = -1;
_exit(0);
}
void
Mission::set_mission_items()
{
/* make sure param is up to date */
updateParams();
/* reset the altitude foh 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();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
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) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(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_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
return;
}
if (pos_sp_triplet->current.valid) {
_on_arrival_yaw = _mission_item.yaw;
}
/* do takeoff on first waypoint for rotary wing vehicles */
if (_navigator->get_vstatus()->is_rotary_wing) {
/* force takeoff if landed (additional protection) */
if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
/* new current mission item set, check if we need takeoff */
if (_need_takeoff && (
_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
_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 ||
_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
_takeoff = true;
_need_takeoff = false;
}
}
if (_takeoff) {
/* do takeoff before going to setpoint */
/* set mission item as next position setpoint */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
/* next SP is not takeoff anymore */
pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
/* calculate takeoff altitude */
float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
} else {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "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;
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
} else {
/* skip takeoff */
_takeoff = false;
}
}
if (_takeoff_finished) {
/* we just finished takeoff */
/* in case we still have to move to the takeoff waypoint we need a waypoint mission item */
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_takeoff_finished = false;
}
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
/* 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 read next mission item */
struct mission_item_s mission_item_next;
if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next, &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();
}
__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now,
double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
// headed towards the end point.
// Determine if the current position is inside or outside the sector between the line from the center
// to the arc start and the line from the center to the arc end
float bearing_sector_start;
float bearing_sector_end;
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
bool in_sector;
int return_value = ERROR; // Set error flag, cleared when valid result calculated.
crosstrack_error->past_end = false;
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (radius < 0.1f) { return return_value; }
if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
}
in_sector = false;
// Case where sector does not span zero
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start
&& bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero
if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start
|| bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point
if (in_sector) {
crosstrack_error->past_end = false;
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
if (dist_to_center <= radius) {
crosstrack_error->distance = radius - dist_to_center;
crosstrack_error->bearing = bearing_now + M_PI_F;
} else {
crosstrack_error->distance = dist_to_center - radius;
crosstrack_error->bearing = bearing_now;
}
// If out of the sector then calculate dist and bearing to start or end point
} else {
// Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude)
// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
// calculate the position of the start and end points. We should not be doing this often
// as this function generally will not be called repeatedly when we are out of the sector.
double start_disp_x = (double)radius * sin(arc_start_bearing);
double start_disp_y = (double)radius * cos(arc_start_bearing);
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
if (dist_to_start < dist_to_end) {
crosstrack_error->distance = dist_to_start;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
} else {
crosstrack_error->past_end = true;
crosstrack_error->distance = dist_to_end;
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
}
}
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
bool
MissionBlock::is_mission_item_reached()
{
/* handle non-navigation or indefinite waypoints */
switch (_mission_item.nav_cmd) {
case NAV_CMD_DO_SET_SERVO:
return true;
case NAV_CMD_LAND: /* fall through */
case NAV_CMD_VTOL_LAND:
return _navigator->get_land_detected()->landed;
case NAV_CMD_IDLE: /* fall through */
case NAV_CMD_LOITER_UNLIMITED:
return false;
case NAV_CMD_DO_LAND_START:
case NAV_CMD_DO_TRIGGER_CONTROL:
case NAV_CMD_DO_DIGICAM_CONTROL:
case NAV_CMD_IMAGE_START_CAPTURE:
case NAV_CMD_IMAGE_STOP_CAPTURE:
case NAV_CMD_VIDEO_START_CAPTURE:
case NAV_CMD_VIDEO_STOP_CAPTURE:
case NAV_CMD_DO_MOUNT_CONFIGURE:
case NAV_CMD_DO_MOUNT_CONTROL:
case NAV_CMD_DO_SET_ROI:
case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case NAV_CMD_SET_CAMERA_MODE:
return true;
case NAV_CMD_DO_VTOL_TRANSITION:
/*
* We wait half a second to give the transition command time to propagate.
* Then monitor the transition status for completion.
*/
// TODO: check desired transition state achieved and drop _action_start
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
_action_start = 0;
return true;
} else {
return false;
}
case NAV_CMD_DO_CHANGE_SPEED:
return true;
default:
/* do nothing, this is a 3D waypoint */
break;
}
hrt_abstime now = hrt_absolute_time();
if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
/* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */
if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) {
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
/* close to waypoint, but altitude error greater than twice acceptance */
if ((dist >= 0.0f)
&& (dist_z > 2 * _navigator->get_altitude_acceptance_radius())
&& (dist_xy < 2 * _navigator->get_loiter_radius())) {
/* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
curr_sp->loiter_radius = _navigator->get_loiter_radius();
curr_sp->loiter_direction = 1;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
/* restore SETPOINT_TYPE_POSITION */
if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
/* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */
if ((dist >= 0.0f)
&& (dist_z < _navigator->get_loiter_radius())
&& (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) {
curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
_navigator->set_position_setpoint_triplet_updated();
}
}
}
}
if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
&& _navigator->get_vstatus()->is_rotary_wing) {
/* We want to avoid the edge case where the acceptance radius is bigger or equal than
* the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
* take-off procedures like leaving the landing gear down. */
float takeoff_alt = _mission_item.altitude_is_relative ?
_mission_item.altitude :
(_mission_item.altitude - _navigator->get_home_position()->alt);
float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius();
/* It should be safe to just use half of the takoeff_alt as an acceptance radius. */
if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) {
altitude_acceptance_radius = takeoff_alt / 2.0f;
}
/* require only altitude for takeoff for multicopter */
if (_navigator->get_global_position()->alt >
altitude_amsl - altitude_acceptance_radius) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
/* Loiter mission item on a non rotary wing: the aircraft is going to circle the
* coordinates with a radius equal to the loiter_radius field. It is not flying
* through the waypoint center.
* Therefore the item is marked as reached once the system reaches the loiter
* radius (+ some margin). Time inside and turn count is handled elsewhere.
*/
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
} else {
_time_first_inside_orbit = 0;
}
} else if (!_navigator->get_vstatus()->is_rotary_wing &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter
// first check if the altitude setpoint is the mission setpoint
struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current;
if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) {
// check if the initial loiter has been accepted
dist_xy = -1.0f;
dist_z = -1.0f;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
// now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item
curr_sp->alt = altitude_amsl;
_navigator->set_position_setpoint_triplet_updated();
}
} else {
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f)
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
// set required yaw from bearing to the next mission item
if (_mission_item.force_heading) {
struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next;
if (next_sp.valid) {
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
_waypoint_yaw_reached = false;
} else {
_waypoint_yaw_reached = true;
}
}
}
}
} else if (_mission_item.nav_cmd == NAV_CMD_DELAY) {
_waypoint_position_reached = true;
_waypoint_yaw_reached = true;
_time_wp_reached = now;
} else {
/* for normal mission items used their acceptance radius */
float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);
/* if set to zero use the default instead */
if (mission_acceptance_radius < NAV_EPSILON_POSITION) {
mission_acceptance_radius = _navigator->get_acceptance_radius();
}
/* for vtol back transition calculate acceptance radius based on time and ground speed */
if (_mission_item.vtol_back_transition) {
float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx +
_navigator->get_local_position()->vy * _navigator->get_local_position()->vy);
if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) {
mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() *
velocity;
}
}
if (dist >= 0.0f && dist <= mission_acceptance_radius
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
}
if (_waypoint_position_reached) {
// reached just now
_time_wp_reached = now;
}
}
/* Check if the waypoint and the requested yaw setpoint. */
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
if ((_navigator->get_vstatus()->is_rotary_wing
|| (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading))
&& PX4_ISFINITE(_mission_item.yaw)) {
/* check course if defined only for rotary wing except takeoff */
float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f(
_navigator->get_global_position()->vel_e,
_navigator->get_global_position()->vel_n);
float yaw_err = _wrap_pi(_mission_item.yaw - cog);
/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
_waypoint_yaw_reached = true;
}
/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
if (!_waypoint_yaw_reached && _mission_item.force_heading &&
(_param_yaw_timeout.get() >= FLT_EPSILON) &&
(now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
_navigator->set_mission_failure("unable to reach heading within timeout");
}
} else {
_waypoint_yaw_reached = true;
}
}
/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
// exit xtrack location
// reset lat/lon of loiter waypoint so vehicle follows a tangent
if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) &&
(_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range);
// Compute "ideal" tangent origin
if (curr_sp.loiter_direction > 0) {
bearing -= inner_angle;
} else {
bearing += inner_angle;
}
// Replace current setpoint lat/lon with tangent coordinate
waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
bearing, curr_sp.loiter_radius,
&curr_sp.lat, &curr_sp.lon);
}
return true;
}
}
// all acceptance criteria must be met in the same iteration
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
return false;
}
bool
Mission::check_dist_1wp()
{
if (_dist_1wp_ok) {
/* always return true after at least one successful check */
return true;
}
/* check if first waypoint is not too far from home */
if (_param_dist_1wp.get() > 0.0f) {
if (_navigator->get_vstatus()->condition_home_position_valid) {
struct mission_item_s mission_item;
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < _offboard_mission.count; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* check only items with valid lat/lon */
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
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 ||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
return false;
}
}
} else {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
_dist_1wp_ok = true;
return true;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
return false;
}
} else {
return true;
}
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
{
if (_dist_1wp_ok) {
/* always return true after at least one successful check */
return true;
}
/* check if first waypoint is not too far from home */
if (dist_first_wp > 0.0f) {
struct mission_item_s mission_item;
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < nMissionItems; i++) {
if (dm_read(dm_current, i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* Check non navigation item */
if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){
/* check actuator number */
if (mission_item.actuator_num < 0 || mission_item.actuator_num > 5) {
mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.actuator_num);
warning_issued = true;
return false;
}
/* check actuator value */
if (mission_item.actuator_value < -2000 || mission_item.actuator_value > 2000) {
mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.actuator_value);
warning_issued = true;
return false;
}
}
/* check only items with valid lat/lon */
else if (isPositionCommand(mission_item.nav_cmd)) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon, curr_lat, curr_lon);
if (dist_to_1wp < dist_first_wp) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
warning_issued = true;
return false;
}
}
} else {
/* error reading, mission is invalid */
mavlink_log_info(_mavlink_fd, "error reading offboard mission");
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
_dist_1wp_ok = true;
return true;
} else {
return true;
}
}
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
struct mission_item_s missionitem_previous;
if (i != 0) {
if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
if (wp_distance > _nav_caps.landing_flare_length) {
/* Last wp is before flare region */
if (delta_altitude < 0) {
if (missionitem_previous.altitude <= slope_alt_req) {
/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region");
return false;
}
} else {
mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint");
return false;
}
}
}
/* No landing waypoints or no waypoints */
return true;
}
void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
{
static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
static float lat_next;
static float lon_next;
static float pitch_current;
static float thrust_total;
if (initialized == false) {
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
global_data_unlock(&global_data_parameter_storage->access_conf);
}
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
//TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// if(counter % 5 == 0)
// {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout);
// }
pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction);
}
att_sp.roll = roll_desired;
att_sp.pitch = pitch_desired;
counter++;
}
bool
MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
bool landing_valid = false;
bool land_start_found = false;
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
// if DO_LAND_START found then require valid landing AFTER
if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
if (land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.");
return false;
} else {
land_start_found = true;
do_land_start_index = i;
}
}
if (missionitem.nav_cmd == NAV_CMD_LAND) {
mission_item_s missionitem_previous {};
if (i > 0) {
landing_approach_index = i - 1;
if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
if (MissionBlock::item_contains_position(missionitem_previous)) {
uORB::Subscription<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
landing_status.forcedUpdate();
const bool landing_status_valid = (landing_status.get().timestamp > 0);
const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
missionitem.lat, missionitem.lon);
if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
/* Last wp is before flare region */
const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
if (delta_altitude < 0) {
const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
const float slope_angle_rad = landing_status.get().slope_angle_rad;
const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
horizontal_slope_displacement, slope_angle_rad);
if (missionitem_previous.altitude > slope_alt_req + 1.0f) {
/* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.");
const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude,
missionitem.altitude, horizontal_slope_displacement, slope_angle_rad);
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.",
(int)ceilf(slope_alt_req - missionitem_previous.altitude),
(int)ceilf(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.");
return false;
}
} else {
/* Last wp is in flare region */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.");
return false;
}
landing_valid = true;
} else {
// mission item before land doesn't have a position
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.");
return false;
}
} else {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.");
return false;
}
}
}
if (land_start_req && !land_start_found) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.");
return false;
}
if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.");
return false;
}
/* No landing waypoints or no waypoints */
return true;
}
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();
}
void
Mission::heading_sp_update()
{
/* we don't want to be yawing during takeoff, landing or aligning for a transition */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
|| _mission_item.nav_cmd == NAV_CMD_LAND
|| _mission_item.nav_cmd == NAV_CMD_VTOL_LAND
|| _work_item_type == WORK_ITEM_TYPE_ALIGN) {
return;
}
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) {
return;
}
/* set yaw angle for the waypoint if a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
// XXX: should actually be param4 from mission item
// at the moment it will just keep the heading it has
//_mission_item.yaw = _on_arrival_yaw;
//pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
/* Calculate direction the vehicle should point to. */
double point_from_latlon[2];
double point_to_latlon[2];
point_from_latlon[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon;
/* target location is home */
if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
|| _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
// need to be rotary wing for this but not in a transition
// in VTOL mode this will prevent updating yaw during FW flight
// (which would result in a wrong yaw setpoint spike during back transition)
&& _navigator->get_vstatus()->is_rotary_wing
&& !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) {
point_to_latlon[0] = _navigator->get_home_position()->lat;
point_to_latlon[1] = _navigator->get_home_position()->lon;
/* target location is next (current) waypoint */
} else {
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.lon;
}
float d_current = get_distance_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_latlon[1]);
/* stop if positions are close together to prevent excessive yawing */
if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0],
point_from_latlon[1],
point_to_latlon[0],
point_to_latlon[1]);
/* always keep the back of the rotary wing pointing towards home */
if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = _wrap_pi(yaw + M_PI_F);
pos_sp_triplet->current.yaw = _mission_item.yaw;
} else {
_mission_item.yaw = yaw;
pos_sp_triplet->current.yaw = _mission_item.yaw;
}
}
}
// we set yaw directly so we can run this in parallel to the FOH update
_navigator->set_position_setpoint_triplet_updated();
}
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
build_gps_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
struct gps_module_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_used;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
msg.gps_fix = (uint8_t)(gps.fix_type + 48);
/* No point collecting more data if we don't have a 3D fix yet */
if (gps.fix_type > 2) {
/* Current flight direction */
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
/* GPS speed */
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat)) * 1e-7d;
/* Set the N or S specifier */
msg.latitude_ns = 0;
if (lat < 0) {
msg.latitude_ns = 1;
lat = abs(lat);
}
int deg;
int min;
int sec;
convert_to_degrees_minutes_seconds(lat, °, &min, &sec);
uint16_t lat_min = (uint16_t)(deg * 100 + min);
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
uint16_t lat_sec = (uint16_t)(sec);
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon)) * 1e-7d;
/* Set the E or W specifier */
msg.longitude_ew = 0;
if (lon < 0) {
msg.longitude_ew = 1;
lon = abs(lon);
}
convert_to_degrees_minutes_seconds(lon, °, &min, &sec);
uint16_t lon_min = (uint16_t)(deg * 100 + min);
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
uint16_t lon_sec = (uint16_t)(sec);
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) _home_sub position report */
bool updated;
orb_check(_home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), _home_sub, &home);
_home_lat = home.lat;
_home_lon = home.lon;
_home_position_set = true;
}
/* Distance from home */
if (_home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
msg.distance_L = (uint8_t)dist & 0xff;
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
/* Direction back to home */
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
msg.home_direction = (uint8_t)bearing >> 1;
}
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
bool setpoint = true;
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
// XXX re-visit
float baro_altitude = _global_pos.alt;
/* filter speed and altitude for controller */
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (_control_mode.flag_control_position_enabled) {
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* current waypoint (the one currently heading for) */
math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
/* previous waypoint */
math::Vector<2> prev_wp;
if (pos_sp_triplet.previous.valid) {
prev_wp(0) = (float)pos_sp_triplet.previous.lat;
prev_wp(1) = (float)pos_sp_triplet.previous.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = (float)pos_sp_triplet.current.lat;
prev_wp(1) = (float)pos_sp_triplet.current.lon;
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
/* heading hold, along the line connecting this and the last waypoint */
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (pos_sp_triplet.previous.valid) {
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
land_noreturn_horizontal = true;
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* Vertical landing control */
//xxx: using the tecs altitude controller for slope control for now
// /* do not go down too early */
// if (wp_distance > 50.0f) {
// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
// _tecs.set_speed_weight(2.0f);
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
}
}
float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
/* avoid climbout */
if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
{
flare_curve_alt_rel = 0.0f; // stay on ground
land_stayonground = true;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_pitch_angle_rad,
0.0f, throttle_max, throttle_land,
flare_pitch_angle_rad, math::radians(15.0f));
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
land_noreturn_vertical = true;
}
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
flare_curve_alt_rel_last = flare_curve_alt_rel;
} else {
/* intersect glide slope:
* minimize speed to approach speed
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* */
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
land_onslope = true;
}
} else {
/* continue horizontally */
altitude_desired_rel = math::max(relative_alt, L_altitude_rel);
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
// warnx("Launch detection running");
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
}
} else {
/* no takeoff detection --> fly */
launch_detected = true;
warnx("launchdetection off");
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (launch_detected) {
usePreTakeoffThrust = false;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
}
} else {
usePreTakeoffThrust = true;
}
}
// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
// (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
// (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
/* reset landing state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (0/* posctrl mode enabled */) {
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
// if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* altctrl mode enabled */) {
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* user switched off throttle */
if (_manual.z < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.x > 0.3f && _manual.z > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else {
/** MANUAL FLIGHT **/
/* no flight mode applies, do not publish an attitude setpoint */
setpoint = false;
/* reset landing and takeoff state */
if (!last_manual) {
reset_landing_state();
reset_takeoff_state();
}
}
if (usePreTakeoffThrust) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
} else {
last_manual = true;
}
return setpoint;
}
