Ascend/fixed wing rebased

boards/modalai/voxl2-slpi/default.px4board

@@ -28,6 +28,12 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_COMMANDER=y

boards/modalai/voxl2/scripts/install-voxl.sh

@@ -20,7 +20,6 @@ adb shell chmod a+x /usr/bin/voxl-px4-hitl-start
 
 # Push configuration file
 adb shell mkdir -p /etc/modalai
-adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai
 
@@ -79,6 +78,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-modalai_gps_timer"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms4525_airspeed"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms5525_airspeed"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-msp_osd"
+adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-msp_dp_osd"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-muorb"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-muorb_test"
 adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-navigator"

boards/modalai/voxl2/src/CMakeLists.txt

@@ -44,4 +44,5 @@ add_library(drivers_board
 
 # Add custom drivers
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/modal_io_bridge)
+add_subdirectory(${PX4_BOARD_DIR}/src/drivers/msp_dp_osd)
 add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus)

boards/modalai/voxl2/src/drivers/msp_dp_osd/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2022 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__msp_dp_osd
+	MAIN msp_dp_osd
+	INCLUDES
+		${PX4_SOURCE_DIR}/src/drivers/osd/msp_osd/MessageDisplay
+		${PX4_SOURCE_DIR}/src/drivers/osd/msp_osd/osd/msp_defines.h
+		${PX4_SOURCE_DIR}/src/drivers/osd/msp_osd/osd/uorb_to_msp.hpp
+	SRCS
+		msp_dp_osd.cpp
+		MspDPV1.cpp
+		uorb_to_msp_dp.cpp
+	DEPENDS
+		message_display
+		px4_work_queue
+	)

boards/modalai/voxl2/src/drivers/msp_dp_osd/MspDPV1.cpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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 <px4_platform_common/px4_config.h>
+#include <syslog.h>
+
+#include <sys/types.h>
+#include <stdbool.h>
+#include <float.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/osd/msp_osd/msp_defines.h>
+#include "msp_dp_defines.h"
+#include "MspDPV1.hpp"
+
+#include <px4_platform_common/log.h>
+
+MspDPV1::MspDPV1(int fd) :
+	_fd(fd)
+{
+}
+
+int MspDPV1::GetMessageSize(int message_type)
+{
+	return 0;
+}
+
+struct msp_message_descriptor_t {
+	uint8_t message_id;
+	bool fixed_size;
+	uint8_t message_size;
+};
+
+// #define MSP_DESCRIPTOR_COUNT 11
+#define MSP_DESCRIPTOR_COUNT 16
+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)},
+	{MSP_ANALOG, true, sizeof(msp_analog_t)},
+	{MSP_STATUS, true, sizeof(msp_dp_status_t)},
+	{MSP_BATTERY_STATE, true, sizeof(msp_battery_state_t)},
+	{MSP_RAW_GPS, true, sizeof(msp_raw_gps_t)},
+	{MSP_ATTITUDE, true, sizeof(msp_attitude_t)},
+	{MSP_ALTITUDE, true, sizeof(msp_altitude_t)},
+	{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)},
+	{MSP_SET_OSD_CANVAS, true, sizeof(msp_dp_canvas_t)},
+	{MSP_FC_VARIANT, true, sizeof(msp_fc_variant_t)},
+	{MSP_VTX_CONFIG, true, sizeof(msp_dp_vtx_config_t)},
+	{MSP_CMD_DISPLAYPORT, false, sizeof(msp_dp_cmd_t)},
+};
+
+#define MSP_FRAME_START_SIZE 5
+#define MSP_CRC_SIZE 1
+bool MspDPV1::Send(const uint8_t message_id, const void *payload, mspDirection_e direction)
+{
+	uint32_t payload_size = 0;
+
+	msp_message_descriptor_t *desc = nullptr;
+
+	for (int i = 0; i < MSP_DESCRIPTOR_COUNT; i++) {
+		if (message_id == msp_message_descriptors[i].message_id) {
+			desc = (msp_message_descriptor_t *)&msp_message_descriptors[i];
+			break;
+		}
+	}
+
+	if (!desc) {
+		return false;
+	}
+
+	// need to handle different size Displayport commands
+	if (!desc->fixed_size) {
+		if (desc->message_id ==  MSP_CMD_DISPLAYPORT){
+			uint8_t subcmd[1]{0};
+			memcpy(subcmd, payload, 1);
+			if (subcmd[0] == MSP_DP_DRAW_SCREEN){
+				payload_size = 1;
+			} else if(subcmd[0] == MSP_DP_WRITE_STRING){	// Case when we write string.. payload size may vary 
+				payload_size+=sizeof(msp_dp_cmd_t);
+				char dp_payload[sizeof(msp_dp_cmd_t)+MSP_OSD_MAX_STRING_LENGTH];
+				memcpy(dp_payload, payload, sizeof(dp_payload));
+				// Find length of string in input (may not be whole array)
+				for (int i=0;i<MSP_OSD_MAX_STRING_LENGTH;++i){
+					if(dp_payload[MSP_OSD_DP_WRITE_PAYLOAD + i] == '\0') break;
+					payload_size++;
+				}
+			} else {
+				payload_size = desc->message_size;
+			}
+		} 
+	} else {
+		payload_size = desc->message_size;
+	}
+
+	uint8_t packet[MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE];
+	uint8_t crc;
+
+	packet[0] = MSP_HEADER;
+	packet[1] = MSP_START;
+	packet[2] = direction ? MSP_CMD : MSP_REPLY;	// HDZero VTX firmware only supports 'replies'...
+	packet[3] = payload_size;
+	packet[4] = message_id;
+
+	crc = payload_size ^ message_id;
+
+	memcpy(packet + MSP_FRAME_START_SIZE, payload, payload_size);
+
+	for (uint32_t i = 0; i < payload_size; i ++) {
+		crc ^= packet[MSP_FRAME_START_SIZE + i];
+	}
+
+	packet[MSP_FRAME_START_SIZE + payload_size] = crc;
+
+	int packet_size =  MSP_FRAME_START_SIZE + payload_size + MSP_CRC_SIZE;
+	return  write(_fd, packet, packet_size) == packet_size;
+}

boards/modalai/voxl2/src/drivers/msp_dp_osd/MspDPV1.hpp

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#define MSP_READ_BUFFER_SIZE 128
+
+// MSP communication direction
+typedef enum __attribute__((packed)){
+    MSP_DIRECTION_REPLY = 0,
+    MSP_DIRECTION_REQUEST = 1
+} mspDirection_e;
+
+
+class MspDPV1
+{
+public:
+	MspDPV1(int fd);
+	int GetMessageSize(int message_type);
+	bool Send(const uint8_t message_id, const void *payload, mspDirection_e direction = MSP_DIRECTION_REQUEST);
+
+private:
+	int _fd{-1};
+};
+

boards/modalai/voxl2/src/drivers/msp_dp_osd/msp_dp_defines.h

@@ -0,0 +1,94 @@
+#pragma once
+
+// protocol bytes
+#define MSP_HEADER '$'
+#define MSP_START  'M'
+#define MSP_REPLY  '>'
+#define MSP_CMD    '<'
+
+// requests & replies
+#define MSP_VTX_CONFIG            88 //out message         	Get vtx settings 
+#define MSP_SET_VTX_CONFIG        89 //in message          	Set vtx settings 
+#define MSP_CMD_DISPLAYPORT      182 //out message			Write osd to display port 
+#define MSP_SET_OSD_CANVAS       188 // in message          Set osd canvas size COLSxROWS
+#define MSP_OSD_CANVAS           189 // out message         Get osd canvas size COLSxROWS
+
+#define MSP_OSD_DP_WRITE_PAYLOAD 4
+#define MSP_OSD_MAX_STRING_LENGTH 30 
+#define DISPLAYPORT_MSP_ATTR_BLINK   (1UL << (6)) // Device local blink
+
+// VTX CONFIG 
+struct msp_dp_vtx_config_t {
+	uint8_t  protocol;
+	uint8_t  band;	
+	uint8_t  channel;
+	uint8_t  power;	
+	uint8_t  pit;	 
+	uint16_t freq;	
+} __attribute__((packed));
+
+// STATUS 
+struct msp_dp_status_t {
+	uint8_t  unused0;
+	uint8_t  unused1;
+	uint8_t  unused2;
+	uint8_t  unused3;
+	uint8_t  unused4;
+	uint8_t  unused5;
+	uint8_t  armed;				//msp_rx_buf[6] 
+	uint8_t  arming_disable_flags_count;
+	uint32_t arming_disable_flags;
+} __attribute__((packed));
+
+#define SD_COL_MAX 30
+#define SD_ROW_MAX 16
+#define HD_COL_MAX 50
+#define HD_ROW_MAX 18
+// OSD CANVAS 
+struct msp_dp_canvas_t {
+	uint8_t  row_max;
+	uint8_t  col_max;
+} __attribute__((packed));
+
+// DISPLAYPORT CONFIG
+struct msp_dp_config_t {
+	uint8_t subcmd;
+	uint8_t fontType;
+	uint8_t resolution;
+} __attribute__((packed));
+
+// DISPLAYPORT WRITE COMMAND 
+struct msp_dp_cmd_t {	// Message is added later since it can be variable size and HDZero OSD writes the whole buffer sent, doesn't stop at delimiter
+	uint8_t subcmd;
+	uint8_t row;	
+	uint8_t col;	
+	uint8_t attr{0};		// HDZero VTX doesn't support blinking
+} __attribute__((packed));
+
+struct msp_dp_rc_sticks_t {
+	int32_t throttle;
+	int32_t roll;
+	int32_t pitch;
+	int32_t yaw;
+} __attribute__((packed));
+
+// Display Port command enums
+typedef enum __attribute__((packed)){
+    MSP_DP_HEARTBEAT = 0,       // Heartbeat
+    MSP_DP_RELEASE = 1,         // Release the display after clearing and updating
+    MSP_DP_CLEAR_SCREEN = 2,    // Clear the display
+    MSP_DP_WRITE_STRING = 3,    // Write a string at given coordinates
+    MSP_DP_DRAW_SCREEN = 4,     // Trigger a screen draw
+    MSP_DP_CONFIG = 5,          // CONFIG COMMAND -Not used by Betaflight, used by Ardupilot and INAV // CONFIG COMM
+    MSP_DP_SYS = 6,             // Display system element displayportSystemElement_e at given coordinates
+    MSP_DP_COUNT,
+} displayportMspCommand_e ;
+
+// OSD width x height
+typedef enum __attribute__((packed)) {
+    SD_3016,	
+    HD_5018,
+    HD_3016,
+    HD_5320,
+} resolutionType_e;
+