GHST: Add support for GPS Telemetry (#19953)

Add support for the basic GPS telemetry values when using the GHST protocol.

* Fix formatting in GHST GPS telemetry changes

* GHST GPS Telemetry formatting cleanup

* GHST GPS Telemetry, Last formatting change

src/drivers/rc_input/ghst_telemetry.cpp

@@ -61,6 +61,14 @@ bool GHSTTelemetry::update(const hrt_abstime &now)
 			success = send_battery_status();
 			break;
 
+		case 1U:
+			success = send_gps1_status();
+			break;
+
+		case 2U:
+			success = send_gps2_status();
+			break;
+
 		default:
 			success = false;
 			break;
@@ -93,3 +101,39 @@ bool GHSTTelemetry::send_battery_status()
 
 	return success;
 }
+
+bool GHSTTelemetry::send_gps1_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		return false;
+	}
+
+	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
+	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
+	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
+
+	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
+}
+
+bool GHSTTelemetry::send_gps2_status()
+{
+	sensor_gps_s vehicle_gps_position;
+
+	if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
+		return false;
+	}
+
+	uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
+	uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
+	uint8_t num_sats = vehicle_gps_position.satellites_used;
+
+	// TBD: Can these be computed in a RC telemetry driver?
+	uint16_t home_dist = 0;
+	uint16_t home_dir = 0;
+	uint8_t flags = 0;
+
+	return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
+}
+

src/drivers/rc_input/ghst_telemetry.hpp

@@ -44,6 +44,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_gps.h>
 #include <drivers/drv_hrt.h>
 
 /**
@@ -69,14 +70,17 @@ public:
 
 private:
 	bool send_battery_status();
+	bool send_gps1_status();
+	bool send_gps2_status();
 
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
 	int _uart_fd;
 	hrt_abstime _last_update {0U};
 	uint32_t _next_type {0U};
 
-	static constexpr uint32_t NUM_DATA_TYPES {1U};	// number of different telemetry data types
+	static constexpr uint32_t NUM_DATA_TYPES {3U};	// number of different telemetry data types
 	static constexpr uint32_t UPDATE_RATE_HZ {10U};	// update rate [Hz]
 
 	// Factors that should be applied to get correct values

src/lib/rc/ghst.cpp

@@ -350,6 +350,19 @@ static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
 	offset += 2;
 }
 
+/**
+ * write an uint32_t value to a buffer at a given offset and increment the offset
+ */
+static inline void write_uint32_t(uint8_t *buf, int &offset, uint32_t value)
+{
+	// Little Endian
+	buf[offset] = value & 0xFFU;
+	buf[offset + 1] = (value & 0xFF00) >> 8U;
+	buf[offset + 2] = (value & 0xFF0000) >> 16U;
+	buf[offset + 3] = (value & 0xFF000000) >> 24U;
+	offset += 4;
+}
+
 /**
  * write frame header
  */
@@ -385,3 +398,34 @@ bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
 
 	return write(uart_fd, buf, offset) == offset;
 }
+
+bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsPrimary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint32_t(buf, offset, latitude);
+	write_uint32_t(buf, offset, longitude);
+	write_uint16_t(buf, offset, altitude);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+
+bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course, uint8_t numSats,
+				     uint16_t home_dist, uint16_t home_dir, uint8_t flags)
+{
+	uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
+	int offset = 0;
+	write_frame_header(buf, offset, ghstTelemetryType::gpsSecondary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
+	write_uint16_t(buf, offset, ground_speed);
+	write_uint16_t(buf, offset, ground_course);
+	write_uint8_t(buf, offset, numSats);
+	write_uint16_t(buf, offset, home_dist);
+	write_uint16_t(buf, offset, home_dir);
+	write_uint8_t(buf, offset, flags);
+	write_frame_crc(buf, offset, sizeof(buf));
+
+	return write(uart_fd, buf, offset) == offset;
+}
+

src/lib/rc/ghst.hpp

@@ -64,7 +64,9 @@ enum class ghstFrameType {
 };
 
 enum class ghstTelemetryType {
-	batteryPack = 0x23	// battery pack status frame type
+	batteryPack = 0x23,	// battery pack status frame type
+	gpsPrimary = 0x25,	// GPS primary data (lat/long/alt)
+	gpsSecondary = 0x26	// GPS secondary data (course, dist, flags)
 };
 
 struct ghst_frame_header_t {
@@ -139,4 +141,27 @@ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len,
 __EXPORT bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
 		uint16_t current_in_10mA, uint16_t fuel_in_10mAh);
 
+/**
+ * Send primary GPS information
+ * @param uart_fd UART file descriptor
+ * @param latitude GPS latitude [1e-7 degrees]
+ * @param longitude GPS longitude [1e-7 degrees]
+ * @param altitude GPS altitude [1m]
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude);
+
+/**
+ * Send secondary GPS information
+ * @param uart_fd UART file descriptor
+ * @param ground_speed Ground Speed [1 km/h]
+ * @param ground_course Ground Course [1e-7 degrees]
+ * @param num_sats GPS Satellite count
+ * @param home_dist Distance to Home [10 m]
+ * @param home_dir Direction to Home [1e-7 degrees]
+ * @param flags GPS Flags
+ * @return true on success
+ */
+__EXPORT bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course,
+		uint8_t num_sats, uint16_t home_dist, uint16_t home_dir, uint8_t flags);
 __END_DECLS