ghst_telemetry: send battery status

Send battery status (ghst_telemetry). Apply factors to show correct values of volts, amps and mAh. Change ghost protocol code to follow more MISRA C++ guidelines.

src/drivers/rc_input/RCInput.cpp

@@ -654,16 +654,14 @@ void RCInput::Run()
 						_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST;
 						fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi);
 
-						// Enable GHST Telemetry only on the Omnibus, because on Pixhawk (-related) boards
+						// ghst telemetry works on fmu-v5
+						// on other Pixhawk (-related) boards it does not work because
 						// we cannot write to the RC UART
-						// It might work on FMU-v5. Or another option is to use a different UART port
-#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD
 
 						if (!_rc_scan_locked && !_ghst_telemetry) {
 							_ghst_telemetry = new GHSTTelemetry(_rcs_fd);
 						}
 
-#endif /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */
 
 						_rc_scan_locked = true;
 

src/drivers/rc_input/RCInput.hpp

@@ -41,7 +41,7 @@
 #include <drivers/drv_rc_input.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/rc/crsf.h>
-#include <lib/rc/ghst.h>
+#include <lib/rc/ghst.hpp>
 #include <lib/rc/dsm.h>
 #include <lib/rc/sbus.h>
 #include <lib/rc/st24.h>
@@ -61,7 +61,7 @@
 #include <uORB/topics/vehicle_command.h>
 
 #include "crsf_telemetry.h"
-#include "ghst_telemetry.h"
+#include "ghst_telemetry.hpp"
 
 #ifdef HRT_PPM_CHANNEL
 # include <systemlib/ppm_decode.h>

src/drivers/rc_input/ghst_telemetry.cpp

@@ -40,8 +40,10 @@
  * @author Juraj Ciberlin <jciberlin1@gmail.com>
  */
 
-#include "ghst_telemetry.h"
-#include <lib/rc/ghst.h>
+#include "ghst_telemetry.hpp"
+#include <lib/rc/ghst.hpp>
+
+using time_literals::operator ""_s;
 
 GHSTTelemetry::GHSTTelemetry(int uart_fd) :
 	_uart_fd(uart_fd)
@@ -50,36 +52,44 @@ GHSTTelemetry::GHSTTelemetry(int uart_fd) :
 
 bool GHSTTelemetry::update(const hrt_abstime &now)
 {
-	const int update_rate_hz = 10;
+	bool success = false;
 
-	if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
-		return false;
+	if ((now - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) {
+
+		switch (_next_type) {
+		case 0U:
+			success = send_battery_status();
+			break;
+
+		default:
+			success = false;
+			break;
+		}
+
+		_last_update = now;
+		_next_type = (_next_type + 1U) % NUM_DATA_TYPES;
 	}
 
-	bool sent = false;
-
-	switch (_next_type) {
-	case 0:
-		sent = send_battery_status();
-		break;
-	}
-
-	_last_update = now;
-	_next_type = (_next_type + 1) % num_data_types;
-
-	return sent;
+	return success;
 }
 
 bool GHSTTelemetry::send_battery_status()
 {
+	bool success = false;
+	float voltage_in_10mV;
+	float current_in_10mA;
+	float fuel_in_10mAh;
 	battery_status_s battery_status;
 
-	if (!_battery_status_sub.update(&battery_status)) {
-		return false;
+	if (_battery_status_sub.update(&battery_status)) {
+		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
+		success = ghst_send_telemetry_battery_status(_uart_fd,
+				static_cast<uint16_t>(voltage_in_10mV),
+				static_cast<uint16_t>(current_in_10mA),
+				static_cast<uint16_t>(fuel_in_10mAh));
 	}
 
-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
-	uint16_t fuel = battery_status.discharged_mah;
-	return ghst_send_telemetry_battery(_uart_fd, voltage, current, fuel);
+	return success;
 }

src/drivers/rc_input/ghst_telemetry.hpp

@@ -46,8 +46,6 @@
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_hrt.h>
 
-using namespace time_literals;
-
 /**
  * High-level class that handles sending of GHST telemetry data
  */
@@ -58,7 +56,7 @@ public:
 	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
 	 * already.
 	 */
-	GHSTTelemetry(int uart_fd);
+	explicit GHSTTelemetry(int uart_fd);
 
 	~GHSTTelemetry() = default;
 
@@ -74,10 +72,16 @@ private:
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 
-	hrt_abstime _last_update{0};
-
-	static constexpr int num_data_types{1}; // number of different telemetry data types
-	int _next_type{0};
-
 	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 UPDATE_RATE_HZ {10U};	// update rate [Hz]
+
+	// Factors that should be applied to get correct values
+	static constexpr float FACTOR_VOLTS_TO_10MV {100.0F};
+	static constexpr float FACTOR_AMPS_TO_10MA {100.0F};
+	static constexpr float FACTOR_MAH_TO_10MAH {0.1F};
+
 };