From 52275923ad738f98f46ab81a90b4367e97d8db7c Mon Sep 17 00:00:00 2001
From: Igor Misic <igy1000mb@gmail.com>
Date: Mon, 19 Dec 2022 12:16:26 +0100
Subject: [PATCH] adsb: add support for callsign

---
 src/lib/adsb/parameters.c                     | 23 +++++++++
 .../mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp | 48 ++++++++++++++++++-
 2 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/src/lib/adsb/parameters.c b/src/lib/adsb/parameters.c
index 6bf8865cc5..6c2174d2bf 100644
--- a/src/lib/adsb/parameters.c
+++ b/src/lib/adsb/parameters.c
@@ -226,3 +226,26 @@ PARAM_DEFINE_INT32(ADSB_GPS_OFF_LAT, 0);
  * @group ADSB
  */
 PARAM_DEFINE_INT32(ADSB_GPS_OFF_LON, 0);
+
+/**
+ * First 4 characters of CALLSIGN
+ *
+ * Sets first 4 characters of a total of 8. Valid characters are A-Z, 0-9, " ". Example "PX4 " -> 1347957792
+ * For CALLSIGN shorter than 8 characters use the null terminator at the end '\0'.
+ *
+ * @reboot_required true
+ * @group ADSB
+ */
+
+PARAM_DEFINE_INT32(ADSB_CALLSIGN_1, 0);
+
+/**
+* Second 4 characters of CALLSIGN
+*
+* Sets second 4 characters of a total of 8. Valid characters are A-Z, 0-9, " " only. Example "TEST" -> 1413829460
+* For CALLSIGN shorter than 8 characters use the null terminator at the end '\0'.
+*
+* @reboot_required true
+* @group ADSB
+*/
+PARAM_DEFINE_INT32(ADSB_CALLSIGN_2, 0);
diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
index a8767b76d2..722b390ece 100644
--- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
+++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
@@ -66,6 +66,27 @@ private:
 				_stall_speed = static_cast<uint16_t>(fw_airspd_stall * 100.0f); // convert [m/s] to [cm/s]
 			}
 		}
+
+		int32_t callsign_part1 = _adsb_callsign_part1.get();
+		int32_t callsign_part2 = _adsb_callsign_part2.get();
+
+		bool success = false;
+
+		if (copy_and_check_callsign(_callsign, callsign_part1)) {
+			if (copy_and_check_callsign(_callsign + sizeof(int32_t), callsign_part2)) {
+				success = true;
+			}
+		}
+
+		if (success) {
+			_callsign[sizeof(_callsign) - 1] = '\0';
+
+		} else {
+			_callsign[0] = '\0';
+			events::send(events::ID("uavionix_adsb_out_cfg_unsupported_callsign_format"),
+				     events::Log::Critical, "Unsupported callsign format");
+		}
+
 	}
 
 	DEFINE_PARAMETERS(
@@ -73,10 +94,32 @@ private:
 		(ParamInt<px4::params::ADSB_LEN_WIDTH>)		_adsb_len_width,
 		(ParamInt<px4::params::ADSB_EMIT_TYPE>)		_adsb_emit_type,
 		(ParamInt<px4::params::ADSB_GPS_OFF_LAT>)	_adsb_gps_offset_lat,
-		(ParamInt<px4::params::ADSB_GPS_OFF_LON>)	_adsb_gps_offset_lon
+		(ParamInt<px4::params::ADSB_GPS_OFF_LON>)	_adsb_gps_offset_lon,
+		(ParamInt<px4::params::ADSB_CALLSIGN_1>)	_adsb_callsign_part1,
+		(ParamInt<px4::params::ADSB_CALLSIGN_2>)	_adsb_callsign_part2
 	);
 
 	uint16_t _stall_speed{0}; // [cm/s]
+	char _callsign[9];
+
+	bool copy_and_check_callsign(char *ptr_callsign, int32_t part)
+	{
+		for (uint8_t i = 0; i < sizeof(int32_t); ++i) {
+
+			char character = static_cast<char>(part >>(i * 8));
+
+			if ((character >= 'A' && character <= 'Z') || (character >= '0' && character <= '9') || character == ' '
+			    || character == '\0') {
+
+				ptr_callsign[sizeof(int32_t) - i - 1] = character;
+
+			} else {
+				return false;
+			}
+		}
+
+		return true;
+	}
 
 	bool send() override
 	{
@@ -92,7 +135,8 @@ private:
 			.rfSelect = UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED
 		};
 
-		//memcpy(cfg_msg.callsign, "PX4 UAV ", sizeof(cfg_msg.callsign)); //TODO: add support for callsign
+		static_assert(sizeof(cfg_msg.callsign) == sizeof(_callsign), "Size mismatch");
+		memcpy(cfg_msg.callsign, _callsign, sizeof(cfg_msg.callsign));
 
 		mavlink_msg_uavionix_adsb_out_cfg_send_struct(_mavlink->get_channel(), &cfg_msg);