AdsbConflict.cpp

/****************************************************************************
 *
 *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/




#include "AdsbConflict.h"
#include "geo/geo.h"

#include <uORB/topics/transponder_report.h>

#include <float.h>


void AdsbConflict::detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now,
		float vz_now)

{

	float d_hor, d_vert;
	get_distance_to_point_global_wgs84(lat_now, lon_now, alt_now,
					   _transponder_report.lat, _transponder_report.lon, _transponder_report.altitude, &d_hor, &d_vert);

	const float xyz_traffic_speed = sqrtf(_transponder_report.hor_velocity * _transponder_report.hor_velocity +
					      _transponder_report.ver_velocity * _transponder_report.ver_velocity);

	const float hor_uav_speed = sqrtf(vx_now * vx_now + vy_now * vy_now);
	const float xyz_uav_speed = sqrtf(hor_uav_speed * hor_uav_speed + vz_now * vz_now);

	//assume always pointing at each other
	const float relative_uav_traffic_speed = xyz_traffic_speed + xyz_uav_speed;


	// Predict until the vehicle would have passed this system at its current speed
	const float prediction_distance = d_hor + TRAFFIC_TO_UAV_DISTANCE_EXTENSION;

	double end_lat, end_lon;
	waypoint_from_heading_and_distance(_transponder_report.lat, _transponder_report.lon,
					   _transponder_report.heading, prediction_distance, &end_lat, &end_lon);


	const bool cs_distance_conflict_threshold = (!get_distance_to_line(_crosstrack_error, lat_now,
			lon_now, _transponder_report.lat,
			_transponder_report.lon, end_lat,
			end_lon))
			&& (!_crosstrack_error.past_end)
			&& (fabsf(_crosstrack_error.distance) < _conflict_detection_params.crosstrack_separation);

	const bool _crosstrack_separation_check = (fabsf(alt_now - _transponder_report.altitude) <
			_conflict_detection_params.crosstrack_separation);

	bool collision_time_check = false;

	const float d_xyz = sqrtf(d_hor * d_hor + d_vert * d_vert);

	if (relative_uav_traffic_speed > FLT_EPSILON) {
		const float time_to_collsion =  d_xyz / relative_uav_traffic_speed;
		collision_time_check = (time_to_collsion < _conflict_detection_params.collision_time_threshold);
	}

	_conflict_detected = (cs_distance_conflict_threshold && _crosstrack_separation_check
			      && collision_time_check);
}

int AdsbConflict::find_icao_address_in_conflict_list(uint32_t icao_address)
{

	for (uint8_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
		if (_traffic_buffer.icao_address[i] == icao_address) {
			return i;
		}
	}

	return -1;
}

void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
{
	_traffic_buffer.icao_address.remove(traffic_index);
	_traffic_buffer.timestamp.remove(traffic_index);
}

void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address)
{
	_traffic_buffer.timestamp.push_back(hrt_absolute_time());
	_traffic_buffer.icao_address.push_back(icao_address);
}

void AdsbConflict::get_traffic_state()
{

	const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address);

	const bool old_conflict = (traffic_index >= 0);
	const bool new_traffic = (traffic_index < 0);
	const bool _traffic_buffer_full = (_traffic_buffer.icao_address.size() >= NAVIGATOR_MAX_TRAFFIC);

	bool old_conflict_warning_expired = false;

	if (old_conflict && _conflict_detected) {
		old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);

	}

	if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
		add_icao_address_from_conflict_list(_transponder_report.icao_address);
		_traffic_state = TRAFFIC_STATE::ADD_CONFLICT;

	} else if (new_traffic && _conflict_detected && _traffic_buffer_full) {
		_traffic_state = TRAFFIC_STATE::BUFFER_FULL;

	} else if (old_conflict && _conflict_detected
		   && old_conflict_warning_expired) {
		_traffic_buffer.timestamp[traffic_index] = hrt_absolute_time();
		_traffic_state = TRAFFIC_STATE::REMIND_CONFLICT;

	} else if (old_conflict && !_conflict_detected) {
		remove_icao_address_from_conflict_list(traffic_index);
		_traffic_state = TRAFFIC_STATE::REMOVE_OLD_CONFLICT;

	} else {
		_traffic_state = TRAFFIC_STATE::NO_CONFLICT;
	}

}


bool AdsbConflict::handle_traffic_conflict()
{

	get_traffic_state();

	char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages

	//convert UAS_id byte array to char array for User Warning
	for (int i = 0; i < 5; i++) {
		snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
	}

	uint64_t uas_id_int = 0;

	for (int i = 0; i < 8; i++) {
		uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
	}


	bool take_action = false;

	switch (_traffic_state) {

	case TRAFFIC_STATE::ADD_CONFLICT:
	case TRAFFIC_STATE::REMIND_CONFLICT: {

			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
							   _transponder_report.callsign,
							   uas_id_int);

		}
		break;

	case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {

			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!",
					     _transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ?
					     _transponder_report.callsign : uas_id);

			events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
					       uas_id_int);

		}
		break;

	case TRAFFIC_STATE::BUFFER_FULL: {

			if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) {
				PX4_WARN("Too much traffic! Showing all messages from now on");
			}

			//stop buffering incoming conflicts
			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
							   _transponder_report.callsign,
							   uas_id_int);

		}
		break;

	case TRAFFIC_STATE::NO_CONFLICT: {

		}
		break;
	}


	_traffic_state_previous = _traffic_state;

	return take_action;

}

void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
		int collision_time_threshold, uint8_t traffic_avoidance_mode)
{

	_conflict_detection_params.crosstrack_separation = crosstrack_separation;
	_conflict_detection_params.vertical_separation = vertical_separation;
	_conflict_detection_params.collision_time_threshold = collision_time_threshold;
	_conflict_detection_params.traffic_avoidance_mode = traffic_avoidance_mode;

}


bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
					char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int)
{

	switch (_conflict_detection_params.traffic_avoidance_mode) {

	case 0: {
			PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
				 tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
				 traffic_seperation,
				 traffic_direction);
			break;
		}

	case 1: {
			mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t",
					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
					     traffic_seperation,
					     traffic_direction);
			/* EVENT
			 * @description
			 * - ID: {1}
			 * - Distance: {2m}
			 * - Direction: {3} degrees
			 */
			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
					uas_id_int, traffic_seperation, traffic_direction);
			break;
		}

	case 2: {
			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t",
					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
					     traffic_seperation,
					     traffic_direction);
			/* EVENT
			 * @description
			 * - ID: {1}
			 * - Distance: {2m}
			 * - Direction: {3} degrees
			 */
			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
					"Traffic alert, returning home",
					uas_id_int, traffic_seperation, traffic_direction);

			return true;

			break;
		}

	case 3: {
			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t",
					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
					     traffic_seperation,
					     traffic_direction);
			/* EVENT
			 * @description
			 * - ID: {1}
			 * - Distance: {2m}
			 * - Direction: {3} degrees
			 */
			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
					"Traffic alert, landing",
					uas_id_int, traffic_seperation, traffic_direction);

			return true;

			break;

		}

	case 4: {
			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t",
					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
					     traffic_seperation,
					     traffic_direction);
			/* EVENT
			 * @description
			 * - ID: {1}
			 * - Distance: {2m}
			 * - Direction: {3} degrees
			 */
			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
					"Traffic alert, holding position",
					uas_id_int, traffic_seperation, traffic_direction);

			return true;

			break;

		}
	}


	return false;

}

