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