/** * Creates a fake traffic measurement with supplied parameters. * */ void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff, float hor_velocity, float ver_velocity) { double lat, lon; waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat, &lon); float alt = get_global_position()->alt + altitude_diff; // float vel_n = get_global_position()->vel_n; // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; transponder_report_s tr = {}; tr.timestamp = hrt_absolute_time(); tr.ICAO_address = 1234; tr.lat = lat; // Latitude, expressed as degrees tr.lon = lon; // Longitude, expressed as degrees tr.altitude_type = 0; tr.altitude = alt; tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign)); tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum tr.tslc = 2; // Time since last communication in seconds tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields tr.squawk = 6667; orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(h); }
__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) { if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; } else if (dist >= FLT_EPSILON) { float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } else { float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); } }
void MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) { /* set the correct setpoint for vtol transition */ if (item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && PX4_ISFINITE(item->yaw) && item->params[0] >= vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW - 0.5f) { sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; waypoint_from_heading_and_distance(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, item->yaw, 1000000.0f, &sp->lat, &sp->lon); sp->alt = _navigator->get_global_position()->alt; } /* don't change the setpoint for non-position items */ if (!item_contains_position(item)) { return; } sp->valid = true; sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; sp->acceptance_radius = item->acceptance_radius; sp->disable_mc_yaw_control = false; sp->cruising_speed = _navigator->get_cruising_speed(); sp->cruising_throttle = _navigator->get_cruising_throttle(); switch (item->nav_cmd) { case NAV_CMD_IDLE: sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: case NAV_CMD_VTOL_TAKEOFF: sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; break; case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_land.get()) { sp->disable_mc_yaw_control = true; } break; case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; if (_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()) { sp->disable_mc_yaw_control = true; } break; default: sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } }
void Navigator::check_traffic() { double lat = get_global_position()->lat; double lon = get_global_position()->lon; float alt = get_global_position()->alt; // TODO for non-multirotors predicting the future // position as accurately as possible will become relevant // float vel_n = get_global_position()->vel_n; // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; bool changed; orb_check(_traffic_sub, &changed); float horizontal_separation = 500; float vertical_separation = 500; while (changed) { transponder_report_s tr; orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr); uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; if ((tr.flags & required_flags) != required_flags) { orb_check(_traffic_sub, &changed); continue; } float d_hor, d_vert; get_distance_to_point_global_wgs84(lat, lon, alt, tr.lat, tr.lon, tr.altitude, &d_hor, &d_vert); // predict final altitude (positive is up) in prediction time frame float end_alt = tr.altitude + (d_vert / tr.hor_velocity) * tr.ver_velocity; // Predict until the vehicle would have passed this system at its current speed float prediction_distance = d_hor + 1000.0f; // If the altitude is not getting close to us, do not calculate // the horizontal separation. // Since commercial flights do most of the time keep flight levels // check for the current and for the predicted flight level. // we also make the implicit assumption that this system is on the lowest // flight level close to ground in the // (end_alt - horizontal_separation < alt) condition. If this system should // ever be used in normal airspace this implementation would anyway be // inappropriate as it should be replaced with a TCAS compliant solution. if ((fabsf(alt - tr.altitude) < vertical_separation) || ((end_alt - horizontal_separation) < alt)) { double end_lat, end_lon; waypoint_from_heading_and_distance(tr.lat, tr.lon, tr.heading, prediction_distance, &end_lat, &end_lon); struct crosstrack_error_s cr; if (!get_distance_to_line(&cr, lat, lon, tr.lat, tr.lon, end_lat, end_lon)) { if (!cr.past_end && (fabsf(cr.distance) < horizontal_separation)) { // direction of traffic in human-readable 0..360 degree in earth frame int traffic_direction = math::degrees(tr.heading) + 180; switch (_param_traffic_avoidance_mode.get()) { case 0: { /* ignore */ PX4_WARN("TRAFFIC %s, hdg: %d", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); break; } case 1: { mavlink_log_critical(&_mavlink_log_pub, "WARNING TRAFFIC %s at heading %d, land immediately", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); break; } case 2: { mavlink_log_critical(&_mavlink_log_pub, "AVOIDING TRAFFIC %s heading %d, returning home", tr.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr.callsign : "unknown", traffic_direction); // set the return altitude to minimum _rtl.set_return_alt_min(true); // ask the commander to execute an RTL vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; publish_vehicle_cmd(&vcmd); break; } } } } } orb_check(_traffic_sub, &changed); } }
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; }