void AdsbConflict::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
				float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav,
				double lon_uav,
				float &alt_uav)
{
	double lat{0.0};
	double lon{0.0};

	waypoint_from_heading_and_distance(lat_uav, lon_uav, direction, distance, &lat,
					   &lon);
	float alt = alt_uav + altitude_diff;

	tr.timestamp = hrt_absolute_time();
	tr.icao_address = icao_address;
	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) - 1);
	tr.callsign[sizeof(tr.callsign) - 1] = 0;
	tr.emitter_type = emitter_type; // 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::ADSB_EMITTER_TYPE_UAV & emitter_type ? 0 :
		    transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields
	tr.squawk = 6667;

#ifndef BOARD_HAS_NO_UUID
	px4_guid_t px4_guid;
	board_get_px4_guid(px4_guid);
	memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t)); //simulate own GUID
#else

	for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
		tr.uas_id[i] = 0xe0 + i; //simulate GUID
	}

#endif /* BOARD_HAS_NO_UUID */

	orb_publish(ORB_ID(transponder_report), fake_traffic_report_publisher, &tr);

}


void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
				    float &alt_uav)
{

	/*
	//first conflict
	fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
		     alt_uav);

	//spam
	fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
		     alt_uav);
	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
		     alt_uav);
	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
		     alt_uav);

	//stop spamming

	fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX005", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 5, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX006", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 6, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX007", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 7, lat_uav, lon_uav,
		     alt_uav);


	fake_traffic("LX008", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 8, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX009", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 9, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX010", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 10, lat_uav, lon_uav,
		     alt_uav);

	//buffer full
	fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX012", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 12, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
		     alt_uav);


	//end conflict
	fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
		     alt_uav);
	fake_traffic("LX002", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX003", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX004", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
		     alt_uav);


	//new conflicts with space in buffer
	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
		     alt_uav);

	fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
		     alt_uav);

	for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
		PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
	}
	*/
}