msp_osd: Add VTX config, stick commands and arming status

 - Adds MSPv1 rx parsing to fetch VTX config
 - Allows to inspect and change VTX channel through CLI
 - Forward MSP_RC for stick commands osd
 - Forward MSP_STATUS for arming status for PIT and LP mode

Author: PetervdPerk-NXP

src/drivers/osd/msp_osd/MspV1.cpp

@@ -39,6 +39,7 @@
 #include <float.h>
 #include <string.h>
 #include <math.h>
+#include <stdio.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
@@ -62,7 +63,7 @@ struct msp_message_descriptor_t {
 	uint8_t message_size;
 };
 
-#define MSP_DESCRIPTOR_COUNT 11
+#define MSP_DESCRIPTOR_COUNT 12
 const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_OSD_CONFIG, true, sizeof(msp_osd_config_t)},
 	{MSP_NAME, true, sizeof(msp_name_t)},
@@ -75,10 +76,9 @@ const msp_message_descriptor_t msp_message_descriptors[MSP_DESCRIPTOR_COUNT] = {
 	{MSP_COMP_GPS, true, sizeof(msp_comp_gps_t)},
 	{MSP_ESC_SENSOR_DATA, true, sizeof(msp_esc_sensor_data_dji_t)},
 	{MSP_MOTOR_TELEMETRY, true, sizeof(msp_motor_telemetry_t)},
+	{MSP_RC, true, sizeof(msp_rc_t)},
 };
 
-#define MSP_FRAME_START_SIZE 5
-#define MSP_CRC_SIZE 1
 bool MspV1::Send(const uint8_t message_id, const void *payload)
 {
 	uint32_t payload_size = 0;
@@ -149,3 +149,76 @@ bool MspV1::Send(const uint8_t message_id, const void *payload, uint32_t payload
 	int packet_size =  MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
 	return  write(_fd, packet, packet_size) == packet_size;
 }
+
+
+int MspV1::Receive(uint8_t *payload, uint8_t *message_id)
+{
+	uint8_t payload_size;
+	uint8_t crc;
+	uint8_t calc_crc;
+	int ret;
+
+	while (!has_header) {
+		int bytes_available = 0;
+
+		if (ioctl(_fd, FIONREAD, &bytes_available) < 0) {
+			return -EIO;
+		}
+
+		if (bytes_available < 5) {
+			return -EWOULDBLOCK;
+		}
+
+		while (bytes_available > 4) {
+			if ((ret = read(_fd, header, 1)) != 1) {
+				return ret;
+			}
+
+			bytes_available--;
+
+			if (header[0] == '$') {
+				break;
+			}
+
+		}
+
+		if (header[0] != '$') {
+			return -EWOULDBLOCK;
+		}
+
+		if ((ret = read(_fd, &header[1], 4)) != 4) {
+			return ret;
+		}
+
+		if (header[0] == '$' && header[1] == 'M' && header[2] == '<') {
+			has_header = true;
+		}
+	}
+
+	payload_size = header[3];
+	*message_id = header[4];
+
+	if ((ret = read(_fd, payload, payload_size + MSP_CRC_SIZE)) != payload_size + MSP_CRC_SIZE) {
+		if (ret != -EWOULDBLOCK) {
+			has_header = false;
+		}
+
+		return ret;
+	}
+
+	has_header = false;
+
+	crc = payload[payload_size];
+
+	calc_crc = payload_size ^ header[4];
+
+	for (int i = 0; i < payload_size; i++) {
+		calc_crc ^= payload[i];
+	}
+
+	if (calc_crc != crc) {
+		return -EINVAL;
+	}
+
+	return payload_size;
+}

src/drivers/osd/msp_osd/MspV1.hpp

@@ -33,14 +33,20 @@
 
 #pragma once
 
+#define MSP_FRAME_START_SIZE 5
+#define MSP_CRC_SIZE 1
+
 class MspV1
 {
 public:
 	MspV1(int fd);
 	int GetMessageSize(int message_type);
 	bool Send(const uint8_t message_id, const void *payload);
 	bool Send(const uint8_t message_id, const void *payload, uint32_t payload_size);
+	int Receive(uint8_t *payload, uint8_t *message_id);
 
 private:
 	int _fd{-1};
+	uint8_t header[MSP_FRAME_START_SIZE + MSP_CRC_SIZE];
+	bool has_header{false};
 };

src/drivers/osd/msp_osd/module.yaml

@@ -97,3 +97,14 @@ parameters:
             min: 100
             max: 10000
             default: 500
+
+        # RC Stick input
+        OSD_RC_STICK:
+            description:
+                short: OSD RC Stick commands
+                long: |
+                    Forward RC stick input to VTX when disarmed
+            type: int32
+            min: 0
+            max: 1
+            default: 1

src/drivers/osd/msp_osd/msp_defines.h

@@ -48,6 +48,8 @@
 #define MSP_ARMING_CONFIG         61
 #define MSP_RX_MAP                64 // get channel map (also returns number of channels total)
 #define MSP_LOOP_TIME             73 // FC cycle time i.e looptime parameter
+#define MSP_GET_VTX_CONFIG        88
+#define MSP_SET_VTX_CONFIG        89
 #define MSP_STATUS               101
 #define MSP_RAW_IMU              102
 #define MSP_SERVO                103
@@ -75,10 +77,12 @@
 #define MSP_SET_PID              202 // set P I D coeff
 
 // commands
-#define MSP_SET_HEAD             211 // define a new heading hold direction
-#define MSP_SET_RAW_RC           200 // 8 rc chan
-#define MSP_SET_RAW_GPS          201 // fix, numsat, lat, lon, alt, speed
-#define MSP_SET_WP               209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_HEAD                211 // define a new heading hold direction
+#define MSP_SET_RAW_RC              200 // 8 rc chan
+#define MSP_SET_RAW_GPS             201 // fix, numsat, lat, lon, alt, speed
+#define MSP_SET_WP                  209 // sets a given WP (WP#, lat, lon, alt, flags)
+#define MSP_SET_VTXTABLE_BAND       227
+#define MSP_SET_VTXTABLE_POWERLEVEL 228
 
 // bits of getActiveModes() return value
 #define MSP_MODE_ARM          0
@@ -895,6 +899,58 @@ struct msp_status_BF_t {
 	uint8_t  extra_flags;
 } __attribute__((packed));
 
+struct msp_set_vtx_config_t {
+	uint16_t new_freq; //  if setting frequency then full uint16 is the frequency in MHz (ie. 5800)
+	//if setting band channel than band is high 8 bits and channel is low 8 bits
+	uint8_t power_level;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint8_t low_power_disarm;
+	uint16_t pit_freq;
+	uint8_t user_band;
+	uint8_t user_channel;
+	uint16_t user_freq; // in MHz, 0 if using band & channel
+	uint8_t band_count;
+	uint8_t channel_count;
+	uint8_t power_count;
+	uint8_t clear_vtxtable; // Bool
+} __attribute__((packed));
+
+struct msp_get_vtx_config_t {
+	uint8_t vtx_type;
+	uint8_t band;
+	uint8_t channel;
+	uint8_t power_index;
+	uint8_t pit_mode; // 0 = off, 1 = on
+	uint16_t freq; // in MHz, 0 if using band & channel
+	uint8_t device_ready;
+	uint8_t low_power_disarm;
+} __attribute__((packed));
+
+struct msp_set_vtxtable_powerlevel_t {
+	uint8_t index;
+	uint16_t power_value;
+	uint8_t power_label_length;
+	uint8_t power_label_name[3];
+} __attribute__((packed));
+
+#define VTX_TABLE_BAND_NAME_LENGTH 8
+#define VTXDEV_MSP 5
+
+//29 bytes
+
+struct msp_set_vtxtable_band_t {
+	uint8_t band;
+	uint8_t band_name_length;
+	uint8_t band_label_name[VTX_TABLE_BAND_NAME_LENGTH];
+	uint8_t band_letter;
+	uint8_t is_factory_band;
+	uint8_t channel_count;
+	uint16_t frequency[8];
+} __attribute__((packed));
+
+
+
+
 ////ArduPlane
 enum arduPlaneModes_e {
 	MANUAL        = 0,