From ab5cf8ac7041b630b7b24c24db4cf9270e946e94 Mon Sep 17 00:00:00 2001 From: yaapu Date: Tue, 2 Jun 2020 13:16:23 +0200 Subject: [PATCH] AP_MSP: prepare for test version 0.7 --- .gitignore | 3 + ArduCopter/wscript | 1 + ArduPlane/wscript | 1 + Tools/CodeStyle/astylerc | 1 + libraries/AP_BLHeli/AP_BLHeli.cpp | 9 +- libraries/AP_BLHeli/AP_BLHeli.h | 4 +- libraries/AP_Common/AP_Common.h | 2 + .../AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat | 1 + .../hwdef/Pixhawk1-1M/hwdef.dat | 1 + libraries/AP_MSP/AP_MSP.cpp | 805 ++++++++++++++++++ libraries/AP_MSP/AP_MSP.h | 240 ++++++ libraries/AP_MSP/AP_MSP_Telem.cpp | 67 ++ libraries/AP_MSP/AP_MSP_Telem.h | 34 + libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 785 +++++++++++++++++ libraries/AP_MSP/AP_MSP_Telem_Backend.h | 186 ++++ libraries/AP_MSP/AP_MSP_Telem_DJI.cpp | 168 ++++ libraries/AP_MSP/AP_MSP_Telem_DJI.h | 45 + libraries/AP_MSP/msp.cpp | 297 +++++++ libraries/AP_MSP/msp.h | 142 +++ libraries/AP_MSP/msp_osd.h | 147 ++++ .../{AP_BLHeli => AP_MSP}/msp_protocol.h | 55 +- libraries/AP_MSP/msp_sbuf.cpp | 48 ++ libraries/AP_MSP/msp_sbuf.h | 28 + libraries/AP_MSP/msp_version.h | 18 + libraries/AP_MSP/test/msp_loop.sh | 7 + libraries/AP_MSP/test/msp_osd_config.raw | Bin 0 -> 6 bytes libraries/AP_MSP/test/msp_raw_imu.raw | Bin 0 -> 6 bytes libraries/AP_MSP/test/msp_status.raw | Bin 0 -> 6 bytes libraries/AP_Math/crc.cpp | 32 + libraries/AP_Math/crc.h | 3 + libraries/AP_OpticalFlow/AP_OpticalFlow.h | 2 +- .../AP_OpticalFlow/AP_OpticalFlow_MSP.cpp | 108 +++ libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h | 37 + libraries/AP_OpticalFlow/OpticalFlow.cpp | 22 +- libraries/AP_OpticalFlow/OpticalFlow.h | 13 +- .../AP_OpticalFlow/OpticalFlow_backend.h | 3 + libraries/AP_RCTelemetry/AP_RCTelemetry.h | 2 +- libraries/AP_RCTelemetry/StreamBuffer.cpp | 144 ++++ libraries/AP_RCTelemetry/StreamBuffer.h | 57 ++ libraries/AP_RangeFinder/AP_RangeFinder.cpp | 22 + libraries/AP_RangeFinder/AP_RangeFinder.h | 11 + .../AP_RangeFinder/AP_RangeFinder_Backend.h | 1 + .../AP_RangeFinder/AP_RangeFinder_MSP.cpp | 72 ++ libraries/AP_RangeFinder/AP_RangeFinder_MSP.h | 43 + .../AP_SerialManager/AP_SerialManager.cpp | 21 +- libraries/AP_SerialManager/AP_SerialManager.h | 6 + libraries/AP_Vehicle/AP_Vehicle.cpp | 9 +- libraries/AP_Vehicle/AP_Vehicle.h | 5 + 48 files changed, 3670 insertions(+), 38 deletions(-) create mode 100644 libraries/AP_MSP/AP_MSP.cpp create mode 100644 libraries/AP_MSP/AP_MSP.h create mode 100644 libraries/AP_MSP/AP_MSP_Telem.cpp create mode 100644 libraries/AP_MSP/AP_MSP_Telem.h create mode 100644 libraries/AP_MSP/AP_MSP_Telem_Backend.cpp create mode 100644 libraries/AP_MSP/AP_MSP_Telem_Backend.h create mode 100644 libraries/AP_MSP/AP_MSP_Telem_DJI.cpp create mode 100644 libraries/AP_MSP/AP_MSP_Telem_DJI.h create mode 100644 libraries/AP_MSP/msp.cpp create mode 100644 libraries/AP_MSP/msp.h create mode 100644 libraries/AP_MSP/msp_osd.h rename libraries/{AP_BLHeli => AP_MSP}/msp_protocol.h (85%) create mode 100644 libraries/AP_MSP/msp_sbuf.cpp create mode 100644 libraries/AP_MSP/msp_sbuf.h create mode 100644 libraries/AP_MSP/msp_version.h create mode 100755 libraries/AP_MSP/test/msp_loop.sh create mode 100644 libraries/AP_MSP/test/msp_osd_config.raw create mode 100644 libraries/AP_MSP/test/msp_raw_imu.raw create mode 100644 libraries/AP_MSP/test/msp_status.raw create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp create mode 100644 libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h create mode 100644 libraries/AP_RCTelemetry/StreamBuffer.cpp create mode 100644 libraries/AP_RCTelemetry/StreamBuffer.h create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_MSP.h diff --git a/.gitignore b/.gitignore index 1f651f7f23c31..d0e57112cd372 100644 --- a/.gitignore +++ b/.gitignore @@ -126,3 +126,6 @@ segv_*out /ArduSub/scripts/ persistent.dat dumpstack_*out +nbproject/ +libraries/AP_Scripting/generator/gen-bindings + diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 863834ad4ed7d..81b8e336cdfec 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -31,6 +31,7 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_KDECAN', + 'AP_MSP', ], ) diff --git a/ArduPlane/wscript b/ArduPlane/wscript index dde7456ca2535..d13006cdf6bf0 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -30,6 +30,7 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_KDECAN', + 'AP_MSP', ], ) diff --git a/Tools/CodeStyle/astylerc b/Tools/CodeStyle/astylerc index 20bc80e4ec176..9837bca29a738 100644 --- a/Tools/CodeStyle/astylerc +++ b/Tools/CodeStyle/astylerc @@ -7,4 +7,5 @@ min-conditional-indent=0 suffix=none lineend=linux pad-header +indent-switches diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 0aa569173628e..e7f1d3c654442 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -448,8 +448,8 @@ void AP_BLHeli::msp_process_command(void) break; } - case MSP_SET_4WAY_IF: { - debug("MSP_SET_4WAY_IF"); + case MSP_SET_PASSTHROUGH: { + debug("MSP_SET_PASSTHROUGH"); if (msp.dataSize == 0) { msp.escMode = PROTOCOL_4WAY; } else if (msp.dataSize == 2) { @@ -1316,6 +1316,11 @@ void AP_BLHeli::update(void) } +uint8_t AP_BLHeli::get_num_motors(void) +{ + return num_motors; +} + // get the most recent telemetry data packet for a motor bool AP_BLHeli::get_telem_data(uint8_t esc_index, struct telem_data &td) { diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 562f98c3941ad..ec1575edfe098 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -30,7 +30,7 @@ #include #include -#include "msp_protocol.h" +#include #include "blheli_4way_protocol.h" #define AP_BLHELI_MAX_ESCS 8 @@ -56,6 +56,8 @@ class AP_BLHeli { uint32_t timestamp_ms; }; + // how many motors do we have? + uint8_t get_num_motors(void); // get the most recent telemetry data packet for a motor bool get_telem_data(uint8_t esc_index, struct telem_data &td); // return the average motor frequency in Hz for dynamic filtering diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index a31e7087f9996..d902a0f8be994 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -77,6 +77,8 @@ bool. Bitnumber starts at 0 for the first bit */ #define BIT_IS_SET(value, bitnumber) (((value) & (1U<<(bitnumber))) != 0) +#define BIT_SET(value, bitnumber) ((value) |= (((typeof(value))1U) << (bitnumber))) +#define BIT_CLEAR(value, bitnumber) ((value) &= ~(((typeof(value))1U) << (bitnumber))) // get high or low bytes from 2 byte integer #define LOWBYTE(i) ((uint8_t)(i)) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat index 19d8ed9de607b..2aed9de3ea954 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat @@ -149,5 +149,6 @@ define BOARD_PWM_COUNT_DEFAULT 6 define STM32_PWM_USE_ADVANCED TRUE # we are low on flash on this board +define HAL_MSP_ENABLED 1 define HAL_MINIMIZE_FEATURES 1 define HAL_WITH_DSP 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat index 91534d170ff0d..1da7bbfa21a33 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk1-1M/hwdef.dat @@ -4,5 +4,6 @@ include ../Pixhawk1/hwdef.dat FLASH_SIZE_KB 1024 +define HAL_MSP_ENABLED 1 define HAL_MINIMIZE_FEATURES 1 undef STORAGE_FLASH_PAGE diff --git a/libraries/AP_MSP/AP_MSP.cpp b/libraries/AP_MSP/AP_MSP.cpp new file mode 100644 index 0000000000000..1237261ea1635 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP.cpp @@ -0,0 +1,805 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + MSP protocol library +*/ + +#include +#include +#include +#include +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem.h" +#include "AP_MSP_Telem_DJI.h" + +#if HAL_MSP_ENABLED + + +#define OSD_FLIGH_MODE_FOCUS_TIME 2000 +//#define DEBUG_MSP_CMD +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_MSP::var_info[] = { + + // @Param: _OSD_UNITS + // @DisplayName: Display Units + // @Description: Sets the units to use in displaying items + // @Values: 0:Imperial,1:Metric + // @User: Standard + AP_GROUPINFO("_OSD_UNITS", 1, AP_MSP, units, 0), + + // @Param: _OSD_MSGTIME + // @DisplayName: Message display duration in seconds + // @Description: Sets message duration seconds + // @Range: 1 20 + // @User: Standard + AP_GROUPINFO("_OSD_MSGTIME", 2, AP_MSP, msgtime_s, 10), + + // @Param: _OSD_NCELLS + // @DisplayName: Force cell count + // @Description: Force cell count + // @Values: 0:Auto,1 -12 + // @User: Standard + AP_GROUPINFO("_OSD_NCELLS", 3, AP_MSP, cellcount, 0), + + // @Param: _OSD_WIND_EN + // @DisplayName: Enable wind + // @Description: Enable wind + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("_OSD_WIND_EN", 4, AP_MSP, wind_en, 0), + + // @Param: _OSD_ASPD_EN + // @DisplayName: Enable airspeed override + // @Description: Enable airspeed override + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("_OSD_ASPD_EN", 5, AP_MSP, airspeed_en, 0), + + // @Param: _OSD_RSSI_EN + // @DisplayName: _OSD_RSSI_EN + // @Description: Displays RC signal strength + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_RSSI_X + // @DisplayName: _OSD_RSSI_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_RSSI_Y + // @DisplayName: _OSD_RSSI_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(rssi, "_OSD_RSSI", 6, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_ALT_EN + // @DisplayName: _OSD_ALT_EN + // @Description: Enables display of altitude AGL + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_ALT_X + // @DisplayName: _OSD_ALT_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_ALT_Y + // @DisplayName: _OSD_ALT_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(alt, "_OSD_ALT", 7, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_VBAT_EN + // @DisplayName: _OSD_VBAT_EN + // @Description: Displays main battery voltage + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_VBAT_X + // @DisplayName: _OSD_VBAT_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_VBAT_Y + // @DisplayName: _OSD_VBAT_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(vbat, "_OSD_VBAT", 8, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_CURR_EN + // @DisplayName: _OSD_CURR_EN + // @Description: Displays main battery current + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_CURR_X + // @DisplayName: _OSD_CURR_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_CURR_Y + // @DisplayName: _OSD_CURR_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(curr, "_OSD_CURR", 9, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_MAH_Y_EN + // @DisplayName: _OSD_MAH_EN + // @Description: Displays primary battery mAh consumed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_MAH_Y_X + // @DisplayName: _OSD_MAH_Y_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_MAH_Y + // @DisplayName: _OSD_MAH_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(mah, "_OSD_MAH", 10, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_SATS_EN + // @DisplayName: _OSD_SATS_EN + // @Description: Displays number of acquired sattelites + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_SATS_X + // @DisplayName: _OSD_SATS_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_SATS_Y + // @DisplayName: _OSD_SATS_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(nsats, "_OSD_SATS", 11, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_FMODE_EN + // @DisplayName: _OSD_FMODE_EN + // @Description: Displays flight mode + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_FMODE_X + // @DisplayName: _OSD_FMODE_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_FMODE_Y + // @DisplayName: _OSD_FMODE_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(flmode, "_OSD_FMODE", 12, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_MSG_EN + // @DisplayName: _OSD_MSG_EN + // @Description: Displays Mavlink messages + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_MSG_X + // @DisplayName: _OSD_MSG_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_MSG_Y + // @DisplayName: _OSD_MSG_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(name, "_OSD_MSG", 13, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_GSPD_EN + // @DisplayName: _OSD_GSPD_EN + // @Description: Displays GPS ground speed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_GSPD_X + // @DisplayName: _OSD_GSPD_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_GSPD_Y + // @DisplayName: _OSD_GSPD_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(gspd, "_OSD_GSPD", 14, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_HORIZ_EN + // @DisplayName: _OSD_HORIZ_EN + // @Description: Displays artificial horizon + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_HORIZ_X + // @DisplayName: _OSD_HORIZ_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_HORIZ_Y + // @DisplayName: _OSD_HORIZ_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(ahor, "_OSD_HORIZ", 15, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_HDIST_EN + // @DisplayName: _OSD_HDIST_EN + // @Description: Displays distance and relative direction to HOME + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_HDIST_X + // @DisplayName: _OSD_HDIST_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_HDIST_Y + // @DisplayName: _OSD_HDIST_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(hdist, "_OSD_HDIST", 16, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_HDIR_EN + // @DisplayName: _OSD_HDIR_EN + // @Description: Displays distance and relative direction to HOME + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_HDIR_X + // @DisplayName: _OSD_HDIR_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_HDIR_Y + // @DisplayName: _OSD_HDIR_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(hdir, "_OSD_HDIR", 17, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_LAT_EN + // @DisplayName: _OSD_LAT_EN + // @Description: Displays GPS latitude + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_LAT_X + // @DisplayName: _OSD_LAT_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_LAT_Y + // @DisplayName: _OSD_LAT_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(lat, "_OSD_LAT", 18, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_LON_EN + // @DisplayName: _OSD_LON_EN + // @Description: Displays GPS longitude + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_LON_X + // @DisplayName: _OSD_LON_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_LON_Y + // @DisplayName: _OSD_LON_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(lon, "_OSD_LON", 19, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_ROLL_EN + // @DisplayName: _OSD_ROLL_EN + // @Description: Displays degrees of roll from level + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_ROLL_X + // @DisplayName: _OSD_ROLL_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_ROLL_Y + // @DisplayName: _OSD_ROLL_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(roll, "_OSD_ROLL", 20, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_PITCH_EN + // @DisplayName: _OSD_PITCH_EN + // @Description: Displays degrees of pitch from level + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_PITCH_X + // @DisplayName: _OSD_PITCH_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_PITCH_Y + // @DisplayName: _OSD_PITCH_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(pitch, "_OSD_PITCH", 21, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_BATT_EN + // @DisplayName: _OSD_BATT_EN + // @Description: Displays primary battery mAh consumed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_BATT_X + // @DisplayName: _OSD_BATT_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_BATT_Y + // @DisplayName: _OSD_BATT_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(battusg, "_OSD_BATT", 22, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_VSPD_EN + // @DisplayName: _OSD_VSPD_EN + // @Description: Displays primary battery mAh consumed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_VSPD_X + // @DisplayName: _OSD_VSPD_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_VSPD_Y + // @DisplayName: _OSD_VSPD_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(vario, "_OSD_VSPD", 23, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_ARM_EN + // @DisplayName: _OSD_ARM_EN + // @Description: Displays primary battery mAh consumed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_ARM_X + // @DisplayName: _OSD_ARM_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_ARM_Y + // @DisplayName: _OSD_ARM_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(arm, "_OSD_ARM", 24, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_ACELL_EN + // @DisplayName: _OSD_ACELL_EN + // @Description: Displays primary battery mAh consumed + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_ACELL_X + // @DisplayName: _OSD_ACELL_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_ACELL_Y + // @DisplayName: _OSD_ACELL_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(cell, "_OSD_VCELL", 25, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_PWR_EN + // @DisplayName: _OSD_PWR_EN + // @Description: Displays power + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_PWR_X + // @DisplayName: _OSD_PWR_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_PWR_Y + // @DisplayName: _OSD_PWR_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(power, "_OSD_PWR", 26, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_RTC_EN + // @DisplayName: _OSD_RTC_EN + // @Description: Displays RTC + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_RTC_X + // @DisplayName: _OSD_RTC_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_RTC_Y + // @DisplayName: _OSD_RTC_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(rtc, "_OSD_RTC", 27, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_TESC_EN + // @DisplayName: _OSD_TESC_EN + // @Description: Displays RTC + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_TESC_X + // @DisplayName: _OSD_TESC_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_TESC_Y + // @DisplayName: _OSD_TESC_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(esctemp, "_OSD_TESC", 28, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_CROSS_EN + // @DisplayName: _OSD_HORIZ_EN + // @Description: Displays crosshair + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_CROSS_X + // @DisplayName: _OSD_CROSS_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_CROSS_Y + // @DisplayName: _OSD_CROSS_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(cross, "_OSD_CROSS", 29, AP_MSP, AP_OSD_Setting), + + // @Param: _OSD_HBARS_EN + // @DisplayName: _OSD_HBARS_EN + // @Description: Displays artificial horizon ladders + // @Values: 0:Disabled,1:Enabled + + // @Param: _OSD_HBARS_X + // @DisplayName: _OSD_HBARS_X + // @Description: Horizontal position on screen + // @Range: 0 27 + + // @Param: _OSD_HBARS_Y + // @DisplayName: _OSD_HBARS_Y + // @Description: Vertical position on screen + // @Range: 0 15 + AP_SUBGROUPINFO(ahbars, "_OSD_HBARS", 30, AP_MSP, AP_OSD_Setting), + + // @Param: _OPTIONS + // @DisplayName: OSD Options + // @Description: This sets options that change the display + // @Bitmask: 0:EnableTelemetryMode, 1:InvertedWindPointer + // @User: Standard + AP_GROUPINFO("_OPTIONS", 31, AP_MSP, options, 0), + + AP_GROUPEND +}; + +AP_MSP *AP_MSP::singleton; + +AP_MSP::AP_MSP() +{ + singleton = this; + AP_Param::setup_object_defaults(this, var_info); +} + +AP_MSP::~AP_MSP(void) +{ + singleton = nullptr; +} + +/* + * init - perform required initialisation + */ +bool AP_MSP::init() +{ + const AP_SerialManager &serial_manager = AP::serialmanager(); + + uint8_t msp_idx[BACKEND_COUNT] {}; + uint8_t backend_count = 0; + for (uint8_t i=0; iinit(); + } + msp_idx[BACKEND_DJI]++; + backend_count++; + } else if ((msp_port[i].uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_MSP, msp_idx[BACKEND_MSP]))) { + msp_port[i].protocol = AP_SerialManager::SerialProtocol_MSP; + msp_port[i].telem_backend = new AP_MSP_Telem(*this, i, false); + if (msp_port[i].telem_backend != nullptr) { + msp_port[i].telem_backend->init(); + } + msp_idx[BACKEND_MSP]++; + backend_count++; + } + } + + if (backend_count > 0) { + // we've found at least 1 msp decoder, start protocol handler + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_MSP::loop, void), + "MSP", + 1024, AP_HAL::Scheduler::PRIORITY_IO, 1)) { + return false; + } + } + return false; +} + +void AP_MSP::loop(void) +{ + for (uint8_t i=0; iset_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); + msp_port[i].uart->begin(0, AP_SERIALMANAGER_MSP_BUFSIZE_RX, AP_SERIALMANAGER_MSP_BUFSIZE_TX); + } + // one time osd item overrides (overrides require reboot) + if (msp_port[i].telem_backend != nullptr) { + msp_port[i].telem_backend->apply_osd_items_overrides(); + } + } + + bool update_flashing; + while (true) { + hal.scheduler->delay(1); + + // flashing timer + uint32_t now = AP_HAL::millis(); + update_flashing = false; + // toggle flashing every 0.7 seconds + if ((now - flash_start_time) > 700) { + flashing_on = !flashing_on; + flash_start_time = now; + update_flashing = true; + } + + // detect flight mode changes and steal focus from text messages + if (AP::notify().flags.flight_mode != last_flight_mode) { + flight_mode_focus = true; + last_flight_mode = AP::notify().flags.flight_mode; + last_flight_mode_change = AP_HAL::millis(); + } + + if (now - last_flight_mode_change > OSD_FLIGH_MODE_FOCUS_TIME) { + flight_mode_focus = false; + } + + for (uint8_t i=0; i< MSP_MAX_INSTANCES; i++) { + if (msp_port[i].telem_backend != nullptr) { + // dynamically hide/unhide only if flashing status changed + if (update_flashing) { + //gcs().send_text(MAV_SEVERITY_DEBUG, "pos:%d, mode:%d", MSP_OSD_POS(osd_item_settings[OSD_FLYMODE]),msp_port[i].telem_backend->get_osd_flight_mode_bitmask()); + msp_port[i].telem_backend->flash_osd_items(); + } + // process incoming MSP frames + process_serial_data(i); + // push telemetry frames + msp_port[i].telem_backend->run_wfq_scheduler(); + } + } + } +} + +bool AP_MSP::check_option(uint32_t option) +{ + return (options & option) != 0; +} + +/* + invoked by the WFQ telemetry scheduler as a callback at each wfq step +*/ +void AP_MSP::msp_telemetry_callback(uint16_t cmd_msp, uint8_t protocol_instance) +{ +#ifdef DEBUG_MSP_CMD + gcs().send_text(MAV_SEVERITY_DEBUG,"cmd_msp=%d, instance=%d",cmd_msp, protocol_instance); +#endif + uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; + + msp_packet_t reply = { + .buf = { .ptr = outBuf, .end = MSP_ARRAYEND(outBuf), }, + .cmd = (int16_t)cmd_msp, + .flags = 0, + .result = 0, + }; + uint8_t *outBufHead = reply.buf.ptr; + + msp_process_out_command(&msp_port[protocol_instance], cmd_msp, &reply.buf); + sbuf_switch_to_reader(&reply.buf, outBufHead); // change streambuf direction + msp_serial_encode(&msp_port[protocol_instance], &reply, msp_port[protocol_instance].msp_version); + + msp_port[protocol_instance].c_state = MSP_IDLE; + +} + +void AP_MSP::msp_handle_opflow(const msp_opflow_sensor_t &pkt) +{ + OpticalFlow *optflow = AP::opticalflow(); + if (optflow == nullptr) { + return; + } + optflow->handle_msp(pkt); +} + +void AP_MSP::msp_handle_rangefinder(const msp_rangefinder_sensor_t &pkt) +{ + RangeFinder *rangefinder = AP::rangefinder(); + if (rangefinder == nullptr) { + return; + } + rangefinder->handle_msp(pkt); +} + +/* + * read serial + */ +void AP_MSP::process_serial_data(uint8_t instance) +{ + if (msp_port[instance].uart == nullptr) { + return; + } + + uint32_t numc = msp_port[instance].uart->available(); + + if (numc > 0) { + // There are bytes incoming + msp_port[instance].last_activity_ms = AP_HAL::millis(); + + // Process incoming bytes + while (numc-- > 0) { + const uint8_t c = msp_port[instance].uart->read(); + msp_parse_received_data(&msp_port[instance], c); + + if (msp_port[instance].c_state == MSP_COMMAND_RECEIVED) { + msp_process_received_command(&msp_port[instance]); + } + } + } +} + +void AP_MSP::msp_process_received_command(msp_port_t *msp) +{ + uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; + + msp_packet_t reply = { + .buf = { .ptr = outBuf, .end = MSP_ARRAYEND(outBuf), }, + .cmd = -1, + .flags = 0, + .result = 0, + }; + uint8_t *outBufHead = reply.buf.ptr; + + msp_packet_t command = { + .buf = { .ptr = msp->in_buf, .end = msp->in_buf + msp->data_size, }, + .cmd = (int16_t)msp->cmd_msp, // this cast looks suspicious but it's the same in inav, compiler checks in inav are less strict? + .flags = msp->cmd_flags, + .result = 0, + }; + + const msp_result_e status = msp_process_command(msp, &command, &reply); + + if (status != MSP_RESULT_NO_REPLY) { + sbuf_switch_to_reader(&reply.buf, outBufHead); // change streambuf direction + msp_serial_encode(msp, &reply, msp->msp_version); + } + + msp->c_state = MSP_IDLE; +} + +msp_result_e AP_MSP::msp_process_command(msp_port_t *msp, msp_packet_t *cmd, msp_packet_t *reply) +{ +#ifdef DEBUG_MSP_CMD + gcs().send_text(MAV_SEVERITY_DEBUG,"cmd_msp=%d", cmd_msp); +#endif + msp_result_e ret = MSP_RESULT_ACK; + sbuf_t *dst = &reply->buf; + sbuf_t *src = &cmd->buf; + const uint16_t cmdMSP = cmd->cmd; + // initialize reply by default + reply->cmd = cmd->cmd; + + if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) { + ret = msp_process_sensor_command(msp, cmdMSP, src); + } else if (msp_process_out_command(msp, cmdMSP, dst)) { + ret = MSP_RESULT_ACK; + } + + // Process DONT_REPLY flag + if (cmd->flags & MSP_FLAG_DONT_REPLY) { + ret = MSP_RESULT_NO_REPLY; + } + + reply->result = ret; + return ret; +} + +bool AP_MSP::msp_process_out_command(msp_port_t *msp, uint16_t cmd_msp, sbuf_t *dst) +{ + if (msp->telem_backend == nullptr) { + return false; + } + + switch (cmd_msp) { + case MSP_API_VERSION: + return msp->telem_backend->msp_process_out_api_version(dst); + case MSP_FC_VARIANT: + return msp->telem_backend->msp_process_out_fc_variant(dst); + case MSP_FC_VERSION: + return msp->telem_backend->msp_process_out_fc_version(dst); + case MSP_BOARD_INFO: + return msp->telem_backend->msp_process_out_board_info(dst); + case MSP_BUILD_INFO: + return msp->telem_backend->msp_process_out_build_info(dst); + case MSP_NAME: + return msp->telem_backend->msp_process_out_name(dst); + case MSP_OSD_CONFIG: + return msp->telem_backend->msp_process_out_osd_config(dst); + case MSP_STATUS: + case MSP_STATUS_EX: + return msp->telem_backend->msp_process_out_status(dst); + case MSP_RAW_GPS: + return msp->telem_backend->msp_process_out_raw_gps(dst); + case MSP_COMP_GPS: + return msp->telem_backend->msp_process_out_comp_gps(dst); + case MSP_ATTITUDE: + return msp->telem_backend->msp_process_out_attitude(dst); + case MSP_ALTITUDE: + return msp->telem_backend->msp_process_out_altitude(dst); + case MSP_ANALOG: + return msp->telem_backend->msp_process_out_analog(dst); + case MSP_BATTERY_STATE: + return msp->telem_backend->msp_process_out_battery_state(dst); + case MSP_UID: + return msp->telem_backend->msp_process_out_uid(dst); +#ifdef HAVE_AP_BLHELI_SUPPORT + case MSP_ESC_SENSOR_DATA: + return msp->telem_backend->msp_process_out_esc_sensor_data(dst); +#endif + case MSP_RTC: + return msp->telem_backend->msp_process_out_rtc(dst); + case MSP_RC: + return msp->telem_backend->msp_process_out_rc(dst); + default: + return false; + } +} + +msp_result_e AP_MSP::msp_process_sensor_command(msp_port_t* msp, uint16_t cmd_msp, sbuf_t *src) +{ + MSP_UNUSED(src); + + switch (cmd_msp) { + case MSP2_SENSOR_RANGEFINDER: { + const msp_rangefinder_sensor_t pkt = *(const msp_rangefinder_sensor_t *)src->ptr; + msp_handle_rangefinder(pkt); + } + break; + case MSP2_SENSOR_OPTIC_FLOW: { + const msp_opflow_sensor_t pkt = *(const msp_opflow_sensor_t *)src->ptr; + msp_handle_opflow(pkt); + } + break; + } + + return MSP_RESULT_NO_REPLY; +} + +namespace AP +{ +AP_MSP *msp() +{ + return AP_MSP::get_singleton(); +} +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP.h b/libraries/AP_MSP/AP_MSP.h new file mode 100644 index 0000000000000..009587fa85f26 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP.h @@ -0,0 +1,240 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include + +#include "AP_MSP_Telem_Backend.h" + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +#define MSP_MAX_INSTANCES 3 +#define ARMING_DISABLED_ARM_SWITCH 1 << 24 +#define ARMING_DISABLE_FLAGS_COUNT 25 + +#define MSP_OSD_START 2048 +#define MSP_OSD_STEP_X 1 +#define MSP_OSD_STEP_Y 32 +#define MSP_OSD_POS(osd_setting) (MSP_OSD_START + osd_setting->xpos*MSP_OSD_STEP_X + osd_setting->ypos*MSP_OSD_STEP_Y) + +using namespace MSP; + +class AP_MSP +{ +public: + AP_MSP(); + + ~AP_MSP(); + + /* Do not allow copies */ + AP_MSP(const AP_MSP &other) = delete; + AP_MSP &operator=(const AP_MSP&) = delete; + + // User settable parameters + static const struct AP_Param::GroupInfo var_info[]; + + // init - perform required initialisation + bool init(); + void msp_telemetry_callback(uint16_t cmd_msp, uint8_t protocol_instance); + + static AP_MSP *get_singleton(void) + { + return singleton; + } + +private: + // to access the telemetry_info member variable + friend class AP_MSP_Telem; + friend class AP_MSP_Telem_DJI; + friend class AP_MSP_Telem_Backend; + + AP_OSD_Setting rssi {true, 18, 0}; // OSD_RSSI + AP_OSD_Setting vbat {true, 1, 6}; // OSD_MAIN_BATT_VOLTAGE + AP_OSD_Setting ahor {true, 14, 3}; // OSD_ARTIFICIAL_HORIZON + AP_OSD_Setting ahbars {true, 14, 7}; // OSD_HORIZON_SIDEBARS + AP_OSD_Setting cross {true, 14, 7}; // OSD_HORIZON_SIDEBARS + AP_OSD_Setting flmode {true, 17, 15}; // OSD_FLYMODE - not used for now + AP_OSD_Setting name {true, 15, 14}; // OSD_CRAFT_NAME + AP_OSD_Setting curr {true, 1, 12}; // OSD_CURRENT_DRAW + AP_OSD_Setting mah {true, 9, 15}; // OSD_MAH_DRAWN + AP_OSD_Setting gspd {true, 1, 8}; // OSD_GPS_SPEED + AP_OSD_Setting nsats {true, 1, 0}; // OSD_GPS_SATS + AP_OSD_Setting alt {true, 25, 9}; // OSD_ALTITUDE + AP_OSD_Setting cell {true, 1, 4}; // OSD_AVG_CELL_VOLTAGE + AP_OSD_Setting lon {true, 9, 0}; // OSD_GPS_LON + AP_OSD_Setting lat {true, 9, 1}; // OSD_GPS_LAT + AP_OSD_Setting pitch {true, 26, 7}; // OSD_PITCH_ANGLE + AP_OSD_Setting roll {true, 26, 8}; // OSD_ROLL_ANGLE + AP_OSD_Setting yaw {true, 26, 6}; // OSD_NUMERICAL_HEADING + AP_OSD_Setting battusg {true, 9, 15}; // OSD_MAIN_BATT_USAGE + AP_OSD_Setting hdir {true, 1, 2}; // OSD_HOME_DIR + AP_OSD_Setting hdist {true, 1, 1}; // OSD_HOME_DIST + AP_OSD_Setting vario {true, 27, 10}; // OSD_NUMERICAL_VARIO + AP_OSD_Setting arm {true, 11, 8}; // OSD_DISARMED + AP_OSD_Setting power {true, 1, 13}; // OSD_POWER + AP_OSD_Setting rtc {true, 12, 0}; // OSD_RTC_DATETIME + AP_OSD_Setting esctemp {true, 6, 15}; // OSD_ESC_TMP + + enum : uint8_t { + BACKEND_MSP, + BACKEND_DJI, + BACKEND_COUNT + }; + + enum { + OPTION_TELEMETRY_MODE = 1U<<0, + OPTION_INVERTED_WIND = 1U<<1 + }; + + AP_Int32 options; + + AP_Int8 units; + AP_Int8 msgtime_s; + AP_Int8 cellcount; + AP_Int8 wind_en; + AP_Int8 airspeed_en; + + // these are the osd items we support for MSP OSD + AP_OSD_Setting* osd_item_settings[58] = { + &rssi, // OSD_RSSI_VALUE + &vbat, // OSD_MAIN_BATT_VOLTAGE + &cross, // OSD_CROSSHAIRS + &ahor, // OSD_ARTIFICIAL_HORIZON + &ahbars, // OSD_HORIZON_SIDEBARS + nullptr, // OSD_ITEM_TIMER_1 + nullptr, // OSD_ITEM_TIMER_2 + &flmode, // OSD_FLYMODE + &name, // OSD_CRAFT_NAME + nullptr, // OSD_THROTTLE_POS + nullptr, // OSD_VTX_CHANNEL + &curr, // OSD_CURRENT_DRAW + &mah, // OSD_MAH_DRAWN + &gspd, // OSD_GPS_SPEED + &nsats, // OSD_GPS_SATS + &alt, // OSD_ALTITUDE + nullptr, // OSD_ROLL_PIDS + nullptr, // OSD_PITCH_PIDS + nullptr, // OSD_YAW_PIDS + &power, // OSD_POWER + nullptr, // OSD_PIDRATE_PROFILE + nullptr, // OSD_WARNINGS + &cell, // OSD_AVG_CELL_VOLTAGE + &lon, // OSD_GPS_LON + &lat, // OSD_GPS_LAT + nullptr, // OSD_DEBUG + &pitch, // OSD_PITCH_ANGLE + &roll, // OSD_ROLL_ANGLE + &battusg, // OSD_MAIN_BATT_USAGE + &arm, // OSD_DISARMED + &hdir, // OSD_HOME_DIR + &hdist, // OSD_HOME_DIST + &yaw, // OSD_NUMERICAL_HEADING + &vario, // OSD_NUMERICAL_VARIO + nullptr, // OSD_COMPASS_BAR +#ifdef HAVE_AP_BLHELI_SUPPORT + &esctemp, // OSD_ESC_TMP +#else + nullptr, // OSD_ESC_TMP +#endif + nullptr, // OSD_ESC_RPM + nullptr, // OSD_REMAINING_TIME_ESTIMATE + &rtc, // OSD_RTC_DATETIME + nullptr, // OSD_ADJUSTMENT_RANGE + nullptr, // OSD_CORE_TEMPERATURE + nullptr, // OSD_ANTI_GRAVITY + nullptr, // OSD_G_FORCE + nullptr, // OSD_MOTOR_DIAG + nullptr, // OSD_LOG_STATUS + nullptr, // OSD_FLIP_ARROW + nullptr, // OSD_LINK_QUALITY + nullptr, // OSD_FLIGHT_DIST + nullptr, // OSD_STICK_OVERLAY_LEFT + nullptr, // OSD_STICK_OVERLAY_RIGHT + nullptr, // OSD_DISPLAY_NAME + nullptr, // OSD_ESC_RPM_FREQ + nullptr, // OSD_RATE_PROFILE_NAME + nullptr, // OSD_PID_PROFILE_NAME + nullptr, // OSD_PROFILE_NAME + nullptr, // OSD_RSSI_DBM_VALUE + nullptr, // OSD_RC_CHANNELS + nullptr, // OSD_CAMERA_FRAME, + }; + + const int osd_enabled_stats[24] = { + -1, // OSD_STAT_RTC_DATE_TIME + -1, // OSD_STAT_TIMER_1 + -1, // OSD_STAT_TIMER_2 + -1, // OSD_STAT_MAX_SPEED + -1, // OSD_STAT_MAX_DISTANCE + -1, // OSD_STAT_MIN_BATTERY + -1, // OSD_STAT_END_BATTERY + -1, // OSD_STAT_BATTERY + -1, // OSD_STAT_MIN_RSSI + -1, // OSD_STAT_MAX_CURRENT + -1, // OSD_STAT_USED_MAH + -1, // OSD_STAT_MAX_ALTITUDE + -1, // OSD_STAT_BLACKBOX + -1, // OSD_STAT_BLACKBOX_NUMBER + -1, // OSD_STAT_MAX_G_FORCE + -1, // OSD_STAT_MAX_ESC_TEMP + -1, // OSD_STAT_MAX_ESC_RPM + -1, // OSD_STAT_MIN_LINK_QUALITY + -1, // OSD_STAT_FLIGHT_DISTANCE + -1, // OSD_STAT_MAX_FFT + -1, // OSD_STAT_TOTAL_FLIGHTS + -1, // OSD_STAT_TOTAL_TIME + -1, // OSD_STAT_TOTAL_DIST + -1, // OSD_STAT_MIN_RSSI_DBM + }; + + AP_MSP_Telem_Backend::telemetry_info_t telemetry_info; + osd_config_t osd_config; + msp_port_t msp_port[MSP_MAX_INSTANCES]; + // OSD item flashing support + uint32_t flash_start_time; + bool flashing_on; + // we need to detect flight mode changes + uint8_t last_flight_mode = 255; + uint32_t last_flight_mode_change = AP_HAL::millis(); + bool flight_mode_focus; + + + void loop(void); + void process_serial_data(uint8_t instance); + bool check_option(uint32_t option); + + // MSP protocol decoder + void msp_process_received_command(msp_port_t *msp); + msp_result_e msp_process_command(msp_port_t* msp, msp_packet_t *cmd, msp_packet_t *reply); + msp_result_e msp_process_sensor_command(msp_port_t* msp, uint16_t cmd_msp, sbuf_t *src); + bool msp_process_out_command(msp_port_t* msp, uint16_t cmd_msp, sbuf_t *dst); + + // MSP sensor command processing + void msp_handle_opflow(const msp_opflow_sensor_t &pkt); + void msp_handle_rangefinder(const msp_rangefinder_sensor_t &pkt); + + static AP_MSP *singleton; +}; + +namespace AP +{ +AP_MSP *msp(); +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem.cpp b/libraries/AP_MSP/AP_MSP_Telem.cpp new file mode 100644 index 0000000000000..15097c88d9dff --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem.cpp @@ -0,0 +1,67 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + MSP Telemetry library +*/ + +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem.h" + +#if HAL_MSP_ENABLED + +extern const AP_HAL::HAL& hal; + + +AP_MSP_Telem::AP_MSP_Telem(AP_MSP& msp, uint8_t protocol_instance, bool scheduler_enabled) : AP_MSP_Telem_Backend(msp, protocol_instance, scheduler_enabled) +{ +} + +AP_MSP_Telem::~AP_MSP_Telem(void) +{ +} + +bool AP_MSP_Telem::msp_process_out_api_version(sbuf_t *dst) +{ + sbuf_write_u8(dst, MSP_PROTOCOL_VERSION); + sbuf_write_u8(dst, API_VERSION_MAJOR); + sbuf_write_u8(dst, API_VERSION_MINOR); + return true; +} + +bool AP_MSP_Telem::msp_process_out_fc_version(sbuf_t *dst) +{ + sbuf_write_u8(dst, FC_VERSION_MAJOR); + sbuf_write_u8(dst, FC_VERSION_MINOR); + sbuf_write_u8(dst, FC_VERSION_PATCH_LEVEL); + return true; +} + +bool AP_MSP_Telem::msp_process_out_fc_variant(sbuf_t *dst) +{ + sbuf_write_data(dst, "ARDU", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + return true; +} + +uint32_t AP_MSP_Telem::get_osd_flight_mode_bitmask(void) +{ + BIT_SET(osd_hidden_items_bitmask, OSD_FLYMODE); + return 0; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem.h b/libraries/AP_MSP/AP_MSP_Telem.h new file mode 100644 index 0000000000000..d12144bd8955e --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem.h @@ -0,0 +1,34 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_MSP_Telem_Backend.h" + +#if HAL_MSP_ENABLED + +class AP_MSP_Telem : public AP_MSP_Telem_Backend +{ +public: + AP_MSP_Telem(AP_MSP& msp, uint8_t protocol_instance, bool scheduler_enabled); + + ~AP_MSP_Telem(); + + uint32_t get_osd_flight_mode_bitmask(void) override; + bool msp_process_out_api_version(sbuf_t *dst) override; + bool msp_process_out_fc_version(sbuf_t *dst) override; + bool msp_process_out_fc_variant(sbuf_t *dst) override; +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp new file mode 100644 index 0000000000000..e0416a3b1a90e --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -0,0 +1,785 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + MSP Telemetry library +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem_Backend.h" + +//#define ENABLE_MSG_FLASH_ON_FAILSAFE + +#if HAL_MSP_ENABLED + +extern const AP_HAL::HAL& hal; +constexpr uint8_t AP_MSP_Telem_Backend::arrows[8]; + +//#define MSP_OSD_POS_HIDDEN 234 +#define MSP_OSD_POS_HIDDEN 0 + +AP_MSP_Telem_Backend::AP_MSP_Telem_Backend(AP_MSP& msp, uint8_t protocol_instance, bool scheduler_enabled) : AP_RCTelemetry(MSP_TIME_SLOT_MAX), + _protocol_instance(protocol_instance), + _msp(msp), + _scheduler_enabled(scheduler_enabled) +{ +} + +AP_MSP_Telem_Backend::~AP_MSP_Telem_Backend(void) +{ +} + +void AP_MSP_Telem_Backend::setup_wfq_scheduler(void) +{ + // initialize packet weights for the WFQ scheduler + // priority[i] = 1/_scheduler.packet_weight[i] + // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) ) + + set_scheduler_entry(EMPTY_SLOT,50,50); // nothing to send + set_scheduler_entry(NAME,200,200); // 5Hz 12 chars string used for general purpose text messages + set_scheduler_entry(STATUS,500,500); // 2Hz flightmode + set_scheduler_entry(CONFIG,200,200); // 5Hz OSD item positions + set_scheduler_entry(RAW_GPS,250,250); // 4Hz GPS lat/lon + set_scheduler_entry(COMP_GPS,250,250); // 4Hz home direction and distance + set_scheduler_entry(ATTITUDE,200,200); // 5Hz attitude + set_scheduler_entry(ALTITUDE,250,250); // 4Hz altitude(cm) and velocity(cm/s) + set_scheduler_entry(ANALOG,250,250); // 4Hz rssi + batt + set_scheduler_entry(BATTERY_STATE,500,500); // 2Hz battery + set_scheduler_entry(ESC_SENSOR_DATA,500,500); // 2Hz ESC telemetry + set_scheduler_entry(RTC_DATETIME,1000,1000); // 1Hz RTC +} + +/* + * init - perform required initialisation + */ +bool AP_MSP_Telem_Backend::init() +{ + enable_warnings(); + + return AP_RCTelemetry::init(); +} + +void AP_MSP_Telem_Backend::run_wfq_scheduler() +{ + if (!_scheduler_enabled) { + return; + } + + AP_RCTelemetry::run_wfq_scheduler(); +} + +void AP_MSP_Telem_Backend::adjust_packet_weight(bool queue_empty) +{ +} + +// WFQ scheduler +bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty) +{ + switch (idx) { + case EMPTY_SLOT: // empty slot + return true; + case NAME: // used for status_text messages + return true; + case STATUS: // flightmode + return true; + case CONFIG: // OSD config + return true; + case RAW_GPS: // lat,lon, speed + return true; + case COMP_GPS: // home dir,dist + return true; + case ATTITUDE: // Attitude + return true; + case ALTITUDE: // Altitude and Vario + return true; + case ANALOG: // Rssi, Battery, mAh, Current + return true; + case BATTERY_STATE: // voltage, capacity, current, mAh + return true; + case ESC_SENSOR_DATA: // esc temp + rpm + return true; + case RTC_DATETIME: // RTC + return true; + default: + return false; + } +} + +/* + * WFQ scheduler + */ +void AP_MSP_Telem_Backend::process_packet(uint8_t idx) +{ + // at each wfq scheduler step invoke a callback on MSP + // we have a single thread so all access should be safe + if (idx == EMPTY_SLOT) { + return; + } + _msp.msp_telemetry_callback(msp_packet_type_map[idx],_protocol_instance); +} + +bool AP_MSP_Telem_Backend::get_next_msg_chunk(void) +{ + return true; +} + +uint8_t AP_MSP_Telem_Backend::calc_cell_count(float battery_voltage) +{ + return floorf((battery_voltage / CELLFULL) + 1); +} + +float AP_MSP_Telem_Backend::get_vspeed_ms(void) +{ + { + // release semaphore as soon as possible + AP_AHRS &_ahrs = AP::ahrs(); + Vector3f v; + WITH_SEMAPHORE(_ahrs.get_semaphore()); + if (_ahrs.get_velocity_NED(v)) { + return -v.z; + } + } + auto &_baro = AP::baro(); + WITH_SEMAPHORE(_baro.get_semaphore()); + return _baro.get_climb_rate(); +} + +void AP_MSP_Telem_Backend::telem_update_home_pos(telemetry_info_t& _telemetry) +{ + AP_AHRS &_ahrs = AP::ahrs(); + WITH_SEMAPHORE(_ahrs.get_semaphore()); + Location loc; + float alt; + if (_ahrs.get_position(loc) && _ahrs.home_is_set()) { + const Location &home_loc = _ahrs.get_home(); + _telemetry.home_distance_m = home_loc.get_distance(loc); + _telemetry.home_bearing_cd = loc.get_bearing_to(home_loc); + } else { + _telemetry.home_distance_m = 0; + _telemetry.home_bearing_cd = 0; + } + _ahrs.get_relative_position_D_home(alt); + _telemetry.rel_altitude_cm = -alt * 100; + _telemetry.vspeed_ms = get_vspeed_ms(); + _telemetry.home_is_set = _ahrs.home_is_set(); +} + +void AP_MSP_Telem_Backend::telem_update_gps_state(telemetry_info_t& _telemetry) +{ + const AP_GPS& gps = AP::gps(); + _telemetry.gps_fix_type = gps.status(); + + if (_telemetry.gps_fix_type >= AP_GPS::GPS_Status::GPS_OK_FIX_2D) { + _telemetry.gps_num_sats = gps.num_sats(); + } else { + _telemetry.gps_num_sats = 0; + } + + if (_telemetry.gps_fix_type >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) { + const Location &loc = AP::gps().location(); //get gps instance 0 + _telemetry.gps_latitude = loc.lat; + _telemetry.gps_longitude = loc.lng; + _telemetry.gps_altitude_cm = loc.alt; + _telemetry.gps_speed_ms = gps.ground_speed(); + _telemetry.gps_ground_course_cd = gps.ground_course_cd(); + } else { + _telemetry.gps_latitude = 0; + _telemetry.gps_longitude = 0; + _telemetry.gps_altitude_cm = 0; + _telemetry.gps_speed_ms = 0; + _telemetry.gps_ground_course_cd = 0; + } +} + +void AP_MSP_Telem_Backend::telem_update_battery_state(telemetry_info_t& _telemetry) +{ + const AP_BattMonitor &_battery = AP::battery(); + if (!_battery.current_amps(_telemetry.batt_current_a, 0)) { + _telemetry.batt_current_a = 0; + } + if (!_battery.consumed_mah(_telemetry.batt_consumed_mah, 0)) { + _telemetry.batt_consumed_mah = 0; + } + _telemetry.batt_voltage_v =_battery.voltage(0); + _telemetry.batt_capacity_mah = _battery.pack_capacity_mah(0); + + const AP_Notify& notify = AP::notify(); + if (notify.flags.failsafe_battery) { + _telemetry.batt_state = MSP_BATTERY_CRITICAL; + } else { + _telemetry.batt_state = MSP_BATTERY_OK; + } + // detect cellcount once and accept only higher values, we do not want to update it while discharging + uint8_t cc = calc_cell_count(_telemetry.batt_voltage_v); + if (cc > _telemetry.batt_cellcount) { + _telemetry.batt_cellcount = cc; + } + +} + +void AP_MSP_Telem_Backend::telem_update_attitude(telemetry_info_t &_telemetry) +{ + const AP_AHRS &_ahrs = AP::ahrs(); + + _telemetry.roll_cd = _ahrs.roll_sensor; // centidegress to decidegrees -1800,1800 + _telemetry.pitch_cd = _ahrs.pitch_sensor; // centidegress to decidegrees -1800,1800 + _telemetry.yaw_deg = _ahrs.yaw_sensor * 0.01; // degrees +} + +void AP_MSP_Telem_Backend::telem_update_airspeed(telemetry_info_t &_telemetry) +{ + float aspd = 0.0f; + AP_AHRS &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + _telemetry.airspeed_have_estimate = ahrs.airspeed_estimate(aspd); + if (_telemetry.airspeed_have_estimate) { + _telemetry.airspeed_estimate_ms = aspd; + } else { + _telemetry.airspeed_estimate_ms = 0.0; + } +} + + +void AP_MSP_Telem_Backend::telem_update_flight_mode(telemetry_info_t &_telemetry) +{ + // possible layouts + // MANU + // MANU [S] + // MANU [SS] + + const AP_Notify *notify = AP_Notify::get_singleton(); + + char buffer[MSP_TXT_BUFFER_SIZE+1] {}; + char* buffer_ptr = buffer; + + bool simple_mode = gcs().simple_input_active(); + bool supersimple_mode = gcs().supersimple_input_active(); + + // clear + memset(_telemetry.flight_mode_str,0,MSP_TXT_BUFFER_SIZE+1); + // first copy flightmode + strncpy(buffer_ptr, notify->get_flight_mode_str(), 4); + buffer_ptr += 4; + // simple mode + if (simple_mode) { + strncpy(buffer_ptr, " [S]",4); + buffer_ptr += 4; + } else if (supersimple_mode) { + strncpy(buffer_ptr, " [SS]",5); + buffer_ptr += 5; + } + + // center + uint8_t padding = (MSP_TXT_VISIBLE_CHARS - (buffer_ptr - buffer))/2; + if (padding > 0) { + memset(_telemetry.flight_mode_str,' ', padding); + } + // padding + strlen(buffer) <= MSP_TXT_BUFFER_SIZE + strncpy(_telemetry.flight_mode_str+padding, buffer, strlen(buffer)); +} + +void AP_MSP_Telem_Backend::telem_update_localtime(telemetry_info_t &_telemetry) +{ + uint64_t time_usec = 0; + if (AP::rtc().get_utc_usec(time_usec)) { // may fail, leaving time_unix at 0 + const time_t time_sec = time_usec / 1000000; + _telemetry.localtime_tm = *gmtime(&time_sec); + } else { + _telemetry.localtime_tm.tm_year = 0; + _telemetry.localtime_tm.tm_mon = 0; + _telemetry.localtime_tm.tm_mday = 0; + _telemetry.localtime_tm.tm_hour = 0; + _telemetry.localtime_tm.tm_min = 0; + _telemetry.localtime_tm.tm_sec = 0; + } +} + +void AP_MSP_Telem_Backend::telem_update_wind(telemetry_info_t &_telemetry) +{ + const AP_Notify *notify = AP_Notify::get_singleton(); + AP_AHRS &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + Vector3f v = ahrs.wind_estimate(); + AP_MSP* msp = AP::msp(); + if (msp->check_option(AP_OSD::OPTION_INVERTED_WIND)) { + v = -v; + } + // if needed convert m/s to ft/s + float v_length = msp->units > 0 ? v.length() : v.length() * 3.28084; + const char* unit = msp->units > 0 ? "m/s" : "f/s"; + // clear + memset(_telemetry.flight_mode_str,0,MSP_TXT_BUFFER_SIZE+1); + if (v_length > 1.0f) { + int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - ahrs.yaw_sensor); + int32_t interval = 36000 / MSP_ARROW_COUNT; + uint8_t arrow = arrows[((angle + interval / 2) / interval) % MSP_ARROW_COUNT]; + + // flightmode + wind can use up to MSP_TXT_VISIBLE_CHARS-1 chars, leaving 1 visible char for a multibyte utf8 arrow + // example: MANU 4m/s ↗ + int8_t len = snprintf(_telemetry.flight_mode_str, MSP_TXT_VISIBLE_CHARS-1, "%s %d%s ", notify->get_flight_mode_str(), (uint8_t)roundf(v_length), unit); + + // UTF8 arrow bytes 0xE286nn + _telemetry.flight_mode_str[len] = 0xE2; + _telemetry.flight_mode_str[len+1] = 0x86; + _telemetry.flight_mode_str[len+2] = arrow; + } else { + // no more than MSP_TXT_VISIBLE_CHARS chars + int8_t len = snprintf(_telemetry.flight_mode_str, MSP_TXT_VISIBLE_CHARS, "%s ---%s", notify->get_flight_mode_str(), unit); + // UTF8 arrow bytes 0xE286nn + _telemetry.flight_mode_str[len] = 0xE2; + _telemetry.flight_mode_str[len+1] = 0x86; + _telemetry.flight_mode_str[len+2] = arrows[0]; + } +} + + +void AP_MSP_Telem_Backend::enable_warnings() +{ + BIT_SET(_msp.osd_config.enabled_warnings, OSD_WARNING_FAIL_SAFE); + BIT_SET(_msp.osd_config.enabled_warnings, OSD_WARNING_BATTERY_CRITICAL); +} + +bool AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst) +{ + telem_update_gps_state(_msp.telemetry_info); + + sbuf_write_u8(dst, _msp.telemetry_info.gps_fix_type >= 3 ? 2 : 0); // bitmask 1 << 1 is GPS FIX + sbuf_write_u8(dst, _msp.telemetry_info.gps_num_sats); + sbuf_write_u32(dst, _msp.telemetry_info.gps_latitude); + sbuf_write_u32(dst, _msp.telemetry_info.gps_longitude); + sbuf_write_u16(dst, (uint16_t)constrain_int32(_msp.telemetry_info.gps_altitude_cm / 100, 0, UINT16_MAX)); // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. To maintain backwards compatibility compensate to 1m per lsb in MSP again. + // handle airspeed override + if ( _msp.airspeed_en) { + telem_update_airspeed(_msp.telemetry_info); + sbuf_write_u16(dst, _msp.telemetry_info.airspeed_estimate_ms * 100); // airspeed in cm/s + } else { + sbuf_write_u16(dst, (uint16_t)roundf(_msp.telemetry_info.gps_speed_ms * 100)); // speed in cm/s + } + sbuf_write_u16(dst, _msp.telemetry_info.gps_ground_course_cd * 1000); // degrees * 10 == centideg * 1000 + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_comp_gps(sbuf_t *dst) +{ + telem_update_home_pos(_msp.telemetry_info); + telem_update_attitude(_msp.telemetry_info); + + // no need to apply yaw conpensation, the DJI air unit will do it for us :-) + int32_t home_angle_deg = _msp.telemetry_info.home_bearing_cd * 0.01; + if (_msp.telemetry_info.home_distance_m < 2) { + //avoid fast rotating arrow at small distances + home_angle_deg = 0; + } + + sbuf_write_u16(dst, _msp.telemetry_info.home_distance_m); + sbuf_write_u16(dst, (uint16_t)home_angle_deg); //deg + sbuf_write_u8(dst, 1); // 1 is toggle GPS position update + return true; +} + +// Autoscroll message is the same as in minimosd-extra. +// Thanks to night-ghost for the approach. +bool AP_MSP_Telem_Backend::msp_process_out_name(sbuf_t *dst) +{ + AP_Notify * notify = AP_Notify::get_singleton(); + if (notify) { + // text message is visible for _msp.msgtime_s but only if + // a flight mode change did not steal focus + int32_t visible_time = AP_HAL::millis() - notify->get_text_updated_millis(); + if (visible_time < _msp.msgtime_s *1000 && !_msp.flight_mode_focus) { + char buffer[NOTIFY_TEXT_BUFFER_SIZE]; + strncpy(buffer, notify->get_text(), sizeof(buffer)); + uint8_t len = strnlen(buffer, sizeof(buffer)); + + for (uint8_t i=0; i MSP_TXT_VISIBLE_CHARS) { + uint8_t chars_to_scroll = len - MSP_TXT_VISIBLE_CHARS; + uint8_t total_cycles = 2*message_scroll_delay + 2*chars_to_scroll; + uint8_t current_cycle = (visible_time / message_scroll_time_ms) % total_cycles; + + //calculate scroll start_position + if (current_cycle < total_cycles/2) { + //move to the left + start_position = current_cycle - message_scroll_delay; + } else { + //move to the right + start_position = total_cycles - current_cycle; + } + start_position = constrain_int16(start_position, 0, chars_to_scroll); + uint8_t end_position = start_position + MSP_TXT_VISIBLE_CHARS; + + //ensure array boundaries + start_position = MIN(start_position, int(sizeof(buffer)-1)); + end_position = MIN(end_position, int(sizeof(buffer)-1)); + + //trim invisible part + buffer[end_position] = 0; + } + + sbuf_write_data(dst, buffer + start_position, strlen(buffer + start_position)); // max MSP_TXT_VISIBLE_CHARS chars general text... + } else { + if (_msp.wind_en > 0) { + telem_update_wind(_msp.telemetry_info); + } else { + telem_update_flight_mode(_msp.telemetry_info); + } + sbuf_write_data(dst, _msp.telemetry_info.flight_mode_str, MSP_TXT_BUFFER_SIZE); // rendered as up to MSP_TXT_VISIBLE_CHARS chars with UTF8 support + } + } + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_status(sbuf_t *dst) +{ + uint32_t mode_bitmask = get_osd_flight_mode_bitmask(); + sbuf_write_u16(dst, 0); // task delta time + sbuf_write_u16(dst, 0); // I2C error count + sbuf_write_u16(dst, 0); // sensor status + sbuf_write_data(dst, &mode_bitmask, 4); // unconditional part of flags, first 32 bits + sbuf_write_u8(dst, 0); + + sbuf_write_u16(dst, constrain_int16(0, 0, 100)); //system load + sbuf_write_u16(dst, 0); // gyro cycle time + + // Cap BoxModeFlags to 32 bits + sbuf_write_u8(dst, 0); + + // Write arming disable flags + sbuf_write_u8(dst, 1); + sbuf_write_u32(dst, !AP::notify().flags.armed); + + // Extra flags + sbuf_write_u8(dst, 0); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_osd_config(sbuf_t *dst) +{ + sbuf_write_u8(dst, OSD_FLAGS_OSD_FEATURE); // flags + sbuf_write_u8(dst, 0); // video system + // Configuration + sbuf_write_u8(dst, _msp.units); // units + // Alarms + sbuf_write_u8(dst, _msp.osd_config.rssi_alarm); // rssi alarm + sbuf_write_u16(dst, _msp.osd_config.cap_alarm); // capacity alarm + // Reuse old timer alarm (U16) as OSD_ITEM_COUNT + sbuf_write_u8(dst, 0); + sbuf_write_u8(dst, OSD_ITEM_COUNT); // osd items count + + sbuf_write_u16(dst, _msp.osd_config.alt_alarm); // altitude alarm + + // element position and visibility + uint16_t pos = 0; + for (uint8_t i = 0; i < OSD_ITEM_COUNT; i++) { + pos = MSP_OSD_POS_HIDDEN; + if (_msp.osd_item_settings[i] != nullptr) { // ok supported + if (_msp.osd_item_settings[i]->enabled) { // ok enabled + // let's check if we need to hide this dynamically + if (!BIT_IS_SET(osd_hidden_items_bitmask, i)) { + pos = MSP_OSD_POS(_msp.osd_item_settings[i]); + } + } + } + sbuf_write_u16(dst, pos); + } + + // post flight statistics + sbuf_write_u8(dst, OSD_STAT_COUNT); // stats items count + for (uint8_t i = 0; i < OSD_STAT_COUNT; i++ ) { + if ( _msp.osd_enabled_stats[i] >= 0) { + // no stats support yet + sbuf_write_u16(dst, 0); + } else { + // hide this OSD element + sbuf_write_u16(dst, 0); + } + } + + // timers + sbuf_write_u8(dst, OSD_TIMER_COUNT); // timers + for (uint8_t i = 0; i < OSD_TIMER_COUNT; i++) { + // no timer support + sbuf_write_u16(dst, 0); + } + + // Enabled warnings + // API < 1.41 + // Send low word first for backwards compatibility + sbuf_write_u16(dst, (uint16_t)(_msp.osd_config.enabled_warnings & 0xFFFF)); // Enabled warnings + // API >= 1.41 + // Send the warnings count and 32bit enabled warnings flags. + // Add currently active OSD profile (0 indicates OSD profiles not available). + // Add OSD stick overlay mode (0 indicates OSD stick overlay not available). + sbuf_write_u8(dst, OSD_WARNING_COUNT); // warning count + sbuf_write_u32(dst, _msp.osd_config.enabled_warnings); // enabled warning + + // If the feature is not available there is only 1 profile and it's always selected + sbuf_write_u8(dst, 1); // available profiles + sbuf_write_u8(dst, 1); // selected profile + + sbuf_write_u8(dst, 0); // OSD stick overlay + + // API >= 1.43 + // Add the camera frame element width/height + //sbuf_write_u8(dst, osdConfig()->camera_frame_width); + //sbuf_write_u8(dst, osdConfig()->camera_frame_height); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_attitude(sbuf_t *dst) +{ + telem_update_attitude(_msp.telemetry_info); + + sbuf_write_u16(dst, (int16_t)(_msp.telemetry_info.roll_cd * 0.1)); // centidegress to decidegrees -1800,1800 + sbuf_write_u16(dst, (int16_t)(_msp.telemetry_info.pitch_cd * 0.1)); // centidegress to decidegrees -1800,1800 + sbuf_write_u16(dst, (int16_t)_msp.telemetry_info.yaw_deg); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst) +{ + telem_update_home_pos(_msp.telemetry_info); + + sbuf_write_u32(dst, _msp.telemetry_info.rel_altitude_cm); // relative altitude cm + sbuf_write_u16(dst, (int16_t)_msp.telemetry_info.vspeed_ms * 100); // climb rate cm/s + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst) +{ + telem_update_battery_state(_msp.telemetry_info); + AP_RSSI* _rssi = AP::rssi(); + + sbuf_write_u8(dst, (uint8_t)constrain_int16(_msp.telemetry_info.batt_voltage_v * 10, 0, 255)); // battery voltage V to dV + sbuf_write_u16(dst, constrain_int16(_msp.telemetry_info.batt_consumed_mah, 0, 0xFFFF)); // milliamp hours drawn from battery + sbuf_write_u16(dst, _rssi->enabled() ? _rssi->read_receiver_rssi() * 1023 : 0); // rssi 0-1 to 0-1023 + sbuf_write_u16(dst, constrain_int16(_msp.telemetry_info.batt_current_a * 100, -0x8000, 0x7FFF)); // current A to cA (0.01 steps, range is -320A to 320A) + sbuf_write_u16(dst, constrain_int16(_msp.telemetry_info.batt_voltage_v * 100,0,0xFFFF)); // battery voltage in 0.01V steps + + return true; +} + + +bool AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst) +{ + telem_update_battery_state(_msp.telemetry_info); + + // battery characteristics + sbuf_write_u8(dst, (uint8_t)constrain_int16((_msp.cellcount > 0 ? _msp.cellcount : _msp.telemetry_info.batt_cellcount), 0, 255)); // cell count 0 indicates battery not detected. + sbuf_write_u16(dst, _msp.telemetry_info.batt_capacity_mah); // in mAh + + // battery state + sbuf_write_u8(dst, (uint8_t)constrain_int16(_msp.telemetry_info.batt_voltage_v * 10, 0, 255)); // battery voltage V to dV + sbuf_write_u16(dst, (uint16_t)MIN(_msp.telemetry_info.batt_consumed_mah, 0xFFFF)); // milliamp hours drawn from battery + sbuf_write_u16(dst, constrain_int16(_msp.telemetry_info.batt_current_a * 100, -0x8000, 0x7FFF)); // current A to cA (0.01 steps, range is -320A to 320A) + + // battery alerts + sbuf_write_u8(dst, _msp.telemetry_info.batt_state); // BATTERY: OK=0, CRITICAL=2 + + sbuf_write_u16(dst, constrain_int16(_msp.telemetry_info.batt_voltage_v * 100, 0, 0x7FFF)); // battery voltage in 0.01V steps + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst) +{ +#ifdef HAVE_AP_BLHELI_SUPPORT + AP_BLHeli *blheli = AP_BLHeli::get_singleton(); + if (blheli) { + AP_BLHeli::telem_data td; + sbuf_write_u8(dst, blheli->get_num_motors()); + for (uint8_t i = 0; i < blheli->get_num_motors(); i++) { + if (blheli->get_telem_data(i, td)) { + sbuf_write_u8(dst, td.temperature); // deg + sbuf_write_u16(dst, td.rpm * 0.01); // eRpm to 0.01 eRpm + } + } + } +#endif + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) +{ + telem_update_localtime(_msp.telemetry_info); + + sbuf_write_u16(dst, _msp.telemetry_info.localtime_tm.tm_year + 1900); // tm_year is relative to year 1900 + sbuf_write_u8(dst, _msp.telemetry_info.localtime_tm.tm_mon + 1); // MSP requires [1-12] months + sbuf_write_u8(dst, _msp.telemetry_info.localtime_tm.tm_mday); + sbuf_write_u8(dst, _msp.telemetry_info.localtime_tm.tm_hour); + sbuf_write_u8(dst, _msp.telemetry_info.localtime_tm.tm_min); + sbuf_write_u8(dst, _msp.telemetry_info.localtime_tm.tm_sec); + sbuf_write_u16(dst, 0); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) +{ + RCMapper* rcmap = AP::rcmap(); + uint16_t values[16] = {}; + rc().get_radio_in(values, ARRAY_SIZE(values)); + + // send only 4 channels, MSP order is AERT + sbuf_write_u16(dst, values[rcmap->roll()]); // A + sbuf_write_u16(dst, values[rcmap->pitch()]); // E + sbuf_write_u16(dst, values[rcmap->yaw()]); // R + sbuf_write_u16(dst, values[rcmap->throttle()]); // T + + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_board_info(sbuf_t *dst) +{ + const AP_FWVersion &fwver = AP::fwversion(); + + sbuf_write_data(dst, "ARDU", BOARD_IDENTIFIER_LENGTH); + sbuf_write_u16(dst, 0); + sbuf_write_u8(dst, 0); + sbuf_write_u8(dst, 0); + sbuf_write_u8(dst, strlen(fwver.fw_string)); + sbuf_write_data(dst, fwver.fw_string, strlen(fwver.fw_string)); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_build_info(sbuf_t *dst) +{ + const AP_FWVersion &fwver = AP::fwversion(); + + sbuf_write_data(dst, __DATE__, BUILD_DATE_LENGTH); + sbuf_write_data(dst, __TIME__, BUILD_TIME_LENGTH); + sbuf_write_data(dst, fwver.fw_hash_str, GIT_SHORT_REVISION_LENGTH); + return true; +} + +bool AP_MSP_Telem_Backend::msp_process_out_uid(sbuf_t *dst) +{ + sbuf_write_u32(dst, 0xAABBCCDD); + sbuf_write_u32(dst, 0xAABBCCDD); + sbuf_write_u32(dst, 0xAABBCCDD); + return true; +} + +void AP_MSP_Telem_Backend::flash_osd_items(void) +{ + const AP_Notify *notify = AP_Notify::get_singleton(); + // flash satcount when no 3D Fix + if (_msp.telemetry_info.gps_fix_type < AP_GPS::GPS_Status::GPS_OK_FIX_3D) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SATS); + } + } else { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS); + } + // flash home dir and distance if home is not set + if (!_msp.telemetry_info.home_is_set) { + if (_msp.flashing_on) { + BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIR); + BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIST); + } else { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST); + } + } else { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST); + } + // flash airspeed if there's no estimate + if (_msp.airspeed_en) { + if (!_msp.telemetry_info.airspeed_have_estimate) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SPEED); + } + } else { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED); + } + } +#ifdef ENABLE_MSG_FLASH_ON_FAILSAFE + // flash flightmode on failsafe (even if messages are scrolling?) + if (notify->flags.failsafe_battery || notify->flags.failsafe_gcs || notify->flags.failsafe_radio || notify->flags.ekf_bad) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + } + } else { +#endif //ENABLE_MSG_FLASH_ON_FAILSAFE + // flash text flightmode for 3secs after each change + if (_msp.flight_mode_focus) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + } + } else { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + } +#ifdef ENABLE_MSG_FLASH_ON_FAILSAFE + } //ENABLE_MSG_FLASH_ON_FAILSAFE +#endif + // flash battery on failsafe + if (notify->flags.failsafe_battery) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE); + BIT_SET(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE); + } + } + // flash rtc if no time available + if (_msp.telemetry_info.localtime_tm.tm_year == 0) { + if (_msp.flashing_on) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_RTC_DATETIME); + } else { + BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME); + } + } +} + +void AP_MSP_Telem_Backend::apply_osd_items_overrides(void) +{ +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.h b/libraries/AP_MSP/AP_MSP_Telem_Backend.h new file mode 100644 index 0000000000000..25ef4c51ac554 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.h @@ -0,0 +1,186 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include +#include +#include "msp.h" + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +#define MSP_TIME_SLOT_MAX 12 +#define CELLFULL 4.35 +#define MSP_TXT_BUFFER_SIZE 14U // 11 + 3 utf8 chars +#define MSP_TXT_VISIBLE_CHARS 12U +#define MSP_ARROW_COUNT 8 + +using namespace MSP; + +class AP_MSP; + +class AP_MSP_Telem_Backend : AP_RCTelemetry +{ +public: + AP_MSP_Telem_Backend(AP_MSP &msp, uint8_t protocol_instance, bool scheduler_enabled); + + ~AP_MSP_Telem_Backend(); + + typedef struct telemetry_info_s { + int32_t roll_cd; + int32_t pitch_cd; + float yaw_deg; + + bool home_is_set; + float home_bearing_cd; + uint32_t home_distance_m; + + float vspeed_ms; + int32_t rel_altitude_cm; + + float batt_current_a; + float batt_consumed_mah; + float batt_voltage_v; + int32_t batt_capacity_mah; + uint8_t batt_cellcount; + battery_state_e batt_state; + + int32_t gps_latitude; + int32_t gps_longitude; + uint8_t gps_num_sats; + int32_t gps_altitude_cm; + float gps_speed_ms; + uint16_t gps_ground_course_cd; + uint8_t gps_fix_type; + float airspeed_estimate_ms; + bool airspeed_have_estimate; + + char flight_mode_str[MSP_TXT_BUFFER_SIZE+1]; // allow space for the terminator + + tm localtime_tm; // year is relative to 1900 + } telemetry_info_t; + + // init - perform required initialisation + virtual bool init() override; + // scheduler + void run_wfq_scheduler(); + + // telemetry helpers + static uint8_t calc_cell_count(float battery_voltage); + static float get_vspeed_ms(void); + static void telem_update_home_pos(telemetry_info_t &_telemetry); + static void telem_update_battery_state(telemetry_info_t &_telemetry); + static void telem_update_gps_state(telemetry_info_t &_telemetry); + static void telem_update_attitude(telemetry_info_t &_telemetry); + static void telem_update_airspeed(telemetry_info_t &_telemetry); + static void telem_update_flight_mode(telemetry_info_t &_telemetry); + static void telem_update_localtime(telemetry_info_t &_telemetry); + static void telem_update_wind(telemetry_info_t &_telemetry); + + // implementation specific helpers + virtual uint32_t get_osd_flight_mode_bitmask(void) = 0; + virtual void flash_osd_items(void); + virtual void apply_osd_items_overrides(void); + virtual void enable_warnings(); + + // implementation specific MSP out command processing + virtual bool msp_process_out_api_version(sbuf_t *dst) = 0; + virtual bool msp_process_out_fc_version(sbuf_t *dst) = 0; + virtual bool msp_process_out_fc_variant(sbuf_t *dst) = 0; + virtual bool msp_process_out_uid(sbuf_t *dst); + + virtual bool msp_process_out_board_info(sbuf_t *dst); + virtual bool msp_process_out_build_info(sbuf_t *dst); + virtual bool msp_process_out_name(sbuf_t *dst); + virtual bool msp_process_out_status(sbuf_t *dst); + virtual bool msp_process_out_osd_config(sbuf_t *dst); + virtual bool msp_process_out_raw_gps(sbuf_t *dst); + virtual bool msp_process_out_comp_gps(sbuf_t *dst); + virtual bool msp_process_out_attitude(sbuf_t *dst); + virtual bool msp_process_out_altitude(sbuf_t *dst); + virtual bool msp_process_out_analog(sbuf_t *dst); + virtual bool msp_process_out_battery_state(sbuf_t *dst); + virtual bool msp_process_out_esc_sensor_data(sbuf_t *dst); + virtual bool msp_process_out_rtc(sbuf_t *dst); + virtual bool msp_process_out_rc(sbuf_t *dst); + +protected: + enum msp_packet_type : uint8_t { + EMPTY_SLOT = 0, + NAME, + STATUS, + CONFIG, + RAW_GPS, + COMP_GPS, + ATTITUDE, + ALTITUDE, + ANALOG, + BATTERY_STATE, + ESC_SENSOR_DATA, + RTC_DATETIME, + }; + + const uint16_t msp_packet_type_map[MSP_TIME_SLOT_MAX] = { + 0, + MSP_NAME, + MSP_STATUS, + MSP_OSD_CONFIG, + MSP_RAW_GPS, + MSP_COMP_GPS, + MSP_ATTITUDE, + MSP_ALTITUDE, + MSP_ANALOG, + MSP_BATTERY_STATE, + MSP_ESC_SENSOR_DATA, + MSP_RTC + }; + /* UTF-8 encodings + U+2191 ↑ e2 86 91 UPWARDS ARROW + U+2197 ↗ e2 86 97 NORTH EAST ARROW + U+2192 → e2 86 92 RIGHTWARDS ARROW + U+2198 ↘ e2 86 98 SOUTH EAST ARROW + U+2193 ↓ e2 86 93 DOWNWARDS ARROW + U+2199 ↙ e2 86 99 SOUTH WEST ARROW + U+2190 ← e2 86 90 LEFTWARDS ARROW + U+2196 ↖ e2 86 96 NORTH WEST ARROW + */ + static constexpr uint8_t arrows[8] = {0x91, 0x97, 0x92, 0x98, 0x93, 0x99, 0x90, 0x96}; + + //static const uint8_t message_visible_width = 12; + static const uint8_t message_scroll_time_ms = 200; + static const uint8_t message_scroll_delay = 5; + + // reference to creator for callback + AP_MSP& _msp; + // should we have push type telemetry + bool _scheduler_enabled; + // serial port instance + uint8_t _protocol_instance; + // each backend can hide/unhide items dynamically + uint64_t osd_hidden_items_bitmask; + + + // passthrough WFQ scheduler + bool is_packet_ready(uint8_t idx, bool queue_empty) override; + void process_packet(uint8_t idx) override; + void adjust_packet_weight(bool queue_empty) override; + void setup_wfq_scheduler(void) override; + bool get_next_msg_chunk(void) override; +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp new file mode 100644 index 0000000000000..30535319652d7 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp @@ -0,0 +1,168 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + The DJI Air Unit polls the FC for the following MSP messages at around 4Hz. + Note: messages are polled in ascending hex id order. + + Hex|Dec|Name + --------------------------- + 03 03 MSP_FC_VERSION + 0a 10 MSP_NAME + 54 84 MSP_OSD_CONFIG + 5c 92 MSP_FILTER_CONFIG + 5e 94 MSP_PID_ADVANCED + 65 101 MSP_STATUS + 69 105 MSP_RC + 6a 106 MSP_RAW_GPS + 6b 107 MSP_COMP_GPS + 6c 108 MSP_ATTITUDE + 6d 109 MSP_ALTITUDE + 6e 110 MSP_ANALOG + 6f 111 MSP_RC_TUNING + 70 112 MSP_PID + 82 130 MSP_BATTERY_STATE + 86 134 MSP_ESC_SENSOR_DATA + 96 150 MSP_STATUS_EX + f7 247 MSP_RTC +*/ +#include +#include + +#include "AP_MSP.h" +#include "AP_MSP_Telem_DJI.h" + +//#define ENABLE_RESCUE_MODE_DISPLAY +#if HAL_MSP_ENABLED +extern const AP_HAL::HAL& hal; + +AP_MSP_Telem_DJI::AP_MSP_Telem_DJI(AP_MSP& msp, uint8_t protocol_instance, bool scheduler_enabled) : AP_MSP_Telem_Backend(msp, protocol_instance, scheduler_enabled) +{ +} + +AP_MSP_Telem_DJI::~AP_MSP_Telem_DJI(void) +{ +} + +uint32_t AP_MSP_Telem_DJI::get_osd_flight_mode_bitmask(void) +{ + uint32_t mode_mask = 0; + // default is hide the DJI flightmode widget + BIT_SET(osd_hidden_items_bitmask, OSD_FLYMODE); + const AP_Notify& _notify = AP::notify(); + bool force_visible = false; + + // set arming status + if (_notify.flags.armed) { + BIT_SET(mode_mask, DJI_MODE_ARM); + } + + // check failsafe + if (_notify.flags.failsafe_battery || _notify.flags.failsafe_gcs || _notify.flags.failsafe_radio || _notify.flags.ekf_bad ) { + BIT_SET(mode_mask, DJI_MODE_FS); + force_visible = true; + } + +#ifdef ENABLE_RESCUE_MODE_DISPLAY +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + switch (AP::notify().flags.flight_mode) { + case 11: // RTL + case 21: // QRTL + BIT_SET(mode_mask, DJI_MODE_RESC); + force_visible = true; + break; + default: + break; + } +#endif +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) + switch (AP::notify().flags.flight_mode) { + case 6: // RTL + case 21: // SMART RTL + BIT_SET(mode_mask, DJI_MODE_RESC); + force_visible = true; + break; + default: + break; + } +#endif +#if APM_BUILD_TYPE(APM_BUILD_Rover) + switch (AP::notify().flags.flight_mode) { + case 11: // RTL + case 12: // SMART RTL + BIT_SET(mode_mask, DJI_MODE_RESC); + force_visible = true; + break; + default: + break; + } +#endif +#endif //ENABLE_RESCUE_MODE_DISPLAY + // do we need to show the DJI flight mode? + if (force_visible) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_FLYMODE); + } + return mode_mask; +} + +bool AP_MSP_Telem_DJI::msp_process_out_status(sbuf_t *dst) +{ + // DJI OSD relies on a statically defined bit order + // We need a special get_osd_flight_mode_bitmask() + uint32_t mode_bitmask = get_osd_flight_mode_bitmask(); + sbuf_write_u16(dst, 0); // task delta time + sbuf_write_u16(dst, 0); // I2C error count + sbuf_write_u16(dst, 0); // sensor status + sbuf_write_data(dst, &mode_bitmask, 4); // unconditional part of flags, first 32 bits + sbuf_write_u8(dst, 0); + + sbuf_write_u16(dst, constrain_int16(0, 0, 100)); //system load + sbuf_write_u16(dst, 0); // gyro cycle time + + // Cap BoxModeFlags to 32 bits + sbuf_write_u8(dst, 0); + + // Write arming disable flags + sbuf_write_u8(dst, 1); + sbuf_write_u32(dst, !AP::notify().flags.armed); + + // Extra flags + sbuf_write_u8(dst, 0); + return true; +} + +bool AP_MSP_Telem_DJI::msp_process_out_api_version(sbuf_t *dst) +{ + sbuf_write_u8(dst, MSP_PROTOCOL_VERSION); + sbuf_write_u8(dst, API_VERSION_MAJOR); + sbuf_write_u8(dst, API_VERSION_MINOR); + return true; +} + +bool AP_MSP_Telem_DJI::msp_process_out_fc_version(sbuf_t *dst) +{ + sbuf_write_u8(dst, FC_VERSION_MAJOR); + sbuf_write_u8(dst, FC_VERSION_MINOR); + sbuf_write_u8(dst, FC_VERSION_PATCH_LEVEL); + return true; +} + +bool AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst) +{ + sbuf_write_data(dst, "BTFL", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); + return true; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.h b/libraries/AP_MSP/AP_MSP_Telem_DJI.h new file mode 100644 index 0000000000000..efd29a9a762b3 --- /dev/null +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.h @@ -0,0 +1,45 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_MSP_Telem_Backend.h" + +#if HAL_MSP_ENABLED + +class AP_MSP_Telem_DJI : public AP_MSP_Telem_Backend +{ +public: + AP_MSP_Telem_DJI(AP_MSP& msp, uint8_t protocol_instance, bool scheduler_enabled); + + ~AP_MSP_Telem_DJI(); + + // implementation specific helpers + uint32_t get_osd_flight_mode_bitmask(void) override; + bool msp_process_out_api_version(sbuf_t *dst) override; + bool msp_process_out_fc_version(sbuf_t *dst) override; + bool msp_process_out_fc_variant(sbuf_t *dst) override; + bool msp_process_out_status(sbuf_t *dst) override; + + enum { + DJI_MODE_ARM = 0, + DJI_MODE_STAB, + DJI_MODE_HOR, + DJI_MODE_HEAD, + DJI_MODE_FS, + DJI_MODE_RESC, + } flmode_e; +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/msp.cpp b/libraries/AP_MSP/msp.cpp new file mode 100644 index 0000000000000..225c1f407aa56 --- /dev/null +++ b/libraries/AP_MSP/msp.cpp @@ -0,0 +1,297 @@ +#include +#include +#include + +#include "msp.h" + +#if HAL_MSP_ENABLED + +uint8_t MSP::msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, int len) +{ + while (len-- > 0) { + checksum ^= *data++; + } + return checksum; +} + +int MSP::msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, int hdr_len, const uint8_t * data, int data_len, const uint8_t * crc, int crc_len) +{ + if (msp->uart == nullptr) { + return 0; + } + + // We are allowed to send out the response if + // a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling; + // this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care) + // b) Response fits into TX buffer + const uint32_t total_frame_length = hdr_len + data_len + crc_len; + + if (!msp->uart->tx_pending() && (msp->uart->txspace() < total_frame_length)) { + return 0; + } + + // Transmit frame + msp->uart->write(hdr, hdr_len); + msp->uart->write(data, data_len); + msp->uart->write(crc, crc_len); + + return total_frame_length; +} + +int MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version) +{ + static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER; + const int data_len = sbuf_bytes_remaining(&packet->buf); + uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], static_cast(packet->result == MSP_RESULT_ERROR ? '!' : '>')}; + uint8_t crc_buf[2]; + int hdr_len = 3; + int crc_len = 0; + +#define V1_CHECKSUM_STARTPOS 3 + if (msp_version == MSP_V1) { + msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len]; + hdr_len += sizeof(msp_header_v1_t); + hdr_v1->cmd = packet->cmd; + + // Add JUMBO-frame header if necessary + if (data_len >= JUMBO_FRAME_SIZE_LIMIT) { + msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len]; + hdr_len += sizeof(msp_header_jumbo_t); + + hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT; + hdr_jumbo->size = data_len; + } else { + hdr_v1->size = data_len; + } + + // Pre-calculate CRC + crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS); + crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len); + crc_len++; + } else if (msp_version == MSP_V2_OVER_V1) { + msp_header_v1_t * hdr_v1 = (msp_header_v1_t *)&hdr_buf[hdr_len]; + + hdr_len += sizeof(msp_header_v1_t); + + msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len]; + hdr_len += sizeof(msp_header_v2_t); + + const int v1_payload_size = sizeof(msp_header_v2_t) + data_len + 1; // MSPv2 header + data payload + MSPv2 checksum + hdr_v1->cmd = MSP_V2_FRAME_ID; + + // Add JUMBO-frame header if necessary + if (v1_payload_size >= JUMBO_FRAME_SIZE_LIMIT) { + msp_header_jumbo_t * hdr_jumbo = (msp_header_jumbo_t *)&hdr_buf[hdr_len]; + hdr_len += sizeof(msp_header_jumbo_t); + + hdr_v1->size = JUMBO_FRAME_SIZE_LIMIT; + hdr_jumbo->size = v1_payload_size; + } else { + hdr_v1->size = v1_payload_size; + } + + // Fill V2 header + hdr_v2->flags = packet->flags; + hdr_v2->cmd = packet->cmd; + hdr_v2->size = data_len; + + // V2 CRC: only V2 header + data payload + crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t)); + crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len); + crc_len++; + + // V1 CRC: All headers + data payload + V2 CRC byte + crc_buf[crc_len] = msp_serial_checksum_buf(0, hdr_buf + V1_CHECKSUM_STARTPOS, hdr_len - V1_CHECKSUM_STARTPOS); + crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len); + crc_buf[crc_len] = msp_serial_checksum_buf(crc_buf[crc_len], crc_buf, crc_len); + crc_len++; + } else if (msp_version == MSP_V2_NATIVE) { + msp_header_v2_t * hdr_v2 = (msp_header_v2_t *)&hdr_buf[hdr_len]; + hdr_len += sizeof(msp_header_v2_t); + + hdr_v2->flags = packet->flags; + hdr_v2->cmd = packet->cmd; + hdr_v2->size = data_len; + + crc_buf[crc_len] = crc8_dvb_s2_update(0, (uint8_t *)hdr_v2, sizeof(msp_header_v2_t)); + crc_buf[crc_len] = crc8_dvb_s2_update(crc_buf[crc_len], sbuf_ptr(&packet->buf), data_len); + crc_len++; + } else { + // Shouldn't get here + return 0; + } + + // Send the frame + return msp_serial_send_frame(msp, hdr_buf, hdr_len, sbuf_ptr(&packet->buf), data_len, crc_buf, crc_len); +} + +bool MSP::msp_parse_received_data(msp_port_t *msp, uint8_t c) +{ + switch (msp->c_state) { + default: + case MSP_IDLE: // Waiting for '$' character + if (c == '$') { + msp->msp_version = MSP_V1; + msp->c_state = MSP_HEADER_START; + } else { + return false; + } + break; + + case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native) + switch (c) { + case 'M': + msp->c_state = MSP_HEADER_M; + break; + case 'X': + msp->c_state = MSP_HEADER_X; + break; + default: + msp->c_state = MSP_IDLE; + break; + } + break; + + case MSP_HEADER_M: // Waiting for '<' + if (c == '<') { + msp->offset = 0; + msp->checksum1 = 0; + msp->checksum2 = 0; + msp->c_state = MSP_HEADER_V1; + } else { + msp->c_state = MSP_IDLE; + } + break; + + case MSP_HEADER_X: + if (c == '<') { + msp->offset = 0; + msp->checksum2 = 0; + msp->msp_version = MSP_V2_NATIVE; + msp->c_state = MSP_HEADER_V2_NATIVE; + } else { + msp->c_state = MSP_IDLE; + } + break; + + case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable + msp->in_buf[msp->offset++] = c; + msp->checksum1 ^= c; + if (msp->offset == sizeof(msp_header_v1_t)) { + msp_header_v1_t * hdr = (msp_header_v1_t *)&msp->in_buf[0]; + // Check incoming buffer size limit + if (hdr->size > MSP_PORT_INBUF_SIZE) { + msp->c_state = MSP_IDLE; + } else if (hdr->cmd == MSP_V2_FRAME_ID) { + // MSPv1 payload must be big enough to hold V2 header + extra checksum + if (hdr->size >= sizeof(msp_header_v2_t) + 1) { + msp->msp_version = MSP_V2_OVER_V1; + msp->c_state = MSP_HEADER_V2_OVER_V1; + } else { + msp->c_state = MSP_IDLE; + } + } else { + msp->data_size = hdr->size; + msp->cmd_msp = hdr->cmd; + msp->cmd_flags = 0; + msp->offset = 0; // re-use buffer + msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte + } + } + break; + + case MSP_PAYLOAD_V1: + msp->in_buf[msp->offset++] = c; + msp->checksum1 ^= c; + if (msp->offset == msp->data_size) { + msp->c_state = MSP_CHECKSUM_V1; + } + break; + + case MSP_CHECKSUM_V1: + if (msp->checksum1 == c) { + msp->c_state = MSP_COMMAND_RECEIVED; + } else { + msp->c_state = MSP_IDLE; + } + break; + + case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now + msp->in_buf[msp->offset++] = c; + msp->checksum1 ^= c; + msp->checksum2 = crc8_dvb_s2(msp->checksum2, c); + if (msp->offset == (sizeof(msp_header_v2_t) + sizeof(msp_header_v1_t))) { + msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[sizeof(msp_header_v1_t)]; + msp->data_size = hdrv2->size; + + // Check for potential buffer overflow + if (hdrv2->size > MSP_PORT_INBUF_SIZE) { + msp->c_state = MSP_IDLE; + } else { + msp->cmd_msp = hdrv2->cmd; + msp->cmd_flags = hdrv2->flags; + msp->offset = 0; // re-use buffer + msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1; + } + } + break; + + case MSP_PAYLOAD_V2_OVER_V1: + msp->checksum2 = crc8_dvb_s2(msp->checksum2, c); + msp->checksum1 ^= c; + msp->in_buf[msp->offset++] = c; + + if (msp->offset == msp->data_size) { + msp->c_state = MSP_CHECKSUM_V2_OVER_V1; + } + break; + + case MSP_CHECKSUM_V2_OVER_V1: + msp->checksum1 ^= c; + if (msp->checksum2 == c) { + msp->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum + } else { + msp->c_state = MSP_IDLE; + } + break; + + case MSP_HEADER_V2_NATIVE: + msp->in_buf[msp->offset++] = c; + msp->checksum2 = crc8_dvb_s2(msp->checksum2, c); + if (msp->offset == sizeof(msp_header_v2_t)) { + msp_header_v2_t * hdrv2 = (msp_header_v2_t *)&msp->in_buf[0]; + + // Check for potential buffer overflow + if (hdrv2->size > MSP_PORT_INBUF_SIZE) { + msp->c_state = MSP_IDLE; + } else { + msp->data_size = hdrv2->size; + msp->cmd_msp = hdrv2->cmd; + msp->cmd_flags = hdrv2->flags; + msp->offset = 0; // re-use buffer + msp->c_state = msp->data_size > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE; + } + } + break; + + case MSP_PAYLOAD_V2_NATIVE: + msp->checksum2 = crc8_dvb_s2(msp->checksum2, c); + msp->in_buf[msp->offset++] = c; + + if (msp->offset == msp->data_size) { + msp->c_state = MSP_CHECKSUM_V2_NATIVE; + } + break; + + case MSP_CHECKSUM_V2_NATIVE: + if (msp->checksum2 == c) { + msp->c_state = MSP_COMMAND_RECEIVED; + } else { + msp->c_state = MSP_IDLE; + } + break; + } + return true; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/msp.h b/libraries/AP_MSP/msp.h new file mode 100644 index 0000000000000..006025ae437a4 --- /dev/null +++ b/libraries/AP_MSP/msp.h @@ -0,0 +1,142 @@ +#pragma once + +#include +#include +#include "msp_protocol.h" + +#include "msp_sbuf.h" +#include "msp_version.h" +#include "msp_osd.h" + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +// betaflight/src/main/common/utils.h +#define MSP_ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) +#define MSP_ARRAYEND(x) (&(x)[MSP_ARRAYLEN(x)]) +#define MSP_UNUSED(x) (void)(x) +// betaflioght/src/main/msp/msp_serial.c +#define JUMBO_FRAME_SIZE_LIMIT 255 +// betaflioght/src/main/msp/msp.h +#define MSP_V2_FRAME_ID 255 +#define MSP_VERSION_MAGIC_INITIALIZER { 'M', 'M', 'X' } +#define MSP_PORT_INBUF_SIZE 192 +#define MSP_PORT_OUTBUF_SIZE 512 +#define MSP_MAX_HEADER_SIZE 9 +// betaflight/src/main/msp/msp_protocol_v2_sensor.h +#define MSP2_IS_SENSOR_MESSAGE(x) ((x) >= 0x1F00 && (x) <= 0x1FFF) +#define MSP2_SENSOR_RANGEFINDER 0x1F01 +#define MSP2_SENSOR_OPTIC_FLOW 0x1F02 + +class AP_MSP_Telem_Backend; + +namespace MSP +{ + +typedef enum { + MSP_V1 = 0, + MSP_V2_OVER_V1 = 1, + MSP_V2_NATIVE = 2, + MSP_VERSION_COUNT +} msp_version_e; + +// return positive for ACK, negative on error, zero for no reply +typedef enum { + MSP_RESULT_ACK = 1, + MSP_RESULT_ERROR = -1, + MSP_RESULT_NO_REPLY = 0 +} msp_result_e; + +typedef struct msp_packet_s { + sbuf_t buf; + int16_t cmd; + uint8_t flags; + int16_t result; +} msp_packet_t; + +typedef enum { + MSP_FLAG_DONT_REPLY = (1 << 0), +} msp_flags_e; + +typedef enum { + MSP_IDLE, + MSP_HEADER_START, + MSP_HEADER_M, + MSP_HEADER_X, + + MSP_HEADER_V1, + MSP_PAYLOAD_V1, + MSP_CHECKSUM_V1, + + MSP_HEADER_V2_OVER_V1, + MSP_PAYLOAD_V2_OVER_V1, + MSP_CHECKSUM_V2_OVER_V1, + + MSP_HEADER_V2_NATIVE, + MSP_PAYLOAD_V2_NATIVE, + MSP_CHECKSUM_V2_NATIVE, + + MSP_COMMAND_RECEIVED +} msp_state_e; + +typedef struct PACKED { + uint8_t size; + uint8_t cmd; +} msp_header_v1_t; + +typedef struct PACKED { + uint16_t size; +} msp_header_jumbo_t; + +typedef struct PACKED { + uint8_t flags; + uint16_t cmd; + uint16_t size; +} msp_header_v2_t; + +typedef struct msp_port_s { + AP_SerialManager::SerialProtocol protocol; + AP_MSP_Telem_Backend *telem_backend; + AP_HAL::UARTDriver *uart; + uint32_t last_activity_ms; + msp_state_e c_state; + uint8_t in_buf[MSP_PORT_INBUF_SIZE]; + uint_fast16_t offset; + uint_fast16_t data_size; + msp_version_e msp_version; + uint8_t cmd_flags; + uint16_t cmd_msp; + uint8_t checksum1; + uint8_t checksum2; +} msp_port_t; + +typedef struct PACKED { + uint8_t quality; // [0;255] + int32_t distance_mm; // Negative value for out of range +} msp_rangefinder_sensor_t; + +typedef struct PACKED { + uint8_t quality; // [0;255] + int32_t motion_x; + int32_t motion_y; +} msp_opflow_sensor_t; + +// betaflight/src/main/sensors/battery.h +typedef enum : uint8_t { + MSP_BATTERY_OK = 0, + MSP_BATTERY_WARNING, + MSP_BATTERY_CRITICAL, + MSP_BATTERY_NOT_PRESENT, + MSP_BATTERY_INIT +} battery_state_e; + +uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, int len); +int msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, int hdr_len, const uint8_t * data, int data_len, const uint8_t * crc, int crc_len); +int msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version); +bool msp_parse_received_data(msp_port_t *msp, uint8_t c); +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/msp_osd.h b/libraries/AP_MSP/msp_osd.h new file mode 100644 index 0000000000000..91f80e1a52a2d --- /dev/null +++ b/libraries/AP_MSP/msp_osd.h @@ -0,0 +1,147 @@ +#pragma once + +#include + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +#define OSD_FLAGS_OSD_FEATURE (1 << 0) + +namespace MSP +{ +typedef enum { + OSD_RSSI_VALUE, + OSD_MAIN_BATT_VOLTAGE, + OSD_CROSSHAIRS, + OSD_ARTIFICIAL_HORIZON, + OSD_HORIZON_SIDEBARS, + OSD_ITEM_TIMER_1, + OSD_ITEM_TIMER_2, + OSD_FLYMODE, + OSD_CRAFT_NAME, + OSD_THROTTLE_POS, + OSD_VTX_CHANNEL, + OSD_CURRENT_DRAW, + OSD_MAH_DRAWN, + OSD_GPS_SPEED, + OSD_GPS_SATS, + OSD_ALTITUDE, + OSD_ROLL_PIDS, + OSD_PITCH_PIDS, + OSD_YAW_PIDS, + OSD_POWER, + OSD_PIDRATE_PROFILE, + OSD_WARNINGS, + OSD_AVG_CELL_VOLTAGE, + OSD_GPS_LON, + OSD_GPS_LAT, + OSD_DEBUG, + OSD_PITCH_ANGLE, + OSD_ROLL_ANGLE, + OSD_MAIN_BATT_USAGE, + OSD_DISARMED, + OSD_HOME_DIR, + OSD_HOME_DIST, + OSD_NUMERICAL_HEADING, + OSD_NUMERICAL_VARIO, + OSD_COMPASS_BAR, + OSD_ESC_TMP, + OSD_ESC_RPM, + OSD_REMAINING_TIME_ESTIMATE, + OSD_RTC_DATETIME, + OSD_ADJUSTMENT_RANGE, + OSD_CORE_TEMPERATURE, + OSD_ANTI_GRAVITY, + OSD_G_FORCE, + OSD_MOTOR_DIAG, + OSD_LOG_STATUS, + OSD_FLIP_ARROW, + OSD_LINK_QUALITY, + OSD_FLIGHT_DIST, + OSD_STICK_OVERLAY_LEFT, + OSD_STICK_OVERLAY_RIGHT, + OSD_DISPLAY_NAME, + OSD_ESC_RPM_FREQ, + OSD_RATE_PROFILE_NAME, + OSD_PID_PROFILE_NAME, + OSD_PROFILE_NAME, + OSD_RSSI_DBM_VALUE, + OSD_RC_CHANNELS, + OSD_CAMERA_FRAME, + OSD_ITEM_COUNT // MUST BE LAST +} osd_items_e; + +typedef enum { + OSD_STAT_RTC_DATE_TIME, + OSD_STAT_TIMER_1, + OSD_STAT_TIMER_2, + OSD_STAT_MAX_SPEED, + OSD_STAT_MAX_DISTANCE, + OSD_STAT_MIN_BATTERY, + OSD_STAT_END_BATTERY, + OSD_STAT_BATTERY, + OSD_STAT_MIN_RSSI, + OSD_STAT_MAX_CURRENT, + OSD_STAT_USED_MAH, + OSD_STAT_MAX_ALTITUDE, + OSD_STAT_BLACKBOX, + OSD_STAT_BLACKBOX_NUMBER, + OSD_STAT_MAX_G_FORCE, + OSD_STAT_MAX_ESC_TEMP, + OSD_STAT_MAX_ESC_RPM, + OSD_STAT_MIN_LINK_QUALITY, + OSD_STAT_FLIGHT_DISTANCE, + OSD_STAT_MAX_FFT, + OSD_STAT_TOTAL_FLIGHTS, + OSD_STAT_TOTAL_TIME, + OSD_STAT_TOTAL_DIST, + OSD_STAT_MIN_RSSI_DBM, + OSD_STAT_COUNT // MUST BE LAST +} osd_stats_e; + +typedef enum : uint8_t { + OSD_UNIT_IMPERIAL, + OSD_UNIT_METRIC +} osd_unit_e; + +typedef enum { + OSD_TIMER_1, + OSD_TIMER_2, + OSD_TIMER_COUNT +} osd_timer_e; + +typedef enum { + OSD_WARNING_ARMING_DISABLE, + OSD_WARNING_BATTERY_NOT_FULL, + OSD_WARNING_BATTERY_WARNING, + OSD_WARNING_BATTERY_CRITICAL, + OSD_WARNING_VISUAL_BEEPER, + OSD_WARNING_CRASH_FLIP, + OSD_WARNING_ESC_FAIL, + OSD_WARNING_CORE_TEMPERATURE, + OSD_WARNING_RC_SMOOTHING, + OSD_WARNING_FAIL_SAFE, + OSD_WARNING_LAUNCH_CONTROL, + OSD_WARNING_GPS_RESCUE_UNAVAILABLE, + OSD_WARNING_GPS_RESCUE_DISABLED, + OSD_WARNING_RSSI, + OSD_WARNING_LINK_QUALITY, + OSD_WARNING_RSSI_DBM, + OSD_WARNING_COUNT // MUST BE LAST +} osd_warnings_flags_e; + +typedef struct osd_config_s { + osd_unit_e units; + uint8_t rssi_alarm; + uint16_t cap_alarm; + uint16_t alt_alarm; + uint16_t timers[OSD_TIMER_COUNT]; + uint32_t enabled_stats; + uint32_t enabled_warnings; +} osd_config_t; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_BLHeli/msp_protocol.h b/libraries/AP_MSP/msp_protocol.h similarity index 85% rename from libraries/AP_BLHeli/msp_protocol.h rename to libraries/AP_MSP/msp_protocol.h index cda3bcfbff80f..a0380ac2a5234 100644 --- a/libraries/AP_BLHeli/msp_protocol.h +++ b/libraries/AP_MSP/msp_protocol.h @@ -1,24 +1,21 @@ /* - msp_protocol.h imported from betaflight for use in ArduPilot - - Many thanks to tbe Cleanflight and Betaflight developers for a great - reference implementation of this protocol - */ -/* - * This file is part of Cleanflight. + * This file is part of Cleanflight and Betaflight. * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . + * along with this software. + * + * If not, see . */ /** @@ -64,8 +61,8 @@ #define MSP_PROTOCOL_VERSION 0 -#define API_VERSION_MAJOR 1 // increment when major changes are made -#define API_VERSION_MINOR 37 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release) +#define API_VERSION_MAJOR 1 +#define API_VERSION_MINOR 42 // for compatibility with DJI OSD #define API_VERSION_LENGTH 2 @@ -165,10 +162,9 @@ #define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total) #define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP -// FIXME - Provided for backwards compatibility with configurator code until configurator is updated. // DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead. -#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere -#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save +// DEPRECATED - #define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere +// DEPRECATED - #define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save #define MSP_REBOOT 68 //in message reboot settings @@ -284,6 +280,11 @@ #define MSP_GPS_CONFIG 132 //out message GPS configuration #define MSP_COMPASS_CONFIG 133 //out message Compass configuration #define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM) +#define MSP_GPS_RESCUE 135 //out message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_GPS_RESCUE_PIDS 136 //out message GPS Rescues's throttleP and velocity PIDS + yaw P +#define MSP_VTXTABLE_BAND 137 //out message vtxTable band/channel data +#define MSP_VTXTABLE_POWERLEVEL 138 //out message vtxTable powerLevel data +#define MSP_MOTOR_TELEMETRY 139 //out message Per-motor telemetry data (RPM, packet stats, ESC temp, etc.) #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -310,6 +311,10 @@ #define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc) #define MSP_SET_GPS_CONFIG 223 //out message GPS configuration #define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration +#define MSP_SET_GPS_RESCUE 225 //in message GPS Rescues's angle, initialAltitude, descentDistance, rescueGroundSpeed, sanityChecks and minSats +#define MSP_SET_GPS_RESCUE_PIDS 226 //in message GPS Rescues's throttleP and velocity PIDS + yaw P +#define MSP_SET_VTXTABLE_BAND 227 //in message set vtxTable band/channel data (one band at a time) +#define MSP_SET_VTXTABLE_POWERLEVEL 228 //in message set vtxTable powerLevel data (one powerLevel at a time) // #define MSP_BIND 240 //in message no param // #define MSP_ALARMS 242 @@ -319,17 +324,21 @@ #define MSP_RESERVE_2 252 //reserved for system usage #define MSP_DEBUGMSG 253 //out message debug string buffer #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 -#define MSP_RESERVE_3 255 //reserved for system usage +#define MSP_V2_FRAME 255 //MSPv2 payload indicator // Additional commands that are not compatible with MultiWii #define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc #define MSP_UID 160 //out message Unique device ID #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) #define MSP_GPSSTATISTICS 166 //out message get GPS debugging data +#define MSP_MULTIPLE_MSP 230 //out message request multiple MSPs in one request - limit is the TX buffer; returns each MSP in the order they were requested starting with length of MSP; MSPs with input arguments are not supported +#define MSP_MODE_RANGES_EXTRA 238 //out message Reads the extra mode range data #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface +#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...) #define MSP_SET_RTC 246 //in message Sets the RTC clock #define MSP_RTC 247 //out message Gets the RTC clock +#define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board +#define MSP_SET_SIGNATURE 249 //in message Sets the signature of the board and serial number diff --git a/libraries/AP_MSP/msp_sbuf.cpp b/libraries/AP_MSP/msp_sbuf.cpp new file mode 100644 index 0000000000000..bbc46116ac161 --- /dev/null +++ b/libraries/AP_MSP/msp_sbuf.cpp @@ -0,0 +1,48 @@ +#include + +#include "msp_sbuf.h" + +#if HAL_MSP_ENABLED + +uint8_t* MSP::sbuf_ptr(sbuf_t *buf) +{ + return buf->ptr; +} + +int MSP::sbuf_bytes_remaining(const sbuf_t *buf) +{ + return buf->end - buf->ptr; +} + +void MSP::sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base) +{ + buf->end = buf->ptr; + buf->ptr = base; +} + +void MSP::sbuf_write_u8(sbuf_t *dst, uint8_t val) +{ + *dst->ptr++ = val; +} + +void MSP::sbuf_write_u16(sbuf_t *dst, uint16_t val) +{ + sbuf_write_u8(dst, val >> 0); + sbuf_write_u8(dst, val >> 8); +} + +void MSP::sbuf_write_u32(sbuf_t *dst, uint32_t val) +{ + sbuf_write_u8(dst, val >> 0); + sbuf_write_u8(dst, val >> 8); + sbuf_write_u8(dst, val >> 16); + sbuf_write_u8(dst, val >> 24); +} + +void MSP::sbuf_write_data(sbuf_t *dst, const void *data, int len) +{ + memcpy(dst->ptr, data, len); + dst->ptr += len; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/msp_sbuf.h b/libraries/AP_MSP/msp_sbuf.h new file mode 100644 index 0000000000000..a3f48efe1b7e4 --- /dev/null +++ b/libraries/AP_MSP/msp_sbuf.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +namespace MSP +{ +typedef struct sbuf_s { + uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *end; +} sbuf_t; + +//helper functions +uint8_t* sbuf_ptr(sbuf_t *buf); +int sbuf_bytes_remaining(const sbuf_t *buf); +void sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base); +void sbuf_write_u8(sbuf_t *dst, uint8_t val); +void sbuf_write_u16(sbuf_t *dst, uint16_t val); +void sbuf_write_u32(sbuf_t *dst, uint32_t val); +void sbuf_write_data(sbuf_t *dst, const void *data, int len); +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/msp_version.h b/libraries/AP_MSP/msp_version.h new file mode 100644 index 0000000000000..249686e40411b --- /dev/null +++ b/libraries/AP_MSP/msp_version.h @@ -0,0 +1,18 @@ +#pragma once + +#ifndef HAL_MSP_ENABLED +#define HAL_MSP_ENABLED !HAL_MINIMIZE_FEATURES +#endif + +#if HAL_MSP_ENABLED + +// use betaflight 4.1.0 for compatibility with DJI OSD +#define GIT_SHORT_REVISION_LENGTH 8 +#define BUILD_DATE_LENGTH 11 +#define BUILD_TIME_LENGTH 8 + +#define FC_VERSION_MAJOR 4 +#define FC_VERSION_MINOR 1 +#define FC_VERSION_PATCH_LEVEL 0 + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_MSP/test/msp_loop.sh b/libraries/AP_MSP/test/msp_loop.sh new file mode 100755 index 0000000000000..2c89001388c86 --- /dev/null +++ b/libraries/AP_MSP/test/msp_loop.sh @@ -0,0 +1,7 @@ +#!/bin/bash +COUNTER=0 +while [ $COUNTER -lt 1000 ]; do + nc localhost 5763 -w 0 < msp_status.raw + nc localhost 5763 -w 0 < msp_raw_imu.raw + let COUNTER=COUNTER+1 +done diff --git a/libraries/AP_MSP/test/msp_osd_config.raw b/libraries/AP_MSP/test/msp_osd_config.raw new file mode 100644 index 0000000000000000000000000000000000000000..0bf3d8c660ec77e4f6e63c2920b651be244cffb7 GIT binary patch literal 6 NcmY%KwP6Se0RRL`0agG2 literal 0 HcmV?d00001 diff --git a/libraries/AP_MSP/test/msp_raw_imu.raw b/libraries/AP_MSP/test/msp_raw_imu.raw new file mode 100644 index 0000000000000000000000000000000000000000..1a0de6e0130a417f2058cd715a693bef4c89de91 GIT binary patch literal 6 NcmY%KwP8q00{{ep0eS!c literal 0 HcmV?d00001 diff --git a/libraries/AP_MSP/test/msp_status.raw b/libraries/AP_MSP/test/msp_status.raw new file mode 100644 index 0000000000000000000000000000000000000000..4416206878496367bc7f366f1c409a9470f6eced GIT binary patch literal 6 NcmY%KwP8q21powo0eAoa literal 0 HcmV?d00001 diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 3f0b31a87df3e..64f963c5f85d9 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -90,6 +90,38 @@ uint8_t crc_crc8(const uint8_t *p, uint8_t len) return crc & 0xFF; } +// crc8 from betaflight +uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) +{ + return crc8_dvb(crc, a, 0xD5); +} + +// crc8 from betaflight +uint8_t crc8_dvb(uint8_t crc, uint8_t a, uint8_t seed) +{ + crc ^= a; + for (uint8_t i = 0; i < 8; ++i) { + if (crc & 0x80) { + crc = (crc << 1) ^ seed; + } else { + crc = crc << 1; + } + } + return crc; +} + +// crc8 from betaflight +uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length) +{ + const uint8_t *p = (const uint8_t *)data; + const uint8_t *pend = p + length; + + for (; p != pend; p++) { + crc = crc8_dvb_s2(crc, *p); + } + return crc; +} + /* xmodem CRC thanks to avr-liberty https://github.com/dreamiurg/avr-liberty diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h index a970a99c90e17..a056efae72c50 100644 --- a/libraries/AP_Math/crc.h +++ b/libraries/AP_Math/crc.h @@ -19,6 +19,9 @@ uint16_t crc_crc4(uint16_t *data); uint8_t crc_crc8(const uint8_t *p, uint8_t len); +uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a); +uint8_t crc8_dvb(uint8_t crc, uint8_t a, uint8_t seed); +uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); uint16_t crc_xmodem_update(uint16_t crc, uint8_t data); uint16_t crc_xmodem(const uint8_t *data, uint16_t len); uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index 7c68d9e3617be..cc47863fbb6b0 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -1,4 +1,4 @@ /// @file AP_OpticalFlow.h /// @brief Catch-all header that defines all supported optical flow classes. -#include "OpticalFlow.h" +#include "OpticalFlow.h" \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp new file mode 100644 index 0000000000000..c043eaaff069b --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp @@ -0,0 +1,108 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include +#include "AP_OpticalFlow_MSP.h" + +#if HAL_MSP_ENABLED + +#define OPTFLOW_MSP_TIMEOUT_SEC 0.5f // 2Hz + +extern const AP_HAL::HAL& hal; + +// detect the device +AP_OpticalFlow_MSP *AP_OpticalFlow_MSP::detect(OpticalFlow &_frontend) +{ + // we assume mavlink messages will be sent into this driver + AP_OpticalFlow_MSP *sensor = new AP_OpticalFlow_MSP(_frontend); + return sensor; +} + +// read latest values from sensor and fill in x,y and totals. +void AP_OpticalFlow_MSP::update(void) +{ + // record gyro values as long as they are being used + // the sanity check of dt below ensures old gyro values are not used + if (gyro_sum_count < 1000) { + const Vector3f& gyro = AP::ahrs().get_gyro(); + gyro_sum.x += gyro.x; + gyro_sum.y += gyro.y; + gyro_sum_count++; + } + + // return without updating state if no readings + if (count == 0) { + return; + } + + struct OpticalFlow::OpticalFlow_state state {}; + + state.surface_quality = quality_sum / count; + + // calculate dt + const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6; + prev_frame_us = latest_frame_us; + + // sanity check dt + if (is_positive(dt) && (dt < OPTFLOW_MSP_TIMEOUT_SEC)) { + // calculate flow values + const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x; + const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y; + + // copy flow rates to state structure + state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt, + ((float)flow_sum.y / count) * flow_scale_factor_y * dt }; + + // copy average body rate to state structure + state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count }; + + _applyYaw(state.flowRate); + _applyYaw(state.bodyRate); + } else { + // first frame received in some time so cannot calculate flow values + state.flowRate.zero(); + state.bodyRate.zero(); + } + + _update_frontend(state); + + // reset local buffers + flow_sum.zero(); + quality_sum = 0; + count = 0; + + // reset gyro sum + gyro_sum.zero(); + gyro_sum_count = 0; +} + +// handle OPTICAL_FLOW mavlink messages +void AP_OpticalFlow_MSP::handle_msp(const msp_opflow_sensor_t &pkt) +{ + //hal.console->printf("MSP::opflow X=%d, Y=%d\n", pkt.motionX, pkt.motionY); + + // record time message was received + // ToDo: add jitter correction + latest_frame_us = AP_HAL::micros64(); + + // add sensor values to sum + flow_sum.x += pkt.motion_x; + flow_sum.y += pkt.motion_y; + quality_sum += (int)pkt.quality * 100 / 255; + count++; +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h new file mode 100644 index 0000000000000..22dd4ecb44e15 --- /dev/null +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.h @@ -0,0 +1,37 @@ +#pragma once + +#include "OpticalFlow.h" +#include + +#if HAL_MSP_ENABLED + +class AP_OpticalFlow_MSP : public OpticalFlow_backend +{ +public: + /// constructor + using OpticalFlow_backend::OpticalFlow_backend; + + // initialise the sensor + void init() override {} + + // read latest values from sensor and fill in x,y and totals. + void update(void) override; + + // get update from msp + void handle_msp(const msp_opflow_sensor_t &pkt) override; + + // detect if the sensor is available + static AP_OpticalFlow_MSP *detect(OpticalFlow &_frontend); + +private: + + uint64_t prev_frame_us; // system time of last message when update was last called + uint64_t latest_frame_us; // system time of most recent messages processed + Vector2l flow_sum; // sum of sensor's flow_x and flow_y values since last call to update + uint16_t quality_sum; // sum of sensor's quality values since last call to update + uint16_t count; // number of sensor readings since last call to update + Vector2f gyro_sum; // sum of gyro sensor values since last frame from flow sensor + uint16_t gyro_sum_count; // number of gyro sensor values in sum +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 018eacbd39112..1972d5de87d00 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -7,6 +7,7 @@ #include "AP_OpticalFlow_CXOF.h" #include "AP_OpticalFlow_MAV.h" #include "AP_OpticalFlow_HereFlow.h" +#include "AP_OpticalFlow_MSP.h" #include extern const AP_HAL::HAL& hal; @@ -25,7 +26,7 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = { // @Param: _TYPE // @DisplayName: Optical flow sensor type // @Description: Optical flow sensor type - // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:UAVCAN + // @Values: 0:None, 1:PX4Flow, 2:Pixart, 3:Bebop, 4:CXOF, 5:MAVLink, 6:UAVCAN, // @User: Standard // @RebootRequired: True AP_GROUPINFO("_TYPE", 0, OpticalFlow, _type, (int8_t)OPTICAL_FLOW_TYPE_DEFAULT), @@ -135,6 +136,11 @@ void OpticalFlow::init(uint32_t log_bit) backend = new AP_OpticalFlow_HereFlow(*this); #endif break; +#if HAL_MSP_ENABLED + case OpticalFlowType::MSP: + backend = AP_OpticalFlow_MSP::detect(*this); + break; +#endif //HAL_MSP_ENABLED case OpticalFlowType::SITL: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL backend = new AP_OpticalFlow_SITL(*this); @@ -173,6 +179,20 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg) } } +#if HAL_MSP_ENABLED +void OpticalFlow::handle_msp(const msp_opflow_sensor_t &pkt) +{ + // exit immediately if not enabled + if (!enabled()) { + return; + } + + if (backend != nullptr) { + backend->handle_msp(pkt); + } +} +#endif //HAL_MSP_ENABLED + void OpticalFlow::update_state(const OpticalFlow_state &state) { _state = state; diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index d8942125978b0..73cb4de845cc4 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -19,10 +19,15 @@ * Code by Randy Mackay. DIYDrones.com */ +#include #include #include #include +#if HAL_MSP_ENABLED +using namespace MSP; +#endif //HAL_MSP_ENABLED + class OpticalFlow_backend; class AP_AHRS_NavEKF; @@ -50,7 +55,8 @@ class OpticalFlow CXOF = 4, MAVLINK = 5, UAVCAN = 6, - SITL = 10 + MSP = 7, + SITL = 10, }; // init - initialise sensor @@ -68,6 +74,11 @@ class OpticalFlow // handle optical flow mavlink messages void handle_msg(const mavlink_message_t &msg); +#if HAL_MSP_ENABLED + // handle optical flow msp messages + void handle_msp(const msp_opflow_sensor_t &pkt); +#endif //HAL_MSP_ENABLED + // quality - returns the surface quality as a measure from 0 ~ 255 uint8_t quality() const { return _state.surface_quality; } diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.h b/libraries/AP_OpticalFlow/OpticalFlow_backend.h index a774baaea8c40..9ad97c5a6f8e7 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow_backend.h +++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.h @@ -38,6 +38,9 @@ class OpticalFlow_backend // handle optical flow mavlink messages virtual void handle_msg(const mavlink_message_t &msg) {} + // handle optical flow msp messages + virtual void handle_msp(const msp_opflow_sensor_t &pkt) {} + protected: // access to frontend OpticalFlow &frontend; diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h index 43056340f3f50..382929b86a04d 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.h +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.h @@ -21,7 +21,7 @@ #define TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent) // for fair scheduler -#define TELEM_TIME_SLOT_MAX 15 +#define TELEM_TIME_SLOT_MAX 24 //#define TELEM_DEBUG class AP_RCTelemetry { diff --git a/libraries/AP_RCTelemetry/StreamBuffer.cpp b/libraries/AP_RCTelemetry/StreamBuffer.cpp new file mode 100644 index 0000000000000..ef6d596d8e391 --- /dev/null +++ b/libraries/AP_RCTelemetry/StreamBuffer.cpp @@ -0,0 +1,144 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include "StreamBuffer.h" + +StreamBuffer::StreamBuffer(uint8_t *p, uint8_t *end) +{ + ptr = p; + end_ptr = end; +} + +void StreamBuffer::write_uint16(uint16_t val) +{ + write_uint8(val >> 0); + write_uint8(val >> 8); +} + +void StreamBuffer::write_uint32(uint32_t val) +{ + write_uint8(val >> 0); + write_uint8(val >> 8); + write_uint8(val >> 16); + write_uint8(val >> 24); +} + +void StreamBuffer::write_uint16_big_endian(uint16_t val) +{ + write_uint8(val >> 8); + write_uint8((uint8_t)val); +} + +void StreamBuffer::write_uint32_big_endian(uint32_t val) +{ + write_uint8(val >> 24); + write_uint8(val >> 16); + write_uint8(val >> 8); + write_uint8((uint8_t)val); +} + +void StreamBuffer::fill(uint8_t d, int len) +{ + memset(ptr, d, len); + ptr += len; +} + +void StreamBuffer::write_data(const void *d, int len) +{ + memcpy(ptr, d, len); + ptr += len; +} + +void StreamBuffer::write_string(const char *string) +{ + write_data(string, strlen(string)); +} + +void StreamBuffer::write_string_with_zero_terminator(const char *string) +{ + write_data(string, strlen(string) + 1); +} + +uint16_t StreamBuffer::read_uint16() +{ + uint16_t ret; + ret = read_uint8(); + ret |= read_uint8() << 8; + return ret; +} + +uint32_t StreamBuffer::read_uint32() +{ + uint32_t ret; + ret = read_uint8(); + ret |= read_uint8() << 8; + ret |= read_uint8() << 16; + ret |= read_uint8() << 24; + return ret; +} + +void StreamBuffer::read_data(void *d, int len) +{ + memcpy(d, ptr, len); +} + +// reader - return bytes remaining in buffer +// writer - return available space +int32_t StreamBuffer::remaining() +{ + return end_ptr - ptr; +} + +uint8_t* StreamBuffer::data() +{ + return ptr; +} + +// advance buffer pointer +// reader - skip data +// writer - commit written data +void StreamBuffer::advance(uint32_t size) +{ + ptr += size; +} + +// modifies streambuf so that written data are prepared for reading +void StreamBuffer::switch_to_reader(uint8_t *base) +{ + end_ptr = ptr; + ptr = base; +} + +uint8_t StreamBuffer::crc8_high_first(uint8_t *p, uint8_t len) +{ + uint8_t crc = 0x00; + while (len--) { + crc ^= *p++; + for (uint8_t i = 8; i > 0; --i) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31; + } else { + crc = (crc << 1); + } + } + } + return (crc); +} diff --git a/libraries/AP_RCTelemetry/StreamBuffer.h b/libraries/AP_RCTelemetry/StreamBuffer.h new file mode 100644 index 0000000000000..e9830f3326c6e --- /dev/null +++ b/libraries/AP_RCTelemetry/StreamBuffer.h @@ -0,0 +1,57 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include + +class StreamBuffer { +public: + // simple buffer-based serializer/deserializer without implicit size check + + StreamBuffer(uint8_t *ptr, uint8_t *end); + + void write_uint8(uint8_t val) { *ptr++ = val; } + void write_uint16(uint16_t val); + void write_uint32(uint32_t val); + void write_uint16_big_endian(uint16_t val); + void write_uint32_big_endian(uint32_t val); + void fill(uint8_t data, int len); + void write_data(const void *data, int len); + void write_string(const char *string); + void write_string_with_zero_terminator(const char *string); + + uint8_t read_uint8() { return *ptr++; } + uint16_t read_uint16(); + uint32_t read_uint32(); + void read_data(void *data, int len); + + int32_t remaining(); + uint8_t* data(); + void advance(uint32_t size); + void switch_to_reader(uint8_t * base); + + // crc functions + uint8_t crc8_high_first(uint8_t *ptr, uint8_t len); + +private: + uint8_t *ptr; // data pointer must be first (StreamBuffer* is equivalent to uint8_t **) + uint8_t *end_ptr; +}; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 5976aaec6fafc..bb21e9642639e 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -44,6 +44,7 @@ #include "AP_RangeFinder_UAVCAN.h" #include "AP_RangeFinder_Lanbao.h" #include "AP_RangeFinder_LeddarVu8.h" +#include "AP_RangeFinder_MSP.h" #include #include @@ -527,6 +528,15 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; #endif +#if HAL_MSP_ENABLED + case Type::MSP: +#ifndef HAL_BUILD_AP_PERIPH + if (AP_RangeFinder_MSP::detect()) { + drivers[instance] = new AP_RangeFinder_MSP(state[instance], params[instance]); + } +#endif + break; +#endif //HAL_MSP_ENABLED default: break; } @@ -573,6 +583,18 @@ void RangeFinder::handle_msg(const mavlink_message_t &msg) } } +#if HAL_MSP_ENABLED +void RangeFinder::handle_msp(const msp_rangefinder_sensor_t &pkt) +{ + uint8_t i; + for (i=0; ihandle_msp(pkt); + } + } +} +#endif //HAL_MSP_ENABLED + // return true if we have a range finder with the specified orientation bool RangeFinder::has_orientation(enum Rotation orientation) const { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index ddc70f3517aea..ea9f76934ade2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -18,8 +18,10 @@ #include #include #include +#include #include "AP_RangeFinder_Params.h" + // Maximum number of range finder instances available on this platform #ifndef RANGEFINDER_MAX_INSTANCES #define RANGEFINDER_MAX_INSTANCES 10 @@ -33,6 +35,10 @@ #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 50 #endif +#if HAL_MSP_ENABLED +using namespace MSP; +#endif //HAL_MSP_ENABLED + class AP_RangeFinder_Backend; class RangeFinder @@ -80,6 +86,7 @@ class RangeFinder VL53L1X_Short = 28, LeddarVu8_Serial = 29, HC_SR04 = 30, + MSP = 31, }; enum class Function { @@ -132,6 +139,10 @@ class RangeFinder // Handle an incoming DISTANCE_SENSOR message (from a MAVLink enabled range finder) void handle_msg(const mavlink_message_t &msg); +#if HAL_MSP_ENABLED + // Handle an incoming DISTANCE_SENSOR message (from a MSP enabled range finder) + void handle_msp(const msp_rangefinder_sensor_t &pkt); +#endif //HAL_MSP_ENABLED // return true if we have a range finder with the specified orientation bool has_orientation(enum Rotation orientation) const; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 1197c0acdd2bd..9af4d53645f0d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -32,6 +32,7 @@ class AP_RangeFinder_Backend virtual void update() = 0; virtual void handle_msg(const mavlink_message_t &msg) { return; } + virtual void handle_msp(const msp_rangefinder_sensor_t &pkt) { return; } enum Rotation orientation() const { return (Rotation)params.orientation.get(); } uint16_t distance_cm() const { return state.distance_cm; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp new file mode 100644 index 0000000000000..3cdaeb7244c96 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.cpp @@ -0,0 +1,72 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_RangeFinder_MSP.h" +#include + +#if HAL_MSP_ENABLED + +extern const AP_HAL::HAL& hal; + +/* + The constructor also initialises the rangefinder. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the rangefinder +*/ +AP_RangeFinder_MSP::AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : + AP_RangeFinder_Backend(_state, _params) +{ + state.last_reading_ms = AP_HAL::millis(); + distance_cm = 0; +} + +/* + detect if a MSP rangefinder is connected. We'll detect by + checking a parameter. +*/ +bool AP_RangeFinder_MSP::detect() +{ + // Assume that if the user set the RANGEFINDER_TYPE parameter to MSP, + // there is an attached MSP rangefinder + return true; +} + +/* + Set the distance based on a MSP message +*/ +void AP_RangeFinder_MSP::handle_msp(const msp_rangefinder_sensor_t &pkt) +{ + //hal.console->printf("MSP::range cm=%.01f\n", pkt.distance_mm / 10.0f); + state.last_reading_ms = AP_HAL::millis(); + distance_cm = pkt.distance_mm / 10; +} + +/* + update the state of the sensor +*/ +void AP_RangeFinder_MSP::update(void) +{ + //Time out on incoming data; if we don't get new + //data in 500ms, dump it + if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_MSP_TIMEOUT_MS) { + set_status(RangeFinder::Status::NoData); + state.distance_cm = 0; + } else { + state.distance_cm = distance_cm; + update_status(); + } +} + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MSP.h b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.h new file mode 100644 index 0000000000000..59854cfd51fbf --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MSP.h @@ -0,0 +1,43 @@ +#pragma once + +#include "AP_RangeFinder.h" +#include "AP_RangeFinder_Backend.h" + +#if HAL_MSP_ENABLED + +// Data timeout +#define AP_RANGEFINDER_MSP_TIMEOUT_MS 500 + +class AP_RangeFinder_MSP : public AP_RangeFinder_Backend +{ + +public: + // constructor + AP_RangeFinder_MSP(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + // static detection function + static bool detect(); + + // update state + void update(void) override; + + // Get update from msp + void handle_msp(const msp_rangefinder_sensor_t &pkt) override; + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return sensor_type; + } + +private: + uint16_t distance_cm; + + // start a reading + static bool start_reading(void); + static bool get_reading(uint16_t &reading_cm); + + MAV_DISTANCE_SENSOR sensor_type = MAV_DISTANCE_SENSOR_LASER; +}; + +#endif //HAL_MSP_ENABLED \ No newline at end of file diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index ee62e5b3f05d8..623df04e21895 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 1_PROTOCOL // @DisplayName: Telem1 protocol selection // @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP, 30:DJI FPV // @User: Standard // @RebootRequired: True AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, SerialProtocol_MAVLink2), @@ -108,7 +108,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 2_PROTOCOL // @DisplayName: Telemetry 2 protocol selection // @Description: Control what protocol to use on the Telem2 port. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP, 30:DJI FPV // @User: Standard // @RebootRequired: True AP_GROUPINFO("2_PROTOCOL", 3, AP_SerialManager, state[2].protocol, SERIAL2_PROTOCOL), @@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 3_PROTOCOL // @DisplayName: Serial 3 (GPS) protocol selection // @Description: Control what protocol Serial 3 (GPS) should be used for. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP, 30:DJI FPV // @User: Standard // @RebootRequired: True AP_GROUPINFO("3_PROTOCOL", 5, AP_SerialManager, state[3].protocol, SERIAL3_PROTOCOL), @@ -142,7 +142,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 4_PROTOCOL // @DisplayName: Serial4 protocol selection // @Description: Control what protocol Serial4 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP // @User: Standard // @RebootRequired: True AP_GROUPINFO("4_PROTOCOL", 7, AP_SerialManager, state[4].protocol, SERIAL4_PROTOCOL), @@ -159,7 +159,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 5_PROTOCOL // @DisplayName: Serial5 protocol selection // @Description: Control what protocol Serial5 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP, 30:DJI FPV // @User: Standard // @RebootRequired: True AP_GROUPINFO("5_PROTOCOL", 9, AP_SerialManager, state[5].protocol, SERIAL5_PROTOCOL), @@ -178,7 +178,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 6_PROTOCOL // @DisplayName: Serial6 protocol selection // @Description: Control what protocol Serial6 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP, 30:DJI FPV // @User: Standard // @RebootRequired: True AP_GROUPINFO("6_PROTOCOL", 12, AP_SerialManager, state[6].protocol, SERIAL6_PROTOCOL), @@ -277,7 +277,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 7_PROTOCOL // @DisplayName: Serial7 protocol selection // @Description: Control what protocol Serial7 port should be used for. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:MSP // @User: Standard // @RebootRequired: True AP_GROUPINFO("7_PROTOCOL", 23, AP_SerialManager, state[7].protocol, SERIAL7_PROTOCOL), @@ -482,6 +482,13 @@ void AP_SerialManager::init() state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); break; + case SerialProtocol_MSP: + case SerialProtocol_DJI_FPV: + state[i].uart->begin(map_baudrate(state[i].baud), + AP_SERIALMANAGER_MSP_BUFSIZE_RX, + AP_SERIALMANAGER_MSP_BUFSIZE_TX); + break; + default: state[i].uart->begin(map_baudrate(state[i].baud)); } diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index 286efe9a4429d..d4f25066c61f6 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -98,6 +98,10 @@ #define AP_SERIALMANAGER_SLCAN_BUFSIZE_RX 128 #define AP_SERIALMANAGER_SLCAN_BUFSIZE_TX 128 +// MSP protocol default buffer sizes +#define AP_SERIALMANAGER_MSP_BUFSIZE_RX 128 +#define AP_SERIALMANAGER_MSP_BUFSIZE_TX 256 + class AP_SerialManager { public: AP_SerialManager(); @@ -137,6 +141,8 @@ class AP_SerialManager { SerialProtocol_RunCam = 26, SerialProtocol_Hott = 27, SerialProtocol_Scripting = 28, + SerialProtocol_MSP = 29, + SerialProtocol_DJI_FPV = 30, }; // get singleton instance diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 8e4040c4cc7cb..874caf4798006 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -27,6 +27,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(visual_odom, "VISO", 3, AP_Vehicle, AP_VisualOdom), #endif +#if HAL_MSP_ENABLED + // @Group: MSP + // @Path: ../AP_MSP/AP_MSP.cpp + AP_SUBGROUPINFO(msp, "MSP", 4, AP_Vehicle, AP_MSP), +#endif AP_GROUPEND }; @@ -102,7 +107,9 @@ void AP_Vehicle::setup() // init library used for visual position estimation visual_odom.init(); #endif - +#if HAL_MSP_ENABLED + msp.init(); +#endif #if AP_PARAM_KEY_DUMP AP_Param::show_all(hal.console, true); #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 51bc7d8e0a22a..9afca3f85fb61 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -40,6 +40,7 @@ #include #include #include +#include class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -247,6 +248,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_ESC_Telem esc_telem; +#if HAL_MSP_ENABLED + AP_MSP msp; +#endif + static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Scheduler::Task scheduler_tasks[];