void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, position_setpoint_s &missionCmd, position_setpoint_s &lastMissionCmd) { // heading to waypoint float psiTrack = get_bearing_to_next_waypoint( (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, missionCmd.lat, missionCmd.lon); // cross track struct crosstrack_error_s xtrackError; get_distance_to_line(&xtrackError, (double)pos.lat / (double)1e7d, (double)pos.lon / (double)1e7d, lastMissionCmd.lat, lastMissionCmd.lon, missionCmd.lat, missionCmd.lon); _psiCmd = _wrap_2pi(psiTrack - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); }
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); } }