From 282d11316d59557d89a6fa041d7579834ea40dd8 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 29 Jan 2023 17:30:38 +0100 Subject: [PATCH 001/175] Add script to convert a BF unified target to an INAV target The target can be used as a starting point for a full featured INAV target, but won't include any fixed wing functionality. --- src/utils/bf2inav.py | 168 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100755 src/utils/bf2inav.py diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py new file mode 100755 index 00000000000..e9a401a27cf --- /dev/null +++ b/src/utils/bf2inav.py @@ -0,0 +1,168 @@ +#!/usr/bin/python3 +import sys +import os +import io +import getopt +import re +import json + +version = '0.1' + +def translateFunctionName(bffunction, index): + return bffunction + '_' + index + +def translatePin(bfpin): + pin = re.sub("^([A-Z])0*(\d+)$", r'P\1\2', bfpin) + return pin + +def buildMap(inputFile): + map = { 'defines': [], 'features': [], 'pins': {}, 'serial': {}, 'variables': {}} + + + f = open(inputFile, 'r') + while True: + l = f.readline() + if not l: + break + m = re.search(r'^#mcu\s+([0-9A-Za-z]+)$', l) + if m: + map['mcu'] = {'type': m.group(1)} + + m = re.search(r'^#define\s+(\w+)$', l) + if m: + map['defines'].append(m.group(1)) + + m = re.search(r'^feature\s+(-?\w+)$', l) + if m: + map['features'].append(m.group(1)) + + + m = re.search(r'^resource\s+(-?\w+)\s+(\d+)\s+(\w+)$', l) + if m: + resource_type = m.group(1) + resource_index = m.group(2) + pin = translatePin(m.group(3)) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['function'] = translateFunctionName(resource_type, resource_index) + + m = re.search(r'^timer\s+(\w+)\s+AF(\d+)$', l) + if m: + pin = translatePin(m.group(1)) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['AF'] = m.group(2) + + m = re.search(r'^#\s*pin\s+(\w+):\s*(TIM\d+)\s+(CH\d+).+$', l) + if m: + pin = translatePin(m.group(1)) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['TIM'] = m.group(2) + map['pins'][pin]['CH'] = m.group(3) + + m = re.search(r'^dma\s+([A-Za-z0-9]+)\s+([A-Za-z0-9]+)\s+(\d+).*$', l) + if m: + + if(m.group(1) == 'ADC'): + pin = 'ADC' + m.group(2) + else: + pin = translatePin(m.group(2)) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['DMA'] = m.group(3) + +# 1 2 3 4 +# pin B04: DMA1 Stream 4 Channel 5 + + m = re.search(r'^#\s+pin\s+(\w+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + if m: + pin = translatePin(m.group(1)) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['DMA_STREAM'] = m.group(3) + map['pins'][pin]['DMA_CHANNEL'] = m.group(4) + + m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + if m: + pin = 'ADC' + m.group(1) + if not map['pins'].get(pin): + map['pins'][pin] = {} + + map['pins'][pin]['DMA_STREAM'] = m.group(3) + map['pins'][pin]['DMA_CHANNEL'] = m.group(4) + +#serial 0 64 115200 57600 0 115200 + m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) + if m: + idx = m.group(1) + if not map['serial'].get(idx): + map['serial'][idx] = {} + + map['serial'][idx]['FUNCTION'] = m.group(2) + map['serial'][idx]['MSP_BAUD'] = m.group(3) + map['serial'][idx]['GPS_BAUD'] = m.group(4) + map['serial'][idx]['TELEMETRY_BAUD'] = m.group(5) + map['serial'][idx]['BLACKBOX_BAUD'] = m.group(6) + + m = re.search(r'^set\s+(\w+)\s*=\s*(\w+)$', l) + if m: + map['variables'][m.group(1)] = m.group(2) + + + return map + +def printHelp(): + print ("%s -i bf-target.config -o output-directory" % (sys.argv[0])) + print (" -i | --input-config= -- print this help") + print (" -o | --output-dir= -- print this help") + print (" -h | --help -- print this help") + print (" -v | --version -- print version") + return + +def main(argv): + inputfile = '' + outputdir = '.' + global version + + try: + opts, args = getopt.getopt(argv,"hvi:o:", ["input-config=", "output-dir=", 'version', 'help']) + except getopt.GeoptError: + printHelp() + sys.exit(2) + + for opt, arg in opts: + if opt in ('-h', '--help'): + printHelp() + sys.exit(1) + elif opt in ('-i', '--input-config'): + inputfile = arg + elif opt in ('-o', '--output-dir'): + outputdir = arg + elif opt in ('-v', '--version'): + print ("%s: %s" % (sys.argv[0], version)) + sys.exit(0) + + if (os.path.exists(inputfile) and os.path.isdir(outputdir)): + targetDefinition = buildMap(inputfile) + else: + printHelp() + sys.exit(2) + + + map = buildMap(inputfile) + + print (json.dumps(map, indent=4)) + + sys.exit(0) + + +if( __name__ == "__main__"): + main(sys.argv[1:]) + + From 8983e5adba132e12493f8595e7bea758a5133f49 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 29 Jan 2023 22:28:32 +0100 Subject: [PATCH 002/175] Setup basic peripherals. - Leds - Beeper - WS2811 - UART - SOFTSERIAL - SPI - I2C --- src/utils/bf2inav.py | 223 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 221 insertions(+), 2 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index e9a401a27cf..c7273eff5d9 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -15,10 +15,220 @@ def translatePin(bfpin): pin = re.sub("^([A-Z])0*(\d+)$", r'P\1\2', bfpin) return pin +def mcu2target(mcu): +#mcu STM32F405 + if mcu['type'] == 'STM32F405': + return 'target_stm32f405xg' + +#mcu STM32F411 + if mcu['type'] == 'STM32F411': + return 'target_stm32f411xe' + +#mcu STM32F7X2 + if mcu['type'] == 'STM32F7X2': + return 'target_stm32f722xe' + +#mcu STM32F745 + if mcu['type'] == 'STM32F745': + return 'target_stm32f745xg' + +#mcu STM32H743 + if mcu['type'] == 'STM32H743': + return 'target_stm32h743xi' + + print("Unknown MCU: %s" % (mcu)) + sys.exit(-1) + +def writeCmakeLists(outputFolder, map): + file = open(outputFolder + '/CMakeLists.txt', "w+") + + t = mcu2target(map['mcu']) + + file.write("%s(%s SKIP_RELEASES)\n" % (t, map['board_name'])) + + return + + +def findPinsByFunction(function, map): + result = [] + for pin in map['pins']: + pattern = "^%s_" % (function) + if map['pins'][pin].get('function') and re.search(pattern, map['pins'][pin]['function']): + result.append(pin) + + return result + +def findPinByFunction(function, map): + for pin in map['pins']: + if map['pins'][pin].get('function') and map['pins'][pin]['function'] == function: + return pin + + return None + +def writeTargetH(folder, map): + file = open(folder + '/target.h', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +#define USE_TARGET_CONFIG + + \n""" + ) + file.write("#define TARGET_BOARD_IDENTIFIER \"????\"\n") + file.write("#define USBD_PRODUCT_STRING \"%s\"\n" % (map['board_name'])) + + # beeper + file.write("// Beeper\n") + pin = findPinByFunction('BEEPER_1', map) + if pin: + file.write("#define USE_BEEPER\n") + file.write("#define BEEPER %s\n" % (pin)) + if map['variables'].get('beeper_inversion', 'OFF') == 'ON': + file.write("#define BEEPER_INVERTED\n") + + # Leds + file.write("// Leds\n") + pin = findPinByFunction('LED_STRIP_1', map) + if pin: + file.write('#define USE_LED_STRIP\n') + file.write("#define WS2811_PIN %s\n" % (pin)) + + for i in range(1, 9): + pin = findPinByFunction("LED_%i" % (i), map) + if pin: + file.write("#define LED%i %s\n" % (i-1, pin)) + + # Serial ports and usb + file.write("// UARTs\n") + serial_count = 0 + pin = findPinByFunction('USB_DETECT_1', map) + if pin: + file.write("#define USB_IO\n") + file.write("#define USE_VCP\n") + file.write("#define VBUS_SENSING_PIN %s\n" % (pin)) + file.write("#define VBUS_SENSING_ENABLED\n"); + serial_count += 1 + + for i in range(1, 9): + txpin = findPinByFunction("SERIAL_TX_%i" % (i), map) + rxpin = findPinByFunction("SERIAL_RX_%i" % (i), map) + if txpin or rxpin: + file.write("#define USE_UART%i\n" % (i)) + serial_count+=1 + else: + continue + + if txpin: + file.write("#define UART%i_TX_PIN %s\n" % (i, txpin)) + if rxpin: + file.write("#define UART%i_RX_PIN %s\n" % (i, rxpin)) + + # soft serial + for i in range(11, 19): + txpin = findPinByFunction("SERIAL_TX_%i" % (i), map) + rxpin = findPinByFunction("SERIAL_RX_%i" % (i), map) + idx = i - 10 + if txpin != None or rxpin != None: + file.write("#define USE_SOFTSERIAL%i\n" % (idx)) + serial_count+=1 + else: + continue + + if txpin != None: + file.write("#define SOFTSERIAL%i_TX_PIN %s\n" % (idx, txpin)) + else: + file.write("#define SOFTSERIAL%i_TX_PIN %s\n" % (idx, rxpin)) + + if rxpin != None: + file.write("#define SOFTSERIAL%i_RX_PIN %s\n" % (idx, rxpin)) + else: + file.write("#define SOFTSERIAL%i_RX_PIN %s\n" % (idx, txpin)) + + file.write("#define SERIAL_PORT_COUNT %i\n" % (serial_count)) + + file.write("// SPI\n") + use_spi_defined = False + for i in range(1, 9): + sckpin = findPinByFunction("SPI_SCK_%i" % (i), map) + misopin = findPinByFunction("SPI_MISO_%i" % (i), map) + mosipin = findPinByFunction("SPI_MOSI_%i" % (i), map) + if (sckpin or misopin or mosipin): + if (not use_spi_defined): + use_spi_defined = True + file.write("#define USE_SPI\n") + file.write("#define USE_SPI_DEVICE_%i\n" % (i)) + + if sckpin: + file.write("#define SPI%i_SCK_PIN %s\n" % (i, sckpin)) + if misopin: + file.write("#define SPI%i_MISO_PIN %s\n" % (i, misopin)) + if mosipin: + file.write("#define SPI%i_MOSI_PIN %s\n" % (i, mosipin)) + + file.write("// I2C\n") + use_i2c_defined = False + for i in range(1, 9): + sclpin = findPinByFunction("I2C_SCL_%i" % (i), map) + sdapin = findPinByFunction("I2C_SDA_%i" % (i), map) + if (sclpin or sdapin): + if (not use_i2c_defined): + use_i2c_defined = True + file.write("#define USE_I2C\n") + file.write("#define USE_I2C_DEVICE_%i\n" % (i)) + + if sclpin: + file.write("#define I2C%i_SCL %s\n" % (i, sclpin)) + if sdapin: + file.write("#define I2C%i_SDA %s\n" % (i, sdapin)) + # todo: #define DEFAULT_I2C_BUS + file.write("// ADC\n") + # TODO + file.write("// Gyro & ACC\n") + # TODO + file.write("// OSD\n") + # TODO + file.write("// Blackbox\n") + # TODO + + + return + +def writeTargetC(folder, map): + return + +def writeConfigC(folder, map): + return + +def writeTarget(outputFolder, map): + writeCmakeLists(outputFolder, map) + writeTargetH(outputFolder, map) + writeTargetC(outputFolder, map) + writeConfigC(outputFolder, map) + + return + def buildMap(inputFile): map = { 'defines': [], 'features': [], 'pins': {}, 'serial': {}, 'variables': {}} - f = open(inputFile, 'r') while True: l = f.readline() @@ -27,6 +237,14 @@ def buildMap(inputFile): m = re.search(r'^#mcu\s+([0-9A-Za-z]+)$', l) if m: map['mcu'] = {'type': m.group(1)} + + m = re.search(r'^board_name\s+(\w+)$', l) + if m: + map['board_name'] = m.group(1) + + m = re.search(r'^manufacturer_id\s+(\w+)$', l) + if m: + map['manufacturer_id'] = m.group(1) m = re.search(r'^#define\s+(\w+)$', l) if m: @@ -97,7 +315,6 @@ def buildMap(inputFile): map['pins'][pin]['DMA_STREAM'] = m.group(3) map['pins'][pin]['DMA_CHANNEL'] = m.group(4) -#serial 0 64 115200 57600 0 115200 m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) if m: idx = m.group(1) @@ -159,6 +376,8 @@ def main(argv): print (json.dumps(map, indent=4)) + writeTarget(outputdir, map) + sys.exit(0) From 63b39396f9cb04773c36b4366625544b6708aee2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 30 Jan 2023 16:59:03 +0100 Subject: [PATCH 003/175] Add OSD, Blackbox definitions. Also add ADC but still missing ADC DMA --- src/utils/bf2inav.py | 142 ++++++++++++++++++++++++++++++++++++++----- 1 file changed, 128 insertions(+), 14 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index c7273eff5d9..0685b17f86f 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -1,4 +1,12 @@ #!/usr/bin/python3 +# Betaflight unified config to INAV target converter +# +# This script can be used to Generate a basic working target from a Betaflight Configuration. +# The idea is that this target can be used as a starting point for full INAV target. +# +# The generated target will not include any servo assignments or fixed wing features. +# + import sys import os import io @@ -93,7 +101,7 @@ def writeTargetH(folder, map): \n""" ) - file.write("#define TARGET_BOARD_IDENTIFIER \"????\"\n") + file.write("#define TARGET_BOARD_IDENTIFIER \"B2IN\"\n") file.write("#define USBD_PRODUCT_STRING \"%s\"\n" % (map['board_name'])) # beeper @@ -199,16 +207,83 @@ def writeTargetH(folder, map): file.write("#define I2C%i_SCL %s\n" % (i, sclpin)) if sdapin: file.write("#define I2C%i_SDA %s\n" % (i, sdapin)) - # todo: #define DEFAULT_I2C_BUS + file.write("// ADC\n") - # TODO + + + # ADC_BATT ch1 + use_adc = False + pin = findPinByFunction('ADC_BATT_1', map) + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_1_PIN %s\n" % (pin)) + file.write("#define VBAT_ADC_CHANNEL ADC_CHN_1\n"); + + # ADC_CURR ch2 + pin = findPinByFunction('ADC_CURR_1'): + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_2_PIN %s\n" % (pin)) + file.write("#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2\n"); + # ADC_RSSI ch3 + pin = findPinByFunction('ADC_RSSI_1'): + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_3_PIN %s\n" % (pin)) + file.write("#define RSSI_ADC_CHANNEL ADC_CHN_3\n"); + + # ADC_EXT ch4 (airspeed?) + pin = findPinByFunction('ADC_EXT_1'): + if pin: + use_adc = True + file.write("#define ADC_CHANNEL_4_PIN %s\n" % (pin)) + file.write("#define AIRSPEED_ADC_CHANNEL ADC_CHN_4\n"); + + if use_adc: + file.write("#define USE_ADC\n") + file.write("#define ADC_INSTANCE ADC1\n") + # TODO: + #define ADC1_DMA_STREAM DMA2_Stream4 + file.write("// Gyro & ACC\n") # TODO file.write("// OSD\n") - # TODO + osd_spi_bus = map['variables'].get('max7456_spi_bus') + if osd_spi_bus: + file.write("#define USE_MAX7456\n") + pin = findPinByFunction('OSD_CS', map) + file.write("#define MAX7456_CS_PIN %s\n" % (pin)) + file.write("#define MAX7456_SPI_BUS %s\n" % (osd_spi_bus)) file.write("// Blackbox\n") - # TODO + # Flash: + spiflash_bus = map['variables'].get('flash_spi_bus') + if spiflash_bus: + for i in range(1, 9): + cs = findPinByFunction("FLASH_CS_%i" % (i), map) + if cs: + file.write("#define USE_FLASHFS\n") + file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") + file.write("#define USE_FLASH_M25P16\n") + file.write("#define USE_FLASH_W25N01G\n") + file.write("#define M25P16_SPI_BUS %s\n" % (spiflash_bus)) + file.write("#define M25P16_CS_PIN %s\n" % (cs)) + file.write("#define W25N01G_SPI_BUS %s\n" % (spiflash_bus)) + file.write("#define W25N01G_CS_PIN %s\n" % (cs)) + break + + # SD Card: + use_sdcard = False + for i in range(1, 9): + sdio_cmd = findPinByFunction("SDIO_CMD_%i" % (i), map) + if sdio_cmd: + if not use_sdcard: + file.write("#define USE_SDCARD\n") + file.write("#define USE_SDCARD_SDIO\n") + file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT\n") + use_sdcard = True + file.write("#define SDCARD_SDIO_4BIT\n") + file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) return @@ -227,7 +302,7 @@ def writeTarget(outputFolder, map): return def buildMap(inputFile): - map = { 'defines': [], 'features': [], 'pins': {}, 'serial': {}, 'variables': {}} + map = { 'defines': [], 'features': [], 'pins': {}, 'dmas': {}, 'serial': {}, 'variables': {}} f = open(inputFile, 'r') while True: @@ -238,6 +313,10 @@ def buildMap(inputFile): if m: map['mcu'] = {'type': m.group(1)} + m = re.search(r'^#\s+Betaflight\s+/\s+(STM32\w+)\s+\(\w+\).+$', l) + if m: + map['mcu'] = {'type': m.group(1)} + m = re.search(r'^board_name\s+(\w+)$', l) if m: map['board_name'] = m.group(1) @@ -254,7 +333,6 @@ def buildMap(inputFile): if m: map['features'].append(m.group(1)) - m = re.search(r'^resource\s+(-?\w+)\s+(\d+)\s+(\w+)$', l) if m: resource_type = m.group(1) @@ -289,10 +367,10 @@ def buildMap(inputFile): pin = 'ADC' + m.group(2) else: pin = translatePin(m.group(2)) - if not map['pins'].get(pin): - map['pins'][pin] = {} + if not map['dmas'].get(pin): + map['dmas'][pin] = {} - map['pins'][pin]['DMA'] = m.group(3) + map['dmas'][pin]['DMA'] = m.group(3) # 1 2 3 4 # pin B04: DMA1 Stream 4 Channel 5 @@ -309,11 +387,47 @@ def buildMap(inputFile): m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) if m: pin = 'ADC' + m.group(1) - if not map['pins'].get(pin): - map['pins'][pin] = {} + if not map['dmas'].get(pin): + map['dmas'][pin] = {} - map['pins'][pin]['DMA_STREAM'] = m.group(3) - map['pins'][pin]['DMA_CHANNEL'] = m.group(4) + map['dmas'][pin]['DMA_STREAM'] = m.group(3) + map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + if m: + pin = 'TIMUP' + m.group(1) + if not map['dmas'].get(pin): + map['dmas'][pin] = {} + + map['dmas'][pin]['DMA_STREAM'] = m.group(3) + map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + if m: + pin = 'ADC' + m.group(1) + if not map['dmas'].get(pin): + map['dmas'][pin] = {} + + map['dmas'][pin]['DMA_STREAM'] = m.group(3) + map['dmas'][pin]['DMA_REQUEST'] = m.group(4) + + m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + if m: + pin = 'TIMUP' + m.group(1) + if not map['dmas'].get(pin): + map['dmas'][pin] = {} + + map['dmas'][pin]['DMA_STREAM'] = m.group(3) + map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + if m: + pin = 'TIMUP' + m.group(1) + if not map['dmas'].get(pin): + map['dmas'][pin] = {} + + map['dmas'][pin]['DMA_STREAM'] = m.group(3) + map['dmas'][pin]['DMA_REQUEST'] = m.group(4) m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) if m: From be8c03d7a947bfc796ad7f4dc841694ff4c59c54 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 1 Feb 2023 17:10:56 +0100 Subject: [PATCH 004/175] compile error --- src/utils/bf2inav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 0685b17f86f..8e92b5ca53e 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -233,7 +233,7 @@ def writeTargetH(folder, map): file.write("#define RSSI_ADC_CHANNEL ADC_CHN_3\n"); # ADC_EXT ch4 (airspeed?) - pin = findPinByFunction('ADC_EXT_1'): + pin = findPinByFunction('ADC_EXT_1') if pin: use_adc = True file.write("#define ADC_CHANNEL_4_PIN %s\n" % (pin)) From da0cd56c03a5250718033c5cbd4d4f9b4914c8c6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 7 Feb 2023 17:50:44 +0100 Subject: [PATCH 005/175] All files generated. Still missing ADC info --- src/utils/bf2inav.py | 162 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 159 insertions(+), 3 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 8e92b5ca53e..d5f95db139c 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -6,6 +6,7 @@ # # The generated target will not include any servo assignments or fixed wing features. # +# TODO: ADC DMA info import sys import os @@ -73,6 +74,17 @@ def findPinByFunction(function, map): return None +def getGyroAlign(map): + bfalign = map['variables']['gyro_1_sensor_align'] + m = re.search("^CW(\d+)(FLIP)?$", bfalign) + if m: + deg = m.group(1) + flip = m.group(2) + if flip: + return "CW%s_DEG_FLIP" % (deg) + else: + return "CW%s_DEG" % (deg) + def writeTargetH(folder, map): file = open(folder + '/target.h', "w+") @@ -220,20 +232,20 @@ def writeTargetH(folder, map): file.write("#define VBAT_ADC_CHANNEL ADC_CHN_1\n"); # ADC_CURR ch2 - pin = findPinByFunction('ADC_CURR_1'): + pin = findPinByFunction('ADC_CURR_1', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_2_PIN %s\n" % (pin)) file.write("#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2\n"); # ADC_RSSI ch3 - pin = findPinByFunction('ADC_RSSI_1'): + pin = findPinByFunction('ADC_RSSI_1', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_3_PIN %s\n" % (pin)) file.write("#define RSSI_ADC_CHANNEL ADC_CHN_3\n"); # ADC_EXT ch4 (airspeed?) - pin = findPinByFunction('ADC_EXT_1') + pin = findPinByFunction('ADC_EXT_1', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_4_PIN %s\n" % (pin)) @@ -246,6 +258,21 @@ def writeTargetH(folder, map): #define ADC1_DMA_STREAM DMA2_Stream4 file.write("// Gyro & ACC\n") + for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: + found = False + for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: + val = var + supportedgyro + if val in map['defines']: + found = True + break + + if found: + file.write("#define USE_IMU_%s\n" % (supportedgyro)) + file.write("#define %s_CS_PIN %s\n" % (supportedgyro, findPinByFunction('GYRO_CS_1', map))) + file.write("#define %s_SPI_BUS BUS_SPI%s\n" % (supportedgyro, map['variables']['gyro_1_spibus'])) + file.write("#define IMU_%s_ALIGN %s\n" % (supportedgyro, getGyroAlign(map))) + + # TODO file.write("// OSD\n") osd_spi_bus = map['variables'].get('max7456_spi_bus') @@ -285,12 +312,141 @@ def writeTargetH(folder, map): file.write("#define SDCARD_SDIO_4BIT\n") file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) + file.close() return def writeTargetC(folder, map): + file = open(folder + '/target.c', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +""") + + for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: + found = False + for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: + val = var + supportedgyro + if val in map['defines']: + found = True + break + + if found: + file.write("BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) + + + file.write("\ntimerHardware_t timerHardware[] = {\n") + + motors = findPinsByFunction("MOTOR", map) + if motors: + for motor in motors: + timer = map['pins'][motor]['TIM'] + channel = map['pins'][motor]['CH'] + dma = map['dmas'].get(motor, {}).get("DMA", "0") + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_MC_MOTOR, 0, %s),\n" % (timer, channel, motor, dma)) + + servos = findPinsByFunction("SERVO", map) + if servos: + for servo in servos: + timer = map['pins'][servo]['TIM'] + channel = map['pins'][servo]['CH'] + dma = map['dmas'].get(servo, {}).get("DMA", "0") + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_MC_SERVO, 0, %s),\n" % (timer, channel, servo, dma)) + + beeper = findPinByFunction("BEEPER_1", map) + if beeper: + timer = map['pins'][beeper]['TIM'] + channel = map['pins'][beeper]['CH'] + dma = map['dmas'].get(beeper, {}).get("DMA", "0") + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (timer, channel, beeper, dma)) + + led = findPinByFunction("LED_STRIP_1", map) + if led: + timer = map['pins'][led]['TIM'] + channel = map['pins'][led]['CH'] + dma = map['dmas'].get(led, {}).get("DMA", "0") + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (timer, channel, led, dma)) + + + file.write(""" +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); +""") + + file.close() return def writeConfigC(folder, map): + file = open(folder + '/config.c', "w+") + + file.write("""/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ +""") + #//pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + #//pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + #//beeperConfigMutable()->pwmMode = true; + file.write(""" +} + +""") + + file.close() return def writeTarget(outputFolder, map): From 3d1d2802e4fd49f22e669a9d971a19bb1fc7cbe7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 7 Feb 2023 18:01:41 +0100 Subject: [PATCH 006/175] Handle missing timer information for beeper/led --- src/utils/bf2inav.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index d5f95db139c..e301d7f738b 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -382,17 +382,19 @@ def writeTargetC(folder, map): beeper = findPinByFunction("BEEPER_1", map) if beeper: - timer = map['pins'][beeper]['TIM'] - channel = map['pins'][beeper]['CH'] + timer = map['pins'].get(beeper, {}).get('TIM') + channel = map['pins'].get(beeper, {}).get('CH') dma = map['dmas'].get(beeper, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (timer, channel, beeper, dma)) + if timer and channel: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (timer, channel, beeper, dma)) led = findPinByFunction("LED_STRIP_1", map) if led: - timer = map['pins'][led]['TIM'] - channel = map['pins'][led]['CH'] + timer = map['pins'].get(led, {}).get('TIM') + channel = map['pins'].get(led, {}).get('CH') dma = map['dmas'].get(led, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (timer, channel, led, dma)) + if timer and channel: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (timer, channel, led, dma)) file.write(""" From 6269852eb62bc4a1ae68e746aa046db33a0da18e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 8 Feb 2023 21:11:48 +0100 Subject: [PATCH 007/175] Sometimes it generates a working target Works for BEFH-BETAFPVF411.config Fails for HAMO-CRAZYBEEF4SX1280.config Still need to add ADC definition --- src/utils/bf2inav.py | 128 ++++++++++++++++++++++++++++++++++++++----- 1 file changed, 113 insertions(+), 15 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index e301d7f738b..40cb2a901d7 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -48,6 +48,58 @@ def mcu2target(mcu): print("Unknown MCU: %s" % (mcu)) sys.exit(-1) +def getPortConfig(map): + mcu = map['mcu'] +#mcu STM32F405 + if mcu['type'] == 'STM32F405': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +""" + +#mcu STM32F411 + if mcu['type'] == 'STM32F411': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) +""" + +#mcu STM32F7X2 + if mcu['type'] == 'STM32F7X2': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +""" + +#mcu STM32F745 + if mcu['type'] == 'STM32F745': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +""" + +#mcu STM32H743 + if mcu['type'] == 'STM32H743': + return """ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +""" + + print("Unknown MCU: %s" % (mcu)) + sys.exit(-1) + def writeCmakeLists(outputFolder, map): file = open(outputFolder + '/CMakeLists.txt', "w+") @@ -74,6 +126,13 @@ def findPinByFunction(function, map): return None + +def getPwmOutputCount(map): + motors = findPinsByFunction("MOTOR", map) + servos = findPinsByFunction("SERVO", map) + + return len(motors) + len(servos) + def getGyroAlign(map): bfalign = map['variables']['gyro_1_sensor_align'] m = re.search("^CW(\d+)(FLIP)?$", bfalign) @@ -85,6 +144,22 @@ def getGyroAlign(map): else: return "CW%s_DEG" % (deg) +def getSerialByFunction(map, function): + for serial in map.get("serial"): + if map['serial'][serial].get('FUNCTION') == function: + return serial + + return None + +def getSerialMspDisplayPort(map): + return getSerialByFunction(map, "131072") + +def getSerialRx(map): + rx = getSerialByFunction(map, "64") + if(rx != None): + return int(rx) + 1 + return None + def writeTargetH(folder, map): file = open(folder + '/target.h', "w+") @@ -109,7 +184,10 @@ def writeTargetH(folder, map): #pragma once -#define USE_TARGET_CONFIG +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + \n""" ) @@ -139,13 +217,13 @@ def writeTargetH(folder, map): # Serial ports and usb file.write("// UARTs\n") + file.write("#define USB_IO\n") serial_count = 0 pin = findPinByFunction('USB_DETECT_1', map) if pin: - file.write("#define USB_IO\n") - file.write("#define USE_VCP\n") - file.write("#define VBUS_SENSING_PIN %s\n" % (pin)) - file.write("#define VBUS_SENSING_ENABLED\n"); + file.write("#define USE_USB_DETECT\n") + file.write("#define USB_DETECT_PIN %s\n" % (pin)) + #file.write("#define VBUS_SENSING_ENABLED\n"); serial_count += 1 for i in range(1, 9): @@ -174,17 +252,24 @@ def writeTargetH(folder, map): continue if txpin != None: - file.write("#define SOFTSERIAL%i_TX_PIN %s\n" % (idx, txpin)) + file.write("#define SOFTSERIAL_%i_TX_PIN %s\n" % (idx, txpin)) else: - file.write("#define SOFTSERIAL%i_TX_PIN %s\n" % (idx, rxpin)) + file.write("#define SOFTSERIAL_%i_TX_PIN %s\n" % (idx, rxpin)) if rxpin != None: - file.write("#define SOFTSERIAL%i_RX_PIN %s\n" % (idx, rxpin)) + file.write("#define SOFTSERIAL_%i_RX_PIN %s\n" % (idx, rxpin)) else: - file.write("#define SOFTSERIAL%i_RX_PIN %s\n" % (idx, txpin)) + file.write("#define SOFTSERIAL_%i_RX_PIN %s\n" % (idx, txpin)) file.write("#define SERIAL_PORT_COUNT %i\n" % (serial_count)) + serial_rx = getSerialRx(map) + + if serial_rx != None: + file.write("#define DEFAULT_RX_TYPE RX_TYPE_SERIAL\n") + file.write("#define SERIALRX_PROVIDER SERIALRX_CRSF\n") + file.write("#define SERIALRX_UART SERIAL_PORT_USART%s\n" % (serial_rx)) + file.write("// SPI\n") use_spi_defined = False for i in range(1, 9): @@ -278,9 +363,9 @@ def writeTargetH(folder, map): osd_spi_bus = map['variables'].get('max7456_spi_bus') if osd_spi_bus: file.write("#define USE_MAX7456\n") - pin = findPinByFunction('OSD_CS', map) + pin = findPinByFunction('OSD_CS_1', map) file.write("#define MAX7456_CS_PIN %s\n" % (pin)) - file.write("#define MAX7456_SPI_BUS %s\n" % (osd_spi_bus)) + file.write("#define MAX7456_SPI_BUS BUS_SPI%s\n" % (osd_spi_bus)) file.write("// Blackbox\n") # Flash: spiflash_bus = map['variables'].get('flash_spi_bus') @@ -292,9 +377,9 @@ def writeTargetH(folder, map): file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") file.write("#define USE_FLASH_M25P16\n") file.write("#define USE_FLASH_W25N01G\n") - file.write("#define M25P16_SPI_BUS %s\n" % (spiflash_bus)) + file.write("#define M25P16_SPI_BUS BUS_SPI%s\n" % (spiflash_bus)) file.write("#define M25P16_CS_PIN %s\n" % (cs)) - file.write("#define W25N01G_SPI_BUS %s\n" % (spiflash_bus)) + file.write("#define W25N01G_SPI_BUS BUS_SPI%s\n" % (spiflash_bus)) file.write("#define W25N01G_CS_PIN %s\n" % (cs)) break @@ -312,6 +397,19 @@ def writeTargetH(folder, map): file.write("#define SDCARD_SDIO_4BIT\n") file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) + file.write("\n// Otehrs\n\n") + + pwm_outputs = getPwmOutputCount(map) + file.write("#define MAX_PWM_OUTPUT_PORTS %i\n" % (pwm_outputs)) + file.write("#define USE_SERIAL_4WAY_BLHELI_INTERFACE\n") + + file.write("#define USE_DSHOT\n"); + file.write("#define USE_ESC_SENSOR\n"); + + port_config = getPortConfig(map) + + file.write(port_config) + file.close() return @@ -346,7 +444,7 @@ def writeTargetC(folder, map): #include "drivers/pwm_mapping.h" #include "drivers/timer.h" #include "drivers/pinio.h" -#include "drivers/sensor.h" +//#include "drivers/sensor.h" """) @@ -359,7 +457,7 @@ def writeTargetC(folder, map): break if found: - file.write("BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) + file.write("//BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) file.write("\ntimerHardware_t timerHardware[] = {\n") From 98186bc20e2ad8f0db4cf1a67f2fa1c6df179fea Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 8 Feb 2023 21:45:33 +0100 Subject: [PATCH 008/175] Add USE_VCP --- src/utils/bf2inav.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 40cb2a901d7..7c39e434601 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -14,6 +14,8 @@ import getopt import re import json +import random +import string version = '0.1' @@ -191,7 +193,8 @@ def writeTargetH(folder, map): \n""" ) - file.write("#define TARGET_BOARD_IDENTIFIER \"B2IN\"\n") + board_id = ''.join(random.choice(string.ascii_uppercase) for i in range(4)) + file.write("#define TARGET_BOARD_IDENTIFIER \"%s\"\n" % (board_id)) file.write("#define USBD_PRODUCT_STRING \"%s\"\n" % (map['board_name'])) # beeper @@ -218,6 +221,7 @@ def writeTargetH(folder, map): # Serial ports and usb file.write("// UARTs\n") file.write("#define USB_IO\n") + file.write("#define USB_VCP\n") serial_count = 0 pin = findPinByFunction('USB_DETECT_1', map) if pin: From ea67957d68fce539e02f4c90682dc50d3f869db1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 8 Sep 2023 22:35:20 +0200 Subject: [PATCH 009/175] Small update + unofficial target for betafpvf411 5a aio --- src/main/target/BETAFPVF411/CMakeLists.txt | 1 + src/main/target/BETAFPVF411/config.c | 33 ++++++ src/main/target/BETAFPVF411/target.c | 43 ++++++++ src/main/target/BETAFPVF411/target.h | 120 +++++++++++++++++++++ src/utils/bf2inav.py | 6 +- 5 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 src/main/target/BETAFPVF411/CMakeLists.txt create mode 100644 src/main/target/BETAFPVF411/config.c create mode 100644 src/main/target/BETAFPVF411/target.c create mode 100644 src/main/target/BETAFPVF411/target.h diff --git a/src/main/target/BETAFPVF411/CMakeLists.txt b/src/main/target/BETAFPVF411/CMakeLists.txt new file mode 100644 index 00000000000..bc5f1decb7f --- /dev/null +++ b/src/main/target/BETAFPVF411/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f411xe(BETAFPVF411 SKIP_RELEASES) diff --git a/src/main/target/BETAFPVF411/config.c b/src/main/target/BETAFPVF411/config.c new file mode 100644 index 00000000000..cd5aa30d718 --- /dev/null +++ b/src/main/target/BETAFPVF411/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} + diff --git a/src/main/target/BETAFPVF411/target.c b/src/main/target/BETAFPVF411/target.c new file mode 100644 index 00000000000..34dc53522f2 --- /dev/null +++ b/src/main/target/BETAFPVF411/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +//#include "drivers/sensor.h" + +//BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); +//BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h new file mode 100644 index 00000000000..f2f5f7bc5db --- /dev/null +++ b/src/main/target/BETAFPVF411/target.h @@ -0,0 +1,120 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + + + +#define TARGET_BOARD_IDENTIFIER "LGPC" +#define USBD_PRODUCT_STRING "BETAFPVF411" +// Beeper +#define USE_BEEPER +#define BEEPER PB2 +#define BEEPER_INVERTED +// Leds +//#define USE_LED_STRIP +//#define WS2811_PIN PA8 +#define LED0 PC13 +#define LED1 PC14 +// UARTs +#define USB_IO +#define USE_VCP +#define USE_USB_DETECT +#define USB_DETECT_PIN PC15 +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA8 +#define SOFTSERIAL_1_RX_PIN PA8 +//#define SOFTSERIAL_1_TX_PIN PB3 +//#define SOFTSERIAL_1_RX_PIN PB3 +#define USE_SOFTSERIAL2 +#define SOFTSERIAL_2_TX_PIN PB10 +#define SOFTSERIAL_2_RX_PIN PB10 +//#define USE_SOFTSERIAL3 +//#define SOFTSERIAL_3_TX_PIN PA8 +//#define SOFTSERIAL_3_RX_PIN PA8 +#define SERIAL_PORT_COUNT 5 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART1 +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +// ADC +#define ADC_CHANNEL_1_PIN PB0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PB1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define USE_ADC +#define ADC_INSTANCE ADC1 +// Gyro & ACC +#define USE_IMU_BMI270 +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_BUS BUS_SPI1 +#define IMU_BMI270_ALIGN CW180_DEG +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define IMU_MPU6000_ALIGN CW180_DEG +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASH_M25P16 +#define USE_FLASH_W25N01G +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PA0 +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PA0 + +// Otehrs + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 7c39e434601..788a1fc871a 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -239,10 +239,12 @@ def writeTargetH(folder, map): else: continue - if txpin: - file.write("#define UART%i_TX_PIN %s\n" % (i, txpin)) if rxpin: file.write("#define UART%i_RX_PIN %s\n" % (i, rxpin)) + if txpin: + file.write("#define UART%i_TX_PIN %s\n" % (i, txpin)) + else: + file.write("#define UART%i_TX_PIN %s\n" % (i, rxpin)) # soft serial for i in range(11, 19): From 82fcff6940d9ab35b85aadf65e321713cfe31ca2 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 5 Dec 2023 22:22:23 +0100 Subject: [PATCH 010/175] Add remarks about unified-targets being replaced with config.h --- src/utils/bf2inav.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 788a1fc871a..77b07e9924d 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -7,6 +7,14 @@ # The generated target will not include any servo assignments or fixed wing features. # # TODO: ADC DMA info +# BF build API: +# target lists +# https://build.betaflight.com/api/targets +# target release info: +# https://build.betaflight.com/api/targets/{TARGET} +# load target: +# Unified targets are deprecated, replaced by https://github.com/betaflight/config + import sys import os From 23e0741105b33394d971d1cf22110078d0fcc8fa Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 9 Dec 2023 17:55:48 +0100 Subject: [PATCH 011/175] Add file with timer pin mappings. Currently only cover AT32F435 --- src/utils/timer_pins.yaml | 171 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) create mode 100644 src/utils/timer_pins.yaml diff --git a/src/utils/timer_pins.yaml b/src/utils/timer_pins.yaml new file mode 100644 index 00000000000..59a70130a0b --- /dev/null +++ b/src/utils/timer_pins.yaml @@ -0,0 +1,171 @@ +AT32F435: + PA0: + - TMR2: 'CH1' + - TMR5: 'CH1' + PA1: + - TMR2: 'CH2' + - TMR5: 'CH2' + PA2: + - TMR2: 'CH3' + - TMR5: 'CH3' + - TMR9: 'CH1' + PA3: + - TMR2: 'CH4' + - TMR5: 'CH4' + - TMR9: 'CH2' + PA5: + - TMR2: 'CH1' + - TMR8: 'CH1C' + PA6: + - TMR3: 'CH1' + PA7: + - TMR1: 'CH1C' + - TMR3: 'CH2' + - TMR8: 'CH1C' + PA8: + - TMR1: 'CH1' + PA9: + - TMR1: 'CH2' + PA10: + - TMR1: 'CH3' + PA11: + - TMR1: 'CH4' + PA15: + - TMR2: 'CH1' + PB0: + - TMR1: 'CH2C' + - TMR3: 'CH3' + - TMR8: 'CH2C' + PB1: + - TMR1: 'CH3C' + - TMR3: 'CH4' + - TMR8: 'CH3C' + PB2: + - TMR2: 'CH4' + - TMR20: 'CH1' + PB3: + - TMR2: 'CH2' + PB4: + - TMR3: 'CH1' + PB5: + - TMR3: 'CH2' + PB6: + - TMR4: 'CH1' + PB7: + - TMR4: 'CH2' + PB8: + - TMR2: 'CH1' + - TMR4: 'CH3' + - TMR10: 'CH1' + PB9: + - TMR2: 'CH2' + - TMR4: 'CH4' + - TMR11: 'CH1' + PB10: + - TMR2: 'CH3' + PB11: + - TMR2: 'CH4' + - TMR5: 'CH4' + PB12: + - TMR5: 'CH1' + PB13: + - TMR1: 'CH1C' + PB14: + - TMR1: 'CH2C' + - TMR8: 'CH2C' + PB15: + - TMR1: 'CH3C' + - TMR8: 'CH3C' + PC2: + - TMR20: 'CH2' + PC4: + - TMR9: 'CH1' + PC5: + - TMR9: 'CH2' + PC6: + - TMR3: 'CH1' + - TMR8: 'CH1' + PC7: + - TMR3: 'CH2' + - TMR8: 'CH2' + PC8: + - TMR3: 'CH3' + - TMR8: 'CH3' + PC9: + - TMR3: 'CH4' + - TMR8: 'CH4' + PC10: + - TMR5: 'CH2' + PC11: + - TMR5: 'CH3' + PC12: + - TMR11: 'CH1C' + PD12: + - TMR4: 'CH1' + PD13: + - TMR4: 'CH2' + PD14: + - TMR4: 'CH3' + PD15: + - TMR4: 'CH4' + PE3: + - TMR3: 'CH1' + PE4: + - TMR3: 'CH2' + PE5: + - TMR3: 'CH3' + - TMR9: 'CH1' + PE6: + - TMR3: 'CH4' + - TMR9: 'CH2' + PE8: + - TMR1: CH1C + PE9: + - TMR1: CH1 + PE10: + - TMR1: CH2C + PE11: + - TMR1: CH2 + PE12: + - TMR1: CH3C + PE13: + - TMR1: CH3 + PE14: + - TMR1: CH4 + PF2: + - TMR20: CH3 + PF3: + - TMR20: CH4 + PF4: + - TMR20: CH1C + PF5: + - TMR20: CH2C + PF6: + - TMR20: CH4 + - TMR10: CH1 + PF7: + - TMR11: CH1 + PF10: + - TMR5: CH4 + PF12: + - TMR20: CH1 + PF13: + - TMR20: CH2 + PF14: + - TMR20: CH3 + PF15: + - TMR20: CH4 + PG0: + - TMR20: CH1C + PG1: + - TMR20: CH2C + PG2: + - TMR20: CH3C + PH2: + - TMR5: CH1 + PH3: + - TMR5: CH2 + + + + \ No newline at end of file From d5e68df974a64f6ffa311a77fa55e4c1c6eb3499 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 9 Dec 2023 19:09:48 +0100 Subject: [PATCH 012/175] Add STM32F405 --- src/utils/timer_pins.yaml | 145 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 145 insertions(+) diff --git a/src/utils/timer_pins.yaml b/src/utils/timer_pins.yaml index 59a70130a0b..9c80252278f 100644 --- a/src/utils/timer_pins.yaml +++ b/src/utils/timer_pins.yaml @@ -165,7 +165,152 @@ AT32F435: - TMR5: CH1 PH3: - TMR5: CH2 +STM32F405: + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PF6: + - TIM10: CH1 + PF7: + - TIM11: CH1 + PF8: + - TIM13: CH1 + PF9: + - TIM14: CH1 + PA0: + - TIM5: CH1 + PA1: + - TIM5: CH2 + - TIM2: CH2 + PA2: + - TIM5: CH3 + - TIM9: CH1 + - TIM2: CH3 + PA3: + - TIM5: CH4 + - TIM9: CH2 + - TIM2: CH4 + PA5: + - TIM8: CH1N + PA6: + - TIM13: CH1 + - TIM3: CH1 + PA7: + - TIM8: CH1N + - TIM14: CH1 + - TIM3: CH2 + - TIM1: CH1N + PB0: + - TIM3: CH3 + - TIM8: CH2N + - TIM1: CH2N + PB1: + - TIM3: CH4 + - TIM8: CH3N + - TIM1: CH3N + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH3 + PH6: + - TIM13: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM12: CH1 + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM8: CH3N + - TIM12: CH2 + PD12: + - TIM14: CH1 + PD13: + - TIM14: CH2 + PD14: + - TIM4: CH3 + PD5: + - TIM4: CH3 + PC6: + - TIM8: CH1 + - TIM3: CH1 + PC7: + - TIM8: CH2 + - TIM3: CH2 + PC8: + - TIM8: CH3 + - TIM3: CH3 + PC9: + - TIM8: CH4 + - TIM3: CH4 + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH14: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 + + + \ No newline at end of file From 65f6329502b9c47d29b34c28c1af4f3b6f1f6ebd Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 9 Dec 2023 20:44:47 +0100 Subject: [PATCH 013/175] Add STM32H743 --- src/utils/timer_pins.yaml | 171 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) diff --git a/src/utils/timer_pins.yaml b/src/utils/timer_pins.yaml index 9c80252278f..87d578021d3 100644 --- a/src/utils/timer_pins.yaml +++ b/src/utils/timer_pins.yaml @@ -307,6 +307,177 @@ STM32F405: - TIM8: CH2 PI7: - TIM8: CH3 +STM32H743: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + - TIM15: CH1N + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM15: CH1 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM16: CH1N + - TIM4: CH1 + PB7: + - TIM17: CH1N + - TIM4: CH2 + PB8: + - TIM16: CH1 + - TIM4: CH3 + PB9: + - TIM17: CH1 + - TIM4: CH4 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM12: CH1 + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM12: CH2 + - TIM8: CH3N + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE4: + - TIM15: CH1N + PE5: + - TIM15: CH1 + PE6: + - TIM15: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM16: CH1 + PF7: + - TIM17: CH1 + PF8: + - TIM16: CH1N + PF9: + - TIM17: CH1N + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 + PJ6: + - TIM8: CH2 + PJ7: + - TIM8: CH2N + PJ8: + - TIM1: CH3N + - TIM8: CH1 + PJ9: + - TIM1: CH3 + - TIM8: CH1N + PJ10: + - TIM1: CH2N + - TIM8: CH2 + PJ11: + - TIM1: CH2 + - TIM8: CH2N + PK0: + - TIM1: CH1N + - TIM8: CH3 + PK1: + - TIM1: CH1 + - TIM8: CH3N + + + + From d744126aa381d1988ef8323ac7221accaf5ce32e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 15:38:12 +0100 Subject: [PATCH 014/175] More updates to read config.h instead of unified targets --- src/utils/bf2inav.py | 281 +++++++++++++++++++++++++------------------ 1 file changed, 164 insertions(+), 117 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 77b07e9924d..0ddfaee348f 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -66,7 +66,10 @@ def getPortConfig(map): #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff + """ #mcu STM32F411 @@ -75,7 +78,9 @@ def getPortConfig(map): #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff """ #mcu STM32F7X2 @@ -85,26 +90,31 @@ def getPortConfig(map): #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff """ #mcu STM32F745 if mcu['type'] == 'STM32F745': return """ -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff """ #mcu STM32H743 if mcu['type'] == 'STM32H743': return """ -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff +#define TARGET_IO_PORTG 0xffff """ print("Unknown MCU: %s" % (mcu)) @@ -482,7 +492,7 @@ def writeTargetC(folder, map): timer = map['pins'][motor]['TIM'] channel = map['pins'][motor]['CH'] dma = map['dmas'].get(motor, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_MC_MOTOR, 0, %s),\n" % (timer, channel, motor, dma)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (timer, channel, motor, dma)) servos = findPinsByFunction("SERVO", map) if servos: @@ -490,7 +500,7 @@ def writeTargetC(folder, map): timer = map['pins'][servo]['TIM'] channel = map['pins'][servo]['CH'] dma = map['dmas'].get(servo, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_MC_SERVO, 0, %s),\n" % (timer, channel, servo, dma)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (timer, channel, servo, dma)) beeper = findPinByFunction("BEEPER_1", map) if beeper: @@ -572,148 +582,185 @@ def writeTarget(outputFolder, map): return def buildMap(inputFile): - map = { 'defines': [], 'features': [], 'pins': {}, 'dmas': {}, 'serial': {}, 'variables': {}} + map = { 'defines': {}, 'use_defines': [], 'features': ['FEATURE_OSD', 'FEATURE_TELEMETRY', 'FEATURE_CURRENT_METER', 'FEATURE_VBAT', 'FEATURE_TX_PROF_SEL', 'FEATURE_BLACKBOX'], 'pins': {}, 'funcs': {}, 'timer_pin_map': {}} f = open(inputFile, 'r') while True: l = f.readline() if not l: break - m = re.search(r'^#mcu\s+([0-9A-Za-z]+)$', l) - if m: - map['mcu'] = {'type': m.group(1)} - - m = re.search(r'^#\s+Betaflight\s+/\s+(STM32\w+)\s+\(\w+\).+$', l) + m = re.search(r'^#define\s+FC_TARGET_MCU\s+([0-9A-Za-z]+)$', l) if m: map['mcu'] = {'type': m.group(1)} - m = re.search(r'^board_name\s+(\w+)$', l) + m = re.search(r'^#define\s+BOARD_NAME\s+(\w+)$', l) if m: map['board_name'] = m.group(1) - m = re.search(r'^manufacturer_id\s+(\w+)$', l) + m = re.search(r'^#define\s+MANUFACTURER_ID\s+(\w+)$', l) if m: map['manufacturer_id'] = m.group(1) + + m = re.search(r'^#define\s+(USE_\w+)$', l) + if m: + map['use_defines'].append(m.group(1)) - m = re.search(r'^#define\s+(\w+)$', l) + m = re.search('^\s*#define\s+DEFAULT_FEATURES\s+\((.+?)\)\s*$', l) if m: - map['defines'].append(m.group(1)) + features = m.group(1).split('|') + for feat in features: + feat = feat.strip() + if not feat in map['features']: + map['features'].append(feat) - m = re.search(r'^feature\s+(-?\w+)$', l) + m = re.search(r'^#define\s+(\w+)\s+(\S+)\s*$', l) if m: - map['features'].append(m.group(1)) + map['defines'][m.group(1)] = m.group(2) - m = re.search(r'^resource\s+(-?\w+)\s+(\d+)\s+(\w+)$', l) - if m: - resource_type = m.group(1) - resource_index = m.group(2) - pin = translatePin(m.group(3)) - if not map['pins'].get(pin): - map['pins'][pin] = {} - map['pins'][pin]['function'] = translateFunctionName(resource_type, resource_index) - m = re.search(r'^timer\s+(\w+)\s+AF(\d+)$', l) + # i: timer index + # p: pin + # o: timer channel? 1 = first + # d: dma opts + # i p o d + # TIMER_PIN_MAP( 0, PB8 , 2, -1) \ + m = re.search(r'^\s*TIMER_PIN_MAP\s*\(\s*(\d+)\s*,\s*([0-9A-Za-z]+)\s*,\s*(\d+)\s*,\s*(-?\d+)\s*\).+', l) if m: - pin = translatePin(m.group(1)) - if not map['pins'].get(pin): - map['pins'][pin] = {} - - map['pins'][pin]['AF'] = m.group(2) + map['timer_pin_map'][m.group(1)] = { + 'i': m.group(1), + 'p': m.group(2), + 'o': m.group(3), + 'd': m.group(4) + } - m = re.search(r'^#\s*pin\s+(\w+):\s*(TIM\d+)\s+(CH\d+).+$', l) + + m = re.search('^\s*#define\s+(\w+)_PIN\s+([A-Z0-9]+)\s*$', l) if m: - pin = translatePin(m.group(1)) + pin = m.group(2) + func = m.group(1) + if not map['funcs'].get(func): + map['funcs'][func] = {} + + map['funcs'][func] = pin + if not map['pins'].get(pin): map['pins'][pin] = {} - - map['pins'][pin]['TIM'] = m.group(2) - map['pins'][pin]['CH'] = m.group(3) - - m = re.search(r'^dma\s+([A-Za-z0-9]+)\s+([A-Za-z0-9]+)\s+(\d+).*$', l) - if m: - if(m.group(1) == 'ADC'): - pin = 'ADC' + m.group(2) - else: - pin = translatePin(m.group(2)) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} + map['pins'][pin] = func + + + #m = re.search(r'^feature\s+(-?\w+)$', l) + #if m: + # map['features'].append(m.group(1)) + + #m = re.search(r'^resource\s+(-?\w+)\s+(\d+)\s+(\w+)$', l) + #if m: + # resource_type = m.group(1) + # resource_index = m.group(2) + # pin = translatePin(m.group(3)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + + # map['pins'][pin]['function'] = translateFunctionName(resource_type, resource_index) + + #m = re.search(r'^timer\s+(\w+)\s+AF(\d+)$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} + # + # map['pins'][pin]['AF'] = m.group(2) + + #m = re.search(r'^#\s*pin\s+(\w+):\s*(TIM\d+)\s+(CH\d+).+$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} - map['dmas'][pin]['DMA'] = m.group(3) + # map['pins'][pin]['TIM'] = m.group(2) + # map['pins'][pin]['CH'] = m.group(3) + + #m = re.search(r'^dma\s+([A-Za-z0-9]+)\s+([A-Za-z0-9]+)\s+(\d+).*$', l) + #if m: + # if(m.group(1) == 'ADC'): + # pin = 'ADC' + m.group(2) + # else: + # pin = translatePin(m.group(2)) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA'] = m.group(3) # 1 2 3 4 # pin B04: DMA1 Stream 4 Channel 5 - m = re.search(r'^#\s+pin\s+(\w+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) - if m: - pin = translatePin(m.group(1)) - if not map['pins'].get(pin): - map['pins'][pin] = {} + #m = re.search(r'^#\s+pin\s+(\w+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = translatePin(m.group(1)) + # if not map['pins'].get(pin): + # map['pins'][pin] = {} - map['pins'][pin]['DMA_STREAM'] = m.group(3) - map['pins'][pin]['DMA_CHANNEL'] = m.group(4) + # map['pins'][pin]['DMA_STREAM'] = m.group(3) + # map['pins'][pin]['DMA_CHANNEL'] = m.group(4) - m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) - if m: - pin = 'ADC' + m.group(1) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} + #m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'ADC' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} - map['dmas'][pin]['DMA_STREAM'] = m.group(3) - map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) - - m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) - if m: - pin = 'TIMUP' + m.group(1) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + #if m: + # pin = 'ADC' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} - map['dmas'][pin]['DMA_STREAM'] = m.group(3) - map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) - - m = re.search(r'^#\s+ADC\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) - if m: - pin = 'ADC' + m.group(1) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} - - map['dmas'][pin]['DMA_STREAM'] = m.group(3) - map['dmas'][pin]['DMA_REQUEST'] = m.group(4) - - m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) - if m: - pin = 'TIMUP' + m.group(1) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_REQUEST'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Channel\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} - map['dmas'][pin]['DMA_STREAM'] = m.group(3) - map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) - - m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) - if m: - pin = 'TIMUP' + m.group(1) - if not map['dmas'].get(pin): - map['dmas'][pin] = {} - - map['dmas'][pin]['DMA_STREAM'] = m.group(3) - map['dmas'][pin]['DMA_REQUEST'] = m.group(4) + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_CHANNEL'] = m.group(4) + + #m = re.search(r'^#\s+TIMUP\s+(\d+):\s+(DMA\d+)\s+Stream\s+(\d+)\s+Request\s+(\d+)\s*$', l) + #if m: + # pin = 'TIMUP' + m.group(1) + # if not map['dmas'].get(pin): + # map['dmas'][pin] = {} + # map['dmas'][pin]['DMA_STREAM'] = m.group(3) + # map['dmas'][pin]['DMA_REQUEST'] = m.group(4) - m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) - if m: - idx = m.group(1) - if not map['serial'].get(idx): - map['serial'][idx] = {} + #m = re.search(r'^serial\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)\s+(\d+)$', l) + #if m: + # idx = m.group(1) + # if not map['serial'].get(idx): + # map['serial'][idx] = {} - map['serial'][idx]['FUNCTION'] = m.group(2) - map['serial'][idx]['MSP_BAUD'] = m.group(3) - map['serial'][idx]['GPS_BAUD'] = m.group(4) - map['serial'][idx]['TELEMETRY_BAUD'] = m.group(5) - map['serial'][idx]['BLACKBOX_BAUD'] = m.group(6) - - m = re.search(r'^set\s+(\w+)\s*=\s*(\w+)$', l) - if m: - map['variables'][m.group(1)] = m.group(2) + # map['serial'][idx]['FUNCTION'] = m.group(2) + # map['serial'][idx]['MSP_BAUD'] = m.group(3) + # map['serial'][idx]['GPS_BAUD'] = m.group(4) + # map['serial'][idx]['TELEMETRY_BAUD'] = m.group(5) + # map['serial'][idx]['BLACKBOX_BAUD'] = m.group(6) + + #m = re.search(r'^set\s+(\w+)\s*=\s*(\w+)$', l) + #if m: + # map['variables'][m.group(1)] = m.group(2) return map From ca0ff6a5d6b53d8553cd0735ea9f6a7fd591ffc5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 17:09:01 +0100 Subject: [PATCH 015/175] target.h mostly converted to read config.h --- src/utils/bf2inav.py | 204 ++++++++++++++++++++++++++++--------------- 1 file changed, 135 insertions(+), 69 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 0ddfaee348f..72737eebefe 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -54,8 +54,16 @@ def mcu2target(mcu): #mcu STM32H743 if mcu['type'] == 'STM32H743': return 'target_stm32h743xi' + +#mcu 'AT32F435G' + if mcu['type'] == 'AT32F435G': + return 'target_at32f43x_xGT7' + +#mcu 'AT32F435M' + if mcu['type'] == 'AT32F435M': + return 'target_at32f43x_xMT7' - print("Unknown MCU: %s" % (mcu)) + print("Unknown MCU: %s!" % (mcu)) sys.exit(-1) def getPortConfig(map): @@ -115,6 +123,24 @@ def getPortConfig(map): #define TARGET_IO_PORTE 0xffff #define TARGET_IO_PORTF 0xffff #define TARGET_IO_PORTG 0xffff +""" + +#mcu 'AT32F435G' + if mcu['type'] == 'AT32F435G': + return """#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTH 0xffff +""" + +#mcu 'AT32F435M' + if mcu['type'] == 'AT32F435M': + return """#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTH 0xffff """ print("Unknown MCU: %s" % (mcu)) @@ -132,18 +158,17 @@ def writeCmakeLists(outputFolder, map): def findPinsByFunction(function, map): result = [] - for pin in map['pins']: - pattern = "^%s_" % (function) - if map['pins'][pin].get('function') and re.search(pattern, map['pins'][pin]['function']): - result.append(pin) + for func in map['funcs']: + pattern = "^%s" % (function) + if re.search(pattern, func): + result.append(map['funcs'][func]) return result def findPinByFunction(function, map): - for pin in map['pins']: - if map['pins'][pin].get('function') and map['pins'][pin]['function'] == function: - return pin - + if function in map['funcs']: + return map['funcs'][function] + return None @@ -154,15 +179,16 @@ def getPwmOutputCount(map): return len(motors) + len(servos) def getGyroAlign(map): - bfalign = map['variables']['gyro_1_sensor_align'] - m = re.search("^CW(\d+)(FLIP)?$", bfalign) - if m: - deg = m.group(1) - flip = m.group(2) - if flip: - return "CW%s_DEG_FLIP" % (deg) - else: - return "CW%s_DEG" % (deg) + bfalign = map['defines']['GYRO_1_ALIGN'] + return bfalign + #m = re.search("^CW(\d+)(FLIP)?$", bfalign) + #if m: + # deg = m.group(1) + # flip = m.group(2) + # if flip: + # return "CW%s_DEG_FLIP" % (deg) + # else: + # return "CW%s_DEG" % (deg) def getSerialByFunction(map, function): for serial in map.get("serial"): @@ -217,31 +243,38 @@ def writeTargetH(folder, map): # beeper file.write("// Beeper\n") - pin = findPinByFunction('BEEPER_1', map) + pin = findPinByFunction('BEEPER', map) + print ("BEEPER") if pin: + print ("BEEPER: %s" % (pin)) file.write("#define USE_BEEPER\n") file.write("#define BEEPER %s\n" % (pin)) - if map['variables'].get('beeper_inversion', 'OFF') == 'ON': + if 'BEEPER_INVERTED' in map['empty_defines']: file.write("#define BEEPER_INVERTED\n") + print ("INVERTED") # Leds file.write("// Leds\n") - pin = findPinByFunction('LED_STRIP_1', map) + pin = findPinByFunction('LED_STRIP', map) + print ("LED") if pin: + print ("LED: %s" % (pin)) file.write('#define USE_LED_STRIP\n') file.write("#define WS2811_PIN %s\n" % (pin)) - for i in range(1, 9): - pin = findPinByFunction("LED_%i" % (i), map) + for i in range(0, 9): + pin = findPinByFunction("LED%i" % (i), map) if pin: - file.write("#define LED%i %s\n" % (i-1, pin)) + print ("LED%i: %s" % (i, pin)) + file.write("#define LED%i %s\n" % (i, pin)) # Serial ports and usb + print ("SERIAL") file.write("// UARTs\n") file.write("#define USB_IO\n") file.write("#define USB_VCP\n") serial_count = 0 - pin = findPinByFunction('USB_DETECT_1', map) + pin = findPinByFunction('USB_DETECT', map) if pin: file.write("#define USE_USB_DETECT\n") file.write("#define USB_DETECT_PIN %s\n" % (pin)) @@ -249,9 +282,10 @@ def writeTargetH(folder, map): serial_count += 1 for i in range(1, 9): - txpin = findPinByFunction("SERIAL_TX_%i" % (i), map) - rxpin = findPinByFunction("SERIAL_RX_%i" % (i), map) + txpin = findPinByFunction("UART%i_TX" % (i), map) + rxpin = findPinByFunction("UART%i_RX" % (i), map) if txpin or rxpin: + print ("UART%s" % (i)) file.write("#define USE_UART%i\n" % (i)) serial_count+=1 else: @@ -265,11 +299,12 @@ def writeTargetH(folder, map): file.write("#define UART%i_TX_PIN %s\n" % (i, rxpin)) # soft serial - for i in range(11, 19): - txpin = findPinByFunction("SERIAL_TX_%i" % (i), map) - rxpin = findPinByFunction("SERIAL_RX_%i" % (i), map) - idx = i - 10 + for i in range(1, 9): + txpin = findPinByFunction("SOFTSERIAL%i_TX" % (i), map) + rxpin = findPinByFunction("SOFTSERIAL%i_RX" % (i), map) + idx = i if txpin != None or rxpin != None: + print ("SOFTUART%s" % (i)) file.write("#define USE_SOFTSERIAL%i\n" % (idx)) serial_count+=1 else: @@ -287,19 +322,22 @@ def writeTargetH(folder, map): file.write("#define SERIAL_PORT_COUNT %i\n" % (serial_count)) - serial_rx = getSerialRx(map) + file.write("#define DEFAULT_RX_TYPE RX_TYPE_SERIAL\n") + file.write("#define SERIALRX_PROVIDER SERIALRX_CRSF\n") + + # TODO: map default serial uart + #serial_rx = getSerialRx(map) + serial_rx = None if serial_rx != None: - file.write("#define DEFAULT_RX_TYPE RX_TYPE_SERIAL\n") - file.write("#define SERIALRX_PROVIDER SERIALRX_CRSF\n") file.write("#define SERIALRX_UART SERIAL_PORT_USART%s\n" % (serial_rx)) file.write("// SPI\n") use_spi_defined = False for i in range(1, 9): - sckpin = findPinByFunction("SPI_SCK_%i" % (i), map) - misopin = findPinByFunction("SPI_MISO_%i" % (i), map) - mosipin = findPinByFunction("SPI_MOSI_%i" % (i), map) + sckpin = findPinByFunction("SPI%i_SCK" % (i), map) + misopin = findPinByFunction("SPI%i_SDO" % (i), map) + mosipin = findPinByFunction("SPI%i_SDI" % (i), map) if (sckpin or misopin or mosipin): if (not use_spi_defined): use_spi_defined = True @@ -316,10 +354,11 @@ def writeTargetH(folder, map): file.write("// I2C\n") use_i2c_defined = False for i in range(1, 9): - sclpin = findPinByFunction("I2C_SCL_%i" % (i), map) - sdapin = findPinByFunction("I2C_SDA_%i" % (i), map) + sclpin = findPinByFunction("I2C%i_SCL" % (i), map) + sdapin = findPinByFunction("I2C%i_SDA" % (i), map) if (sclpin or sdapin): if (not use_i2c_defined): + print ("I2C") use_i2c_defined = True file.write("#define USE_I2C\n") file.write("#define USE_I2C_DEVICE_%i\n" % (i)) @@ -328,26 +367,36 @@ def writeTargetH(folder, map): file.write("#define I2C%i_SCL %s\n" % (i, sclpin)) if sdapin: file.write("#define I2C%i_SDA %s\n" % (i, sdapin)) + + if 'MAG_I2C_INSTANCE' in map['defines']: + file.write("// MAG\n") + bfintance = map['defines']['MAG_I2C_INSTANCE'] + file.write("#define USE_MAG\n") + file.write("#define USE_MAG_ALL\n") + # (I2CDEV_1) + m = re.search(r'^\s*#define\s+MAG_I2C_INSTANCE\s+\(?I2CDEV_(\d+)\)?\s*$') + if m: + file.write("#define MAG_I2C_BUS BUS_I2C%i" % (m.group(1))) file.write("// ADC\n") # ADC_BATT ch1 use_adc = False - pin = findPinByFunction('ADC_BATT_1', map) + pin = findPinByFunction('ADC_VBAT', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_1_PIN %s\n" % (pin)) file.write("#define VBAT_ADC_CHANNEL ADC_CHN_1\n"); # ADC_CURR ch2 - pin = findPinByFunction('ADC_CURR_1', map) + pin = findPinByFunction('ADC_CURR', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_2_PIN %s\n" % (pin)) file.write("#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2\n"); # ADC_RSSI ch3 - pin = findPinByFunction('ADC_RSSI_1', map) + pin = findPinByFunction('ADC_RSSI', map) if pin: use_adc = True file.write("#define ADC_CHANNEL_3_PIN %s\n" % (pin)) @@ -371,41 +420,58 @@ def writeTargetH(folder, map): found = False for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: val = var + supportedgyro - if val in map['defines']: + if val in map['empty_defines']: found = True break if found: + print (supportedgyro) file.write("#define USE_IMU_%s\n" % (supportedgyro)) - file.write("#define %s_CS_PIN %s\n" % (supportedgyro, findPinByFunction('GYRO_CS_1', map))) - file.write("#define %s_SPI_BUS BUS_SPI%s\n" % (supportedgyro, map['variables']['gyro_1_spibus'])) + file.write("#define %s_CS_PIN %s\n" % (supportedgyro, findPinByFunction('GYRO_1_CS', map))) + file.write("#define %s_SPI_BUS BUS_%s\n" % (supportedgyro, map['defines']['GYRO_1_SPI_INSTANCE'])) file.write("#define IMU_%s_ALIGN %s\n" % (supportedgyro, getGyroAlign(map))) + # TODO: SPI BARO + if 'USE_BARO' in map['empty_defines']: + print ("BARO") + file.write("// BARO\n") + file.write("#define USE_BARO\n") + file.write("#define USE_BARO_ALL\n") + if 'BARO_I2C_INSTANCE' in map['defines']: + m = re.search('I2CDEV_(\d+)', map['defines']['BARO_I2C_INSTANCE']) + if m: + file.write("#define BARO_I2C_BUS BUS_I2C%s\n" % (m.group(1))) + if 'BARO_SPI_INSTANCE' in map['defines']: + file.write("#define USE_BARO_SPI_BMP280\n") + file.write("#define BMP280_SPI_BUS BUS_%s\n" % (map['defines']['BARO_SPI_INSTANCE'])) + file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS'))) + file.write("#define BARO_SPI_BUS BUS_I2C%i\n" % (m.group(1))) + # TODO file.write("// OSD\n") - osd_spi_bus = map['variables'].get('max7456_spi_bus') - if osd_spi_bus: + if 'USE_MAX7456' in map['empty_defines']: + print ("ANALOG OSD") file.write("#define USE_MAX7456\n") - pin = findPinByFunction('OSD_CS_1', map) + pin = findPinByFunction('MAX7456_SPI_CS', map) file.write("#define MAX7456_CS_PIN %s\n" % (pin)) - file.write("#define MAX7456_SPI_BUS BUS_SPI%s\n" % (osd_spi_bus)) + file.write("#define MAX7456_SPI_BUS BUS_%s\n" % (map['defines']['MAX7456_SPI_INSTANCE'])) file.write("// Blackbox\n") # Flash: - spiflash_bus = map['variables'].get('flash_spi_bus') - if spiflash_bus: - for i in range(1, 9): - cs = findPinByFunction("FLASH_CS_%i" % (i), map) - if cs: - file.write("#define USE_FLASHFS\n") - file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") - file.write("#define USE_FLASH_M25P16\n") - file.write("#define USE_FLASH_W25N01G\n") - file.write("#define M25P16_SPI_BUS BUS_SPI%s\n" % (spiflash_bus)) - file.write("#define M25P16_CS_PIN %s\n" % (cs)) - file.write("#define W25N01G_SPI_BUS BUS_SPI%s\n" % (spiflash_bus)) - file.write("#define W25N01G_CS_PIN %s\n" % (cs)) - break + if 'USE_FLASH' in map['empty_defines']: + print ("FLASH BLACKBOX") + cs = findPinByFunction("FLASH_CS", map) + spiflash_bus = map['defines'].get('FLASH_SPI_INSTANCE') + if cs: + # TODO: add more drivers + file.write("#define USE_FLASHFS\n") + file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") + file.write("#define USE_FLASH_M25P16\n") + file.write("#define USE_FLASH_W25N01G\n") + file.write("#define M25P16_SPI_BUS BUS_%s\n" % (spiflash_bus)) + file.write("#define M25P16_CS_PIN %s\n" % (cs)) + file.write("#define W25N01G_SPI_BUS BUS_%s\n" % (spiflash_bus)) + file.write("#define W25N01G_CS_PIN %s\n" % (cs)) # SD Card: use_sdcard = False @@ -421,14 +487,14 @@ def writeTargetH(folder, map): file.write("#define SDCARD_SDIO_4BIT\n") file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) - file.write("\n// Otehrs\n\n") + file.write("\n// Others\n\n") pwm_outputs = getPwmOutputCount(map) file.write("#define MAX_PWM_OUTPUT_PORTS %i\n" % (pwm_outputs)) file.write("#define USE_SERIAL_4WAY_BLHELI_INTERFACE\n") - file.write("#define USE_DSHOT\n"); - file.write("#define USE_ESC_SENSOR\n"); + file.write("#define USE_DSHOT\n") + file.write("#define USE_ESC_SENSOR\n") port_config = getPortConfig(map) @@ -582,7 +648,7 @@ def writeTarget(outputFolder, map): return def buildMap(inputFile): - map = { 'defines': {}, 'use_defines': [], 'features': ['FEATURE_OSD', 'FEATURE_TELEMETRY', 'FEATURE_CURRENT_METER', 'FEATURE_VBAT', 'FEATURE_TX_PROF_SEL', 'FEATURE_BLACKBOX'], 'pins': {}, 'funcs': {}, 'timer_pin_map': {}} + map = { 'defines': {}, 'empty_defines': [], 'features': ['FEATURE_OSD', 'FEATURE_TELEMETRY', 'FEATURE_CURRENT_METER', 'FEATURE_VBAT', 'FEATURE_TX_PROF_SEL', 'FEATURE_BLACKBOX'], 'pins': {}, 'funcs': {}, 'timer_pin_map': {}} f = open(inputFile, 'r') while True: @@ -601,9 +667,9 @@ def buildMap(inputFile): if m: map['manufacturer_id'] = m.group(1) - m = re.search(r'^#define\s+(USE_\w+)$', l) + m = re.search(r'^#define\s+(\w+)\s*$', l) if m: - map['use_defines'].append(m.group(1)) + map['empty_defines'].append(m.group(1)) m = re.search('^\s*#define\s+DEFAULT_FEATURES\s+\((.+?)\)\s*$', l) if m: From c60578437d4a48ffe6c71ec3a56f43cc557a15af Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 18:44:35 +0100 Subject: [PATCH 016/175] Back to stage one. Target files generated, probably needs reviewing pin mappings --- src/utils/bf2inav.py | 148 +++++++++++++++++++++++++++++++++---------- 1 file changed, 113 insertions(+), 35 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 72737eebefe..a6c9d06a4d9 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -24,6 +24,7 @@ import json import random import string +import yaml version = '0.1' @@ -161,6 +162,7 @@ def findPinsByFunction(function, map): for func in map['funcs']: pattern = "^%s" % (function) if re.search(pattern, func): + print ("%s: %s" % (function, func)) result.append(map['funcs'][func]) return result @@ -503,6 +505,58 @@ def writeTargetH(folder, map): file.close() return + +def mcu2timerKey(mcu): + m = re.search(r'^AT32F435[GM]', mcu) + if m: + return 'AT32F435' + + m = re.search(r'^STM32F405', mcu) + if m: + return 'AT32F435' + + m = re.search(r'^STM32F7[2X]2', mcu) + if m: + return 'AT32F722' + + m = re.search(r'^STM32F745', mcu) + if m: + return 'AT32F745' + + m = re.search(r'^STM32H743', mcu) + if m: + return 'AT32F745' + + print ("Unsupported MCU: %s" % (mcu)) + sys.exit(-1) + + +def getTimerInfo(map, pin): + with open("timer_pins.yaml", "r") as f: + pindb = yaml.safe_load(f) + f.close() + + mcu = map['mcu']['type'] + tk = mcu2timerKey(mcu) + + if not tk in pindb: + print ("PINDB not available for MCU: %s" % (mcu)) + sys.exit(-1) + + timers = pindb[tk].get(pin, None) + + if timers: + result = [] + for ti in timers: + timer = list(ti.keys())[0] + channel = ti[timer] + + result.append([timer, channel]) + + return result + + return None + def writeTargetC(folder, map): file = open(folder + '/target.c', "w+") @@ -538,16 +592,16 @@ def writeTargetC(folder, map): """) - for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: - found = False - for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: - val = var + supportedgyro - if val in map['defines']: - found = True - break + #for supportedgyro in ['BMI160', 'BMI270', 'ICM20689', 'ICM42605', 'MPU6000', 'MPU6500', 'MPU9250']: + # found = False + # for var in ['USE_ACCGYRO_', 'USE_ACC_', 'USE_ACC_SPI', 'USE_GYRO_', 'USE_GYRO_SPI_']: + # val = var + supportedgyro + # if val in map['empty_defines']: + # found = True + # break - if found: - file.write("//BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) + # if found: + # file.write("//BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) file.write("\ntimerHardware_t timerHardware[] = {\n") @@ -555,38 +609,62 @@ def writeTargetC(folder, map): motors = findPinsByFunction("MOTOR", map) if motors: for motor in motors: - timer = map['pins'][motor]['TIM'] - channel = map['pins'][motor]['CH'] - dma = map['dmas'].get(motor, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (timer, channel, motor, dma)) + timerInfo = getTimerInfo(map, motor) + if timerInfo: + first = True + print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + file.write("\n") servos = findPinsByFunction("SERVO", map) if servos: for servo in servos: - timer = map['pins'][servo]['TIM'] - channel = map['pins'][servo]['CH'] - dma = map['dmas'].get(servo, {}).get("DMA", "0") - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (timer, channel, servo, dma)) - - beeper = findPinByFunction("BEEPER_1", map) + timerInfo = getTimerInfo(map, servo) + if timerInfo: + first = True + print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + file.write("\n") + + beeper = findPinByFunction("BEEPER", map) if beeper: - timer = map['pins'].get(beeper, {}).get('TIM') - channel = map['pins'].get(beeper, {}).get('CH') - dma = map['dmas'].get(beeper, {}).get("DMA", "0") - if timer and channel: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (timer, channel, beeper, dma)) - - led = findPinByFunction("LED_STRIP_1", map) + timerInfo = getTimerInfo(map, beeper) + if timerInfo: + first = True + print ("BEEPER: %s" % (timerInfo)) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + file.write("\n") + + led = findPinByFunction("LED_STRIP", map) if led: - timer = map['pins'].get(led, {}).get('TIM') - channel = map['pins'].get(led, {}).get('CH') - dma = map['dmas'].get(led, {}).get("DMA", "0") - if timer and channel: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (timer, channel, led, dma)) - - - file.write(""" -}; + timerInfo = getTimerInfo(map, led) + if timerInfo: + first = True + print (timerInfo) + for (t, ch) in timerInfo: + if first: + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (t, ch, led, 0)) + first = False + else: + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (t, ch, led, 0)) + file.write("\n") + + file.write("""}; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); """) From bfaf1a4fe5a8bc7d431edea590e16e5dd0d39fed Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 18:51:54 +0100 Subject: [PATCH 017/175] Small issues found by converting BETAFPVF405 (pavo pico's fc) --- src/utils/bf2inav.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index a6c9d06a4d9..827f8a74fc6 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -372,11 +372,11 @@ def writeTargetH(folder, map): if 'MAG_I2C_INSTANCE' in map['defines']: file.write("// MAG\n") - bfintance = map['defines']['MAG_I2C_INSTANCE'] + bfinstance = map['defines']['MAG_I2C_INSTANCE'] file.write("#define USE_MAG\n") file.write("#define USE_MAG_ALL\n") # (I2CDEV_1) - m = re.search(r'^\s*#define\s+MAG_I2C_INSTANCE\s+\(?I2CDEV_(\d+)\)?\s*$') + m = re.search(r'^\s*#define\s+MAG_I2C_INSTANCE\s+\(?I2CDEV_(\d+)\)?\s*$', bfinstance) if m: file.write("#define MAG_I2C_BUS BUS_I2C%i" % (m.group(1))) @@ -447,8 +447,7 @@ def writeTargetH(folder, map): if 'BARO_SPI_INSTANCE' in map['defines']: file.write("#define USE_BARO_SPI_BMP280\n") file.write("#define BMP280_SPI_BUS BUS_%s\n" % (map['defines']['BARO_SPI_INSTANCE'])) - file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS'))) - file.write("#define BARO_SPI_BUS BUS_I2C%i\n" % (m.group(1))) + file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS', map))) # TODO file.write("// OSD\n") From 291fe98fe5c6074929363d8961603e9c4d92a428 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 19:11:34 +0100 Subject: [PATCH 018/175] Fix mcu2timerKey --- src/utils/bf2inav.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 827f8a74fc6..c5343de4048 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -512,19 +512,19 @@ def mcu2timerKey(mcu): m = re.search(r'^STM32F405', mcu) if m: - return 'AT32F435' + return 'STM32F405' m = re.search(r'^STM32F7[2X]2', mcu) if m: - return 'AT32F722' + return 'STM32F722' m = re.search(r'^STM32F745', mcu) if m: - return 'AT32F745' + return 'STM32F745' m = re.search(r'^STM32H743', mcu) if m: - return 'AT32F745' + return 'STM32H743' print ("Unsupported MCU: %s" % (mcu)) sys.exit(-1) @@ -614,10 +614,10 @@ def writeTargetC(folder, map): print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 1)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 1)) file.write("\n") servos = findPinsByFunction("SERVO", map) @@ -629,10 +629,10 @@ def writeTargetC(folder, map): print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 1)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 1)) file.write("\n") beeper = findPinByFunction("BEEPER", map) @@ -643,10 +643,10 @@ def writeTargetC(folder, map): print ("BEEPER: %s" % (timerInfo)) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 1)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 1)) file.write("\n") led = findPinByFunction("LED_STRIP", map) From 2231d1cce3bffa7baf3e6085c3b855a41d09b2c5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 20:08:29 +0100 Subject: [PATCH 019/175] fix usb errors --- src/utils/bf2inav.py | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index c5343de4048..569132fcf00 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -274,7 +274,7 @@ def writeTargetH(folder, map): print ("SERIAL") file.write("// UARTs\n") file.write("#define USB_IO\n") - file.write("#define USB_VCP\n") + file.write("#define USE_VCP\n") serial_count = 0 pin = findPinByFunction('USB_DETECT', map) if pin: @@ -465,14 +465,19 @@ def writeTargetH(folder, map): spiflash_bus = map['defines'].get('FLASH_SPI_INSTANCE') if cs: # TODO: add more drivers + suppored_flash_chips = [ + 'M25P16', + 'W25M', + 'W25M02G', + 'W25M512', + 'W25N01G' + ] file.write("#define USE_FLASHFS\n") file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") - file.write("#define USE_FLASH_M25P16\n") - file.write("#define USE_FLASH_W25N01G\n") - file.write("#define M25P16_SPI_BUS BUS_%s\n" % (spiflash_bus)) - file.write("#define M25P16_CS_PIN %s\n" % (cs)) - file.write("#define W25N01G_SPI_BUS BUS_%s\n" % (spiflash_bus)) - file.write("#define W25N01G_CS_PIN %s\n" % (cs)) + for flash in suppored_flash_chips: + file.write("#define USE_FLASH_%s\n" % (flash)) + file.write("#define %s_SPI_BUS BUS_%s\n" % (flash, spiflash_bus)) + file.write("#define %s_CS_PIN %s\n" % (flash, cs)) # SD Card: use_sdcard = False @@ -497,6 +502,12 @@ def writeTargetH(folder, map): file.write("#define USE_DSHOT\n") file.write("#define USE_ESC_SENSOR\n") + if 'DEFAULT_VOLTAGE_METER_SCALE' in map['defines']: + file.write("#define VOLTAGE_METER_SCALE %s\n" % (map['defines']['DEFAULT_VOLTAGE_METER_SCALE'])) + if 'DEFAULT_CURRENT_METER_SCALE' in map['defines']: + file.write("#define CURRENT_METER_SCALE %s\n" % (map['defines']['DEFAULT_CURRENT_METER_SCALE'])) + + port_config = getPortConfig(map) file.write(port_config) @@ -614,10 +625,10 @@ def writeTargetC(folder, map): print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 1)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 1)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) file.write("\n") servos = findPinsByFunction("SERVO", map) @@ -629,10 +640,10 @@ def writeTargetC(folder, map): print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 1)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 1)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) file.write("\n") beeper = findPinByFunction("BEEPER", map) @@ -643,10 +654,10 @@ def writeTargetC(folder, map): print ("BEEPER: %s" % (timerInfo)) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 1)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) first = False else: - file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 1)) + file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) file.write("\n") led = findPinByFunction("LED_STRIP", map) From c940461734c0e37820b72fe9e4c2e59c531a422f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 20:13:48 +0100 Subject: [PATCH 020/175] small cosmetic change --- src/utils/bf2inav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 569132fcf00..2b6d39ffea7 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -353,13 +353,13 @@ def writeTargetH(folder, map): if mosipin: file.write("#define SPI%i_MOSI_PIN %s\n" % (i, mosipin)) - file.write("// I2C\n") use_i2c_defined = False for i in range(1, 9): sclpin = findPinByFunction("I2C%i_SCL" % (i), map) sdapin = findPinByFunction("I2C%i_SDA" % (i), map) if (sclpin or sdapin): if (not use_i2c_defined): + file.write("// I2C\n") print ("I2C") use_i2c_defined = True file.write("#define USE_I2C\n") From 610f22af2c87dacbbdf3d39bb211bbe5bf19ef0a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 20:20:03 +0100 Subject: [PATCH 021/175] Add some breif info --- src/utils/bf2inav.py | 53 +++++++++++++++++++++----------------------- 1 file changed, 25 insertions(+), 28 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 2b6d39ffea7..37b4901005a 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -4,16 +4,13 @@ # This script can be used to Generate a basic working target from a Betaflight Configuration. # The idea is that this target can be used as a starting point for full INAV target. # -# The generated target will not include any servo assignments or fixed wing features. +# The generated target will often be enough to get INAV working, but may need some manual editing. # -# TODO: ADC DMA info -# BF build API: -# target lists -# https://build.betaflight.com/api/targets -# target release info: -# https://build.betaflight.com/api/targets/{TARGET} -# load target: -# Unified targets are deprecated, replaced by https://github.com/betaflight/config +# Betaflight Configuration files are available at https://github.com/betaflight/config +# +# Common things to look for: target.c timer definitions. You may need to change timers around +# to get all features working. The script will add commented out lines for all timer possibilites +# for a given pin. import sys @@ -162,7 +159,7 @@ def findPinsByFunction(function, map): for func in map['funcs']: pattern = "^%s" % (function) if re.search(pattern, func): - print ("%s: %s" % (function, func)) + #print ("%s: %s" % (function, func)) result.append(map['funcs'][func]) return result @@ -246,32 +243,32 @@ def writeTargetH(folder, map): # beeper file.write("// Beeper\n") pin = findPinByFunction('BEEPER', map) - print ("BEEPER") + #print ("BEEPER") if pin: - print ("BEEPER: %s" % (pin)) + #print ("BEEPER: %s" % (pin)) file.write("#define USE_BEEPER\n") file.write("#define BEEPER %s\n" % (pin)) if 'BEEPER_INVERTED' in map['empty_defines']: file.write("#define BEEPER_INVERTED\n") - print ("INVERTED") + #print ("INVERTED") # Leds file.write("// Leds\n") pin = findPinByFunction('LED_STRIP', map) - print ("LED") + #print ("LED") if pin: - print ("LED: %s" % (pin)) + #print ("LED: %s" % (pin)) file.write('#define USE_LED_STRIP\n') file.write("#define WS2811_PIN %s\n" % (pin)) for i in range(0, 9): pin = findPinByFunction("LED%i" % (i), map) if pin: - print ("LED%i: %s" % (i, pin)) + #print ("LED%i: %s" % (i, pin)) file.write("#define LED%i %s\n" % (i, pin)) # Serial ports and usb - print ("SERIAL") + #print ("SERIAL") file.write("// UARTs\n") file.write("#define USB_IO\n") file.write("#define USE_VCP\n") @@ -287,7 +284,7 @@ def writeTargetH(folder, map): txpin = findPinByFunction("UART%i_TX" % (i), map) rxpin = findPinByFunction("UART%i_RX" % (i), map) if txpin or rxpin: - print ("UART%s" % (i)) + #print ("UART%s" % (i)) file.write("#define USE_UART%i\n" % (i)) serial_count+=1 else: @@ -306,7 +303,7 @@ def writeTargetH(folder, map): rxpin = findPinByFunction("SOFTSERIAL%i_RX" % (i), map) idx = i if txpin != None or rxpin != None: - print ("SOFTUART%s" % (i)) + #print ("SOFTUART%s" % (i)) file.write("#define USE_SOFTSERIAL%i\n" % (idx)) serial_count+=1 else: @@ -360,7 +357,7 @@ def writeTargetH(folder, map): if (sclpin or sdapin): if (not use_i2c_defined): file.write("// I2C\n") - print ("I2C") + #print ("I2C") use_i2c_defined = True file.write("#define USE_I2C\n") file.write("#define USE_I2C_DEVICE_%i\n" % (i)) @@ -427,7 +424,7 @@ def writeTargetH(folder, map): break if found: - print (supportedgyro) + #print (supportedgyro) file.write("#define USE_IMU_%s\n" % (supportedgyro)) file.write("#define %s_CS_PIN %s\n" % (supportedgyro, findPinByFunction('GYRO_1_CS', map))) file.write("#define %s_SPI_BUS BUS_%s\n" % (supportedgyro, map['defines']['GYRO_1_SPI_INSTANCE'])) @@ -436,7 +433,7 @@ def writeTargetH(folder, map): # TODO: SPI BARO if 'USE_BARO' in map['empty_defines']: - print ("BARO") + #print ("BARO") file.write("// BARO\n") file.write("#define USE_BARO\n") file.write("#define USE_BARO_ALL\n") @@ -452,7 +449,7 @@ def writeTargetH(folder, map): # TODO file.write("// OSD\n") if 'USE_MAX7456' in map['empty_defines']: - print ("ANALOG OSD") + #print ("ANALOG OSD") file.write("#define USE_MAX7456\n") pin = findPinByFunction('MAX7456_SPI_CS', map) file.write("#define MAX7456_CS_PIN %s\n" % (pin)) @@ -460,7 +457,7 @@ def writeTargetH(folder, map): file.write("// Blackbox\n") # Flash: if 'USE_FLASH' in map['empty_defines']: - print ("FLASH BLACKBOX") + #print ("FLASH BLACKBOX") cs = findPinByFunction("FLASH_CS", map) spiflash_bus = map['defines'].get('FLASH_SPI_INSTANCE') if cs: @@ -622,7 +619,7 @@ def writeTargetC(folder, map): timerInfo = getTimerInfo(map, motor) if timerInfo: first = True - print (timerInfo) + #print (timerInfo) for (t, ch) in timerInfo: if first: file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) @@ -637,7 +634,7 @@ def writeTargetC(folder, map): timerInfo = getTimerInfo(map, servo) if timerInfo: first = True - print (timerInfo) + #print (timerInfo) for (t, ch) in timerInfo: if first: file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) @@ -651,7 +648,7 @@ def writeTargetC(folder, map): timerInfo = getTimerInfo(map, beeper) if timerInfo: first = True - print ("BEEPER: %s" % (timerInfo)) + #print ("BEEPER: %s" % (timerInfo)) for (t, ch) in timerInfo: if first: file.write(" DEF_TIM(%s, %s, %s, TIM_USE_BEEPER, 0, %s),\n" % (t, ch, beeper, 0)) @@ -665,7 +662,7 @@ def writeTargetC(folder, map): timerInfo = getTimerInfo(map, led) if timerInfo: first = True - print (timerInfo) + #print (timerInfo) for (t, ch) in timerInfo: if first: file.write(" DEF_TIM(%s, %s, %s, TIM_USE_LED, 0, %s),\n" % (t, ch, led, 0)) From cb9fa0126dfd36774084bdcb4d9d2ca655b6ffaf Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 20:36:08 +0100 Subject: [PATCH 022/175] Remove target added by accident --- src/main/target/BETAFPVF411/CMakeLists.txt | 1 - src/main/target/BETAFPVF411/config.c | 33 ------ src/main/target/BETAFPVF411/target.c | 43 -------- src/main/target/BETAFPVF411/target.h | 120 --------------------- 4 files changed, 197 deletions(-) delete mode 100644 src/main/target/BETAFPVF411/CMakeLists.txt delete mode 100644 src/main/target/BETAFPVF411/config.c delete mode 100644 src/main/target/BETAFPVF411/target.c delete mode 100644 src/main/target/BETAFPVF411/target.h diff --git a/src/main/target/BETAFPVF411/CMakeLists.txt b/src/main/target/BETAFPVF411/CMakeLists.txt deleted file mode 100644 index bc5f1decb7f..00000000000 --- a/src/main/target/BETAFPVF411/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f411xe(BETAFPVF411 SKIP_RELEASES) diff --git a/src/main/target/BETAFPVF411/config.c b/src/main/target/BETAFPVF411/config.c deleted file mode 100644 index cd5aa30d718..00000000000 --- a/src/main/target/BETAFPVF411/config.c +++ /dev/null @@ -1,33 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - * - * This target has been autgenerated by bf2inav.py - */ - -#include - -#include "platform.h" - -#include "fc/fc_msp_box.h" -#include "fc/config.h" - -#include "io/piniobox.h" - -void targetConfiguration(void) -{ - -} - diff --git a/src/main/target/BETAFPVF411/target.c b/src/main/target/BETAFPVF411/target.c deleted file mode 100644 index 34dc53522f2..00000000000 --- a/src/main/target/BETAFPVF411/target.c +++ /dev/null @@ -1,43 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - * - * This target has been autgenerated by bf2inav.py - */ - -#include - -#include "platform.h" - -#include "drivers/bus.h" -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" -#include "drivers/pinio.h" -//#include "drivers/sensor.h" - -//BUSDEV_REGISTER_SPI_TAG(busdev_bmi270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); -//BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); - -timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), - -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h deleted file mode 100644 index f2f5f7bc5db..00000000000 --- a/src/main/target/BETAFPVF411/target.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - * - * This target has been autgenerated by bf2inav.py - */ - -#pragma once - -//#define USE_TARGET_CONFIG - -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) - - - -#define TARGET_BOARD_IDENTIFIER "LGPC" -#define USBD_PRODUCT_STRING "BETAFPVF411" -// Beeper -#define USE_BEEPER -#define BEEPER PB2 -#define BEEPER_INVERTED -// Leds -//#define USE_LED_STRIP -//#define WS2811_PIN PA8 -#define LED0 PC13 -#define LED1 PC14 -// UARTs -#define USB_IO -#define USE_VCP -#define USE_USB_DETECT -#define USB_DETECT_PIN PC15 -#define USE_UART1 -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 -#define USE_UART2 -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_TX_PIN PA8 -#define SOFTSERIAL_1_RX_PIN PA8 -//#define SOFTSERIAL_1_TX_PIN PB3 -//#define SOFTSERIAL_1_RX_PIN PB3 -#define USE_SOFTSERIAL2 -#define SOFTSERIAL_2_TX_PIN PB10 -#define SOFTSERIAL_2_RX_PIN PB10 -//#define USE_SOFTSERIAL3 -//#define SOFTSERIAL_3_TX_PIN PA8 -//#define SOFTSERIAL_3_RX_PIN PA8 -#define SERIAL_PORT_COUNT 5 -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_CRSF -#define SERIALRX_UART SERIAL_PORT_USART1 -// SPI -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 -// I2C -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 -// ADC -#define ADC_CHANNEL_1_PIN PB0 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define ADC_CHANNEL_2_PIN PB1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define USE_ADC -#define ADC_INSTANCE ADC1 -// Gyro & ACC -#define USE_IMU_BMI270 -#define BMI270_CS_PIN PA4 -#define BMI270_SPI_BUS BUS_SPI1 -#define IMU_BMI270_ALIGN CW180_DEG -#define USE_IMU_MPU6000 -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_BUS BUS_SPI1 -#define IMU_MPU6000_ALIGN CW180_DEG -// OSD -#define USE_MAX7456 -#define MAX7456_CS_PIN PB12 -#define MAX7456_SPI_BUS BUS_SPI2 -// Blackbox -#define USE_FLASHFS -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define USE_FLASH_M25P16 -#define USE_FLASH_W25N01G -#define M25P16_SPI_BUS BUS_SPI2 -#define M25P16_CS_PIN PA0 -#define W25N01G_SPI_BUS BUS_SPI2 -#define W25N01G_CS_PIN PA0 - -// Otehrs - -#define MAX_PWM_OUTPUT_PORTS 4 -#define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define USE_DSHOT -#define USE_ESC_SENSOR - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) From 224441b506033531a7b79bb8237022af3f7b6019 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 22:30:06 +0100 Subject: [PATCH 023/175] add stm32f722 7x5 --- src/utils/timer_pins.yaml | 295 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 284 insertions(+), 11 deletions(-) diff --git a/src/utils/timer_pins.yaml b/src/utils/timer_pins.yaml index 87d578021d3..f1b2eb699b3 100644 --- a/src/utils/timer_pins.yaml +++ b/src/utils/timer_pins.yaml @@ -474,14 +474,287 @@ STM32H743: PK1: - TIM1: CH1 - TIM8: CH3N - - - - - - - - - - - \ No newline at end of file +STM32F722: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM9: CH1 + PA3: + - TIM2: CH4 + - TIM5: CH4 + - TIM9: CH2 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + - TIM13: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + - TIM14: CH1 + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM8: CH2N + - TIM12: CH1 + PB15: + - TIM1: CH3N + - TIM8: CH3N + - TIM12: CH2 + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM10: CH1 + PF7: + - TIM10: CH1 + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 +STM32F745: + PA0: + - TIM2: CH1 + - TIM5: CH1 + PA1: + - TIM2: CH2 + - TIM5: CH2 + PA2: + - TIM2: CH3 + - TIM5: CH3 + - TIM9: CH1 + PA3: + - TIM2: CH4 + - TIM5: CH4 + - TIM9: CH2 + PA5: + - TIM2: CH1 + - TIM8: CH1N + PA6: + - TIM3: CH1 + PA7: + - TIM1: CH1N + - TIM3: CH2 + - TIM8: CH1N + PA8: + - TIM1: CH1 + PA9: + - TIM1: CH2 + PA10: + - TIM1: CH3 + PA11: + - TIM1: CH4 + PA15: + - TIM2: CH1 + PB0: + - TIM1: CH2N + - TIM3: CH3 + - TIM8: CH2N + PB1: + - TIM1: CH3N + - TIM3: CH4 + - TIM8: CH3N + PB3: + - TIM2: CH2 + PB4: + - TIM3: CH1 + PB5: + - TIM3: CH2 + PB6: + - TIM4: CH1 + PB7: + - TIM4: CH2 + PB8: + - TIM4: CH3 + - TIM10: CH1 + PB9: + - TIM4: CH4 + - TIM11: CH1 + PB10: + - TIM2: CH3 + PB11: + - TIM2: CH4 + PB13: + - TIM1: CH1N + PB14: + - TIM1: CH2N + - TIM8: CH2N + PB15: + - TIM1: CH3N + - TIM8: CH3N + PC6: + - TIM3: CH1 + - TIM8: CH1 + PC7: + - TIM3: CH2 + - TIM8: CH2 + PC8: + - TIM3: CH3 + - TIM8: CH3 + PC9: + - TIM3: CH4 + - TIM8: CH4 + PD12: + - TIM4: CH1 + PD13: + - TIM4: CH2 + PD14: + - TIM4: CH3 + PD15: + - TIM4: CH4 + PE5: + - TIM9: CH1 + PE6: + - TIM9: CH2 + PE8: + - TIM1: CH1N + PE9: + - TIM1: CH1 + PE10: + - TIM1: CH2N + PE11: + - TIM1: CH2 + PE12: + - TIM1: CH3N + PE13: + - TIM1: CH3 + PE14: + - TIM1: CH4 + PF6: + - TIM10: CH1 + PF7: + - TIM11: CH1 + PF8: + - TIM13: CH1 + PF9: + - TIM14: CH1 + PH6: + - TIM12: CH1 + PH9: + - TIM12: CH2 + PH10: + - TIM5: CH1 + PH11: + - TIM5: CH2 + PH12: + - TIM5: CH3 + PH13: + - TIM8: CH1N + PH14: + - TIM8: CH2N + PH15: + - TIM8: CH3N + PI0: + - TIM5: CH4 + PI2: + - TIM8: CH4 + PI5: + - TIM8: CH1 + PI6: + - TIM8: CH2 + PI7: + - TIM8: CH3 \ No newline at end of file From cfc45ad3b3d4232be86519a3831b0bfc229737f4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 10 Dec 2023 22:47:52 +0100 Subject: [PATCH 024/175] small changes on mcu match --- src/utils/bf2inav.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 37b4901005a..0677c0888e7 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -522,15 +522,15 @@ def mcu2timerKey(mcu): if m: return 'STM32F405' - m = re.search(r'^STM32F7[2X]2', mcu) + m = re.search(r'^STM32F7[2Xx]2', mcu) if m: return 'STM32F722' - m = re.search(r'^STM32F745', mcu) + m = re.search(r'^STM32F7[Xx46]5', mcu) if m: return 'STM32F745' - m = re.search(r'^STM32H743', mcu) + m = re.search(r'^STM32H7[45]3', mcu) if m: return 'STM32H743' From f2cbbd5f60659d051df9b53f51b6bbf6da3ae9b4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 11 Dec 2023 15:25:32 +0100 Subject: [PATCH 025/175] Add documentation for bf2inav.py script --- .../Converting Betaflight Targets.md | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 docs/development/Converting Betaflight Targets.md diff --git a/docs/development/Converting Betaflight Targets.md b/docs/development/Converting Betaflight Targets.md new file mode 100644 index 00000000000..6e12c8c8729 --- /dev/null +++ b/docs/development/Converting Betaflight Targets.md @@ -0,0 +1,74 @@ +If your flight controller does not have an official INAV target, it is possible to use src/utils/bf2inav.py script to generate a good starting point for an unofficial INAV target. + +This script can read the config.h from [Betaflight's target configuration repository](https://github.com/betaflight/config) and it currently supports STM32F405, STM32F722, STM32F745, STM32H743 and AT32F435 targets. +Locate your Flight Controller target config.h in BetaFlight's repo, eg.: [config.h](https://github.com/betaflight/config/blob/master/configs/BETAFPVF405/config.h), for BETAFPVF435 target. + +It is also advisable to record the output of the timer command from BetaFlight, as it will provide useful information on `timer` usage that can be used to adjust the generated target later. + +``` +# timer +timer B08 AF3 +# pin B08: TIM10 CH1 (AF3) +timer C08 AF3 +# pin C08: TIM8 CH3 (AF3) +timer B00 AF2 +# pin B00: TIM3 CH3 (AF2) +timer B01 AF2 +# pin B01: TIM3 CH4 (AF2) +timer A03 AF1 +# pin A03: TIM2 CH4 (AF1) +timer A02 AF1 +# pin A02: TIM2 CH3 (AF1) +timer B06 AF2 +# pin B06: TIM4 CH1 (AF2) +timer A08 AF1 +# pin A08: TIM1 CH1 (AF1) +timer A09 AF1 +# pin A09: TIM1 CH2 (AF1) +timer A10 AF1 +# pin A10: TIM1 CH3 (AF1) +``` +In the above example, `pin B08: TIM10 CH1 (AF3)` means that pind to CH1. This information can be used to fix the generated timer assigned to match BetaFlight's allocation by editing the `target.c` file generated by the `bf2inav.py` script. + + +Using the BETAFPVF405 target mentioned above, to create the target now we need to: + +1. Download INAV source code and be able to build +2. Download the config.h from BetaFlight repository +3. Create a target folder that will be used as the output folder for the `bf2inav.py` script, eg: `inav/src/main/targets/BETAFPVF405` +4. Navigate to the script folder in `inav/src/utils/` +5. `python3 ./bf2inav.py -i config.h -o ../main/target/BETAFPVF405/` +6. Edit generated `target.c` and chose the correct timer definitions to match Betaflight's timer definitions. +``` +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM9, CH1, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), + + //DEF_TIM(TIM3, CH1, PB4, TIM_USE_BEEPER, 0, 0), + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), + +}; +``` +In this particular example, PA3, PA2 were changed to match Betaflight's mapping, and the timer PB4 was disabled, due to a timer conflict. Normal channles are prefered over N channels (CH1, over CH1N) or C channels in AT32 architectures. +7. Now update yout build scripts by running `cmake` and build the target you just created. The target name can be checked in the generated `CMakeLists.txt`, but should match the Betaflight target name. + +For information on how to build INAV, check the documents in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder. \ No newline at end of file From 3443f268a25c932e0370c156f3699d768fadf32a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 11 Dec 2023 15:38:18 +0100 Subject: [PATCH 026/175] Fix typo --- docs/development/Converting Betaflight Targets.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/development/Converting Betaflight Targets.md b/docs/development/Converting Betaflight Targets.md index 6e12c8c8729..47f460f634d 100644 --- a/docs/development/Converting Betaflight Targets.md +++ b/docs/development/Converting Betaflight Targets.md @@ -68,7 +68,7 @@ timerHardware_t timerHardware[] = { }; ``` -In this particular example, PA3, PA2 were changed to match Betaflight's mapping, and the timer PB4 was disabled, due to a timer conflict. Normal channles are prefered over N channels (CH1, over CH1N) or C channels in AT32 architectures. +In this particular example, PA3, PA2 were changed to match Betaflight's mapping, and the timer PB4 was disabled, due to a timer conflict. Normal channels are prefered over N channels (CH1, over CH1N) or C channels in AT32 architectures. 7. Now update yout build scripts by running `cmake` and build the target you just created. The target name can be checked in the generated `CMakeLists.txt`, but should match the Betaflight target name. For information on how to build INAV, check the documents in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder. \ No newline at end of file From 7c532cd028d9ccce5f4d3a452fef1878f213a734 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 12 Dec 2023 12:26:18 +0100 Subject: [PATCH 027/175] Add PINIO --- src/utils/bf2inav.py | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 0677c0888e7..9919ecdfce1 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -431,7 +431,6 @@ def writeTargetH(folder, map): file.write("#define IMU_%s_ALIGN %s\n" % (supportedgyro, getGyroAlign(map))) - # TODO: SPI BARO if 'USE_BARO' in map['empty_defines']: #print ("BARO") file.write("// BARO\n") @@ -446,7 +445,6 @@ def writeTargetH(folder, map): file.write("#define BMP280_SPI_BUS BUS_%s\n" % (map['defines']['BARO_SPI_INSTANCE'])) file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS', map))) - # TODO file.write("// OSD\n") if 'USE_MAX7456' in map['empty_defines']: #print ("ANALOG OSD") @@ -455,6 +453,7 @@ def writeTargetH(folder, map): file.write("#define MAX7456_CS_PIN %s\n" % (pin)) file.write("#define MAX7456_SPI_BUS BUS_%s\n" % (map['defines']['MAX7456_SPI_INSTANCE'])) file.write("// Blackbox\n") + # Flash: if 'USE_FLASH' in map['empty_defines']: #print ("FLASH BLACKBOX") @@ -489,8 +488,20 @@ def writeTargetH(folder, map): use_sdcard = True file.write("#define SDCARD_SDIO_4BIT\n") file.write("#define SDCARD_SDIO_DEVICE SDIODEV_%i\n" % (i)) - - file.write("\n// Others\n\n") + + # PINIO + use_pinio = False + for i in range(1, 9): + pinio = findPinByFunction("PINIO%i" % (i), map) + if pinio != None: + if not use_pinio: + use_pinio = True + file.write("\n// PINIO\n\n") + file.write("#define USE_PINIO\n") + file.write("#define USE_PINIOBOX\n") + file.write("#define PINIO%i_PIN %s\n" % (i, pinio)) + + file.write("\n\n// Others\n\n") pwm_outputs = getPwmOutputCount(map) file.write("#define MAX_PWM_OUTPUT_PORTS %i\n" % (pwm_outputs)) From 4b9254405dc784f25a452e13667318b487f2f0c5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 29 Dec 2023 10:58:35 +0100 Subject: [PATCH 028/175] Fix missing GYRO_1_ALIGN for AIKONF7 --- src/utils/bf2inav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 9919ecdfce1..5c6ce07d094 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -178,7 +178,7 @@ def getPwmOutputCount(map): return len(motors) + len(servos) def getGyroAlign(map): - bfalign = map['defines']['GYRO_1_ALIGN'] + bfalign = map['defines'].get('GYRO_1_ALIGN', 'CW0_DEG') return bfalign #m = re.search("^CW(\d+)(FLIP)?$", bfalign) #if m: From d2a55d4b4d695eada59fc8ca15ab6222b8b0a62e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 29 Dec 2023 11:03:30 +0100 Subject: [PATCH 029/175] Always cound usb vcp as serial. No current fc lacks it. --- src/utils/bf2inav.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 5c6ce07d094..6416ec28e24 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -272,13 +272,12 @@ def writeTargetH(folder, map): file.write("// UARTs\n") file.write("#define USB_IO\n") file.write("#define USE_VCP\n") - serial_count = 0 + serial_count = 1 pin = findPinByFunction('USB_DETECT', map) if pin: file.write("#define USE_USB_DETECT\n") file.write("#define USB_DETECT_PIN %s\n" % (pin)) #file.write("#define VBUS_SENSING_ENABLED\n"); - serial_count += 1 for i in range(1, 9): txpin = findPinByFunction("UART%i_TX" % (i), map) From 4f2345afc1e230f069b4a9ec75c0f52508653963 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 30 Dec 2023 00:43:43 +0100 Subject: [PATCH 030/175] Fix SPI baro case --- src/utils/bf2inav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 6416ec28e24..303fbddca38 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -434,8 +434,8 @@ def writeTargetH(folder, map): #print ("BARO") file.write("// BARO\n") file.write("#define USE_BARO\n") - file.write("#define USE_BARO_ALL\n") if 'BARO_I2C_INSTANCE' in map['defines']: + file.write("#define USE_BARO_ALL\n") m = re.search('I2CDEV_(\d+)', map['defines']['BARO_I2C_INSTANCE']) if m: file.write("#define BARO_I2C_BUS BUS_I2C%s\n" % (m.group(1))) From 4e1442384a6f82ba553e9db0ffdc828082d0c9e0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 30 Dec 2023 00:49:06 +0100 Subject: [PATCH 031/175] Add missing USE_BARO_BMP280 to SPI case --- src/utils/bf2inav.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 303fbddca38..40dcddf2c9e 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -440,6 +440,7 @@ def writeTargetH(folder, map): if m: file.write("#define BARO_I2C_BUS BUS_I2C%s\n" % (m.group(1))) if 'BARO_SPI_INSTANCE' in map['defines']: + file.write("#define USE_BARO_BMP280\n") file.write("#define USE_BARO_SPI_BMP280\n") file.write("#define BMP280_SPI_BUS BUS_%s\n" % (map['defines']['BARO_SPI_INSTANCE'])) file.write("#define BMP280_CS_PIN %s\n" % (findPinByFunction('BARO_CS', map))) From c14f0304411541f7dddf87a2519d2d71a5853db2 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Mon, 1 Jan 2024 12:29:31 -0600 Subject: [PATCH 032/175] bf2inav.py: clearer error messages, number PWM outputs --- src/utils/bf2inav.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index 40dcddf2c9e..3d8b31a2e55 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -621,7 +621,7 @@ def writeTargetC(folder, map): # if found: # file.write("//BUSDEV_REGISTER_SPI_TAG(busdev_%s, DEVHW_%s, %s_SPI_BUS, %s_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_%s_ALIGN);\n" % (supportedgyro.lower(), supportedgyro, supportedgyro, supportedgyro, supportedgyro)) - + snum=1 file.write("\ntimerHardware_t timerHardware[] = {\n") motors = findPinsByFunction("MOTOR", map) @@ -633,8 +633,9 @@ def writeTargetC(folder, map): #print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s), // S%i\n" % (t, ch, motor, 0, snum)) first = False + snum += 1 else: file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, motor, 0)) file.write("\n") @@ -648,8 +649,9 @@ def writeTargetC(folder, map): #print (timerInfo) for (t, ch) in timerInfo: if first: - file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) + file.write(" DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s), // S%i\n" % (t, ch, servo, 0, snum)) first = False + snum += 1 else: file.write(" //DEF_TIM(%s, %s, %s, TIM_USE_OUTPUT_AUTO, 0, %s),\n" % (t, ch, servo, 0)) file.write("\n") @@ -958,11 +960,14 @@ def main(argv): print ("%s: %s" % (sys.argv[0], version)) sys.exit(0) - if (os.path.exists(inputfile) and os.path.isdir(outputdir)): - targetDefinition = buildMap(inputfile) + if (not os.path.isfile(inputfile) ): + print("no such file %s" % inputfile) + sys.exit(2) + if (not os.path.isdir(outputdir) ): + print("no such directory %s" % outputdir) + sys.exit(2) else: - printHelp() - sys.exit(2) + targetDefinition = buildMap(inputfile) map = buildMap(inputfile) From 6f58bdbf7f70b88c477394e51e2be712ba8985f1 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Fri, 29 Mar 2024 13:27:30 +0000 Subject: [PATCH 033/175] Change SmartPort sensor IDs for GPS and Modes The current implementation of INAV SmartPort telemetry uses temperature sensor IDs for Modes and GNSS data. This causes issues on transmitter firmware that more strictly adhere to the telemetry ID's intended use. The Tmp sensors can't be converted to a different type, so they can't be read correctly. This PR changes the IDs to some that are not associated with specific functions or types. There is also an option to use the legacy IDs to allow for a deprecation period. This legacy option should be removed in INAV 10.0. --- docs/Settings.md | 10 ++ docs/Telemetry.md | 8 +- src/main/fc/settings.yaml | 4 + src/main/telemetry/smartport.c | 180 +++++++++++++++++---------------- src/main/telemetry/telemetry.c | 1 + src/main/telemetry/telemetry.h | 1 + 6 files changed, 117 insertions(+), 87 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 61195109625..f34552ec430 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1162,6 +1162,16 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da --- +### frsky_use_legacy_gps_mode_sensor_ids + +S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF' + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### fw_autotune_max_rate_deflection The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 996630e66fa..46118e033d9 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -102,17 +102,21 @@ The following sensors are transmitted * **VSpd** : vertical speed, unit is cm/s. * **Hdg** : heading, North is 0°, South is 180°. * **AccX,Y,Z** : accelerometer values (not sent if `frsky_pitch_roll = ON`). -* **Tmp1** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) : +* **470** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) : * **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode * **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode * **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold * **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode * **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed -* **Tmp2** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites). + + _NOTE_ This sensor used to be **Tmp1**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp1** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID. +* **480** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites). * **A** : 1 = GPS fix, 2 = GPS home fix, 4 = home reset (numbers are additive) * **B** : GPS accuracy based on HDOP (0 = lowest to 9 = highest accuracy) * **C** : number of satellites locked (digit C & D are the number of locked satellites) * **D** : number of satellites locked (if 14 satellites are locked, C = 1 & D = 4) + + _NOTE_ This sensor used to be **Tmp2**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp2** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID. * **GAlt** : GPS altitude, sea level is zero. * **ASpd** : true air speed, from pitot sensor. This is _Knots * 10_ * **A4** : average cell value. Warning : unlike FLVSS and MLVSS sensors, you do not get actual lowest value of a cell, but an average : (total lipo voltage) / (number of cells) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e3e69c7487f..6a82489718a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2986,6 +2986,10 @@ groups: description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" default_value: OFF type: bool + - name: frsky_use_legacy_gps_mode_sensor_ids + description: "S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'" + default_value: OFF + type: bool - name: report_cell_voltage description: "S.Port and IBUS telemetry: Send the average cell voltage if set to ON" default_value: OFF diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 9d804be52d8..94e318e8a7a 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -63,66 +63,68 @@ // these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h enum { - FSSP_DATAID_SPEED = 0x0830 , - FSSP_DATAID_VFAS = 0x0210 , - FSSP_DATAID_CURRENT = 0x0200 , - FSSP_DATAID_RPM = 0x050F , - FSSP_DATAID_ALTITUDE = 0x0100 , - FSSP_DATAID_FUEL = 0x0600 , - FSSP_DATAID_ADC1 = 0xF102 , - FSSP_DATAID_ADC2 = 0xF103 , - FSSP_DATAID_LATLONG = 0x0800 , - FSSP_DATAID_CAP_USED = 0x0600 , - FSSP_DATAID_VARIO = 0x0110 , - FSSP_DATAID_CELLS = 0x0300 , - FSSP_DATAID_CELLS_LAST = 0x030F , - FSSP_DATAID_HEADING = 0x0840 , - FSSP_DATAID_FPV = 0x0450 , - FSSP_DATAID_PITCH = 0x0430 , - FSSP_DATAID_ROLL = 0x0440 , - FSSP_DATAID_ACCX = 0x0700 , - FSSP_DATAID_ACCY = 0x0710 , - FSSP_DATAID_ACCZ = 0x0720 , - FSSP_DATAID_T1 = 0x0400 , - FSSP_DATAID_T2 = 0x0410 , - FSSP_DATAID_HOME_DIST = 0x0420 , - FSSP_DATAID_GPS_ALT = 0x0820 , - FSSP_DATAID_ASPD = 0x0A00 , - FSSP_DATAID_A3 = 0x0900 , - FSSP_DATAID_A4 = 0x0910 , - FSSP_DATAID_AZIMUTH = 0x0460 + FSSP_DATAID_SPEED = 0x0830, + FSSP_DATAID_VFAS = 0x0210, + FSSP_DATAID_CURRENT = 0x0200, + FSSP_DATAID_RPM = 0x050F, + FSSP_DATAID_ALTITUDE = 0x0100, + FSSP_DATAID_FUEL = 0x0600, + FSSP_DATAID_ADC1 = 0xF102, + FSSP_DATAID_ADC2 = 0xF103, + FSSP_DATAID_LATLONG = 0x0800, + FSSP_DATAID_CAP_USED = 0x0600, + FSSP_DATAID_VARIO = 0x0110, + FSSP_DATAID_CELLS = 0x0300, + FSSP_DATAID_CELLS_LAST = 0x030F, + FSSP_DATAID_HEADING = 0x0840, + FSSP_DATAID_FPV = 0x0450, + FSSP_DATAID_PITCH = 0x0430, + FSSP_DATAID_ROLL = 0x0440, + FSSP_DATAID_ACCX = 0x0700, + FSSP_DATAID_ACCY = 0x0710, + FSSP_DATAID_ACCZ = 0x0720, + FSSP_DATAID_LEGACY_MODES = 0x0400, // Deprecated. Should be removed in INAV 10.0 + FSSP_DATAID_LEGACY_GNSS = 0x0410, // Deprecated. Should be removed in INAV 10.0 + FSSP_DATAID_HOME_DIST = 0x0420, + FSSP_DATAID_GPS_ALT = 0x0820, + FSSP_DATAID_ASPD = 0x0A00, + FSSP_DATAID_A3 = 0x0900, + FSSP_DATAID_A4 = 0x0910, + FSSP_DATAID_AZIMUTH = 0x0460, + FSSP_DATAID_MODES = 0x0470, + FSSP_DATAID_GNSS = 0x0480, }; const uint16_t frSkyDataIdTable[] = { - FSSP_DATAID_SPEED , - FSSP_DATAID_VFAS , - FSSP_DATAID_CURRENT , - //FSSP_DATAID_RPM , - FSSP_DATAID_ALTITUDE , - FSSP_DATAID_FUEL , - //FSSP_DATAID_ADC1 , - //FSSP_DATAID_ADC2 , - FSSP_DATAID_LATLONG , - FSSP_DATAID_LATLONG , // twice - //FSSP_DATAID_CAP_USED , - FSSP_DATAID_VARIO , - //FSSP_DATAID_CELLS , + FSSP_DATAID_SPEED, + FSSP_DATAID_VFAS, + FSSP_DATAID_CURRENT, + //FSSP_DATAID_RPM, + FSSP_DATAID_ALTITUDE, + FSSP_DATAID_FUEL, + //FSSP_DATAID_ADC1, + //FSSP_DATAID_ADC2, + FSSP_DATAID_LATLONG, + FSSP_DATAID_LATLONG, // twice + //FSSP_DATAID_CAP_USED, + FSSP_DATAID_VARIO, + //FSSP_DATAID_CELLS, //FSSP_DATAID_CELLS_LAST, - FSSP_DATAID_HEADING , - FSSP_DATAID_FPV , - FSSP_DATAID_PITCH , - FSSP_DATAID_ROLL , - FSSP_DATAID_ACCX , - FSSP_DATAID_ACCY , - FSSP_DATAID_ACCZ , - FSSP_DATAID_T1 , - FSSP_DATAID_T2 , - FSSP_DATAID_HOME_DIST , - FSSP_DATAID_GPS_ALT , - FSSP_DATAID_ASPD , - // FSSP_DATAID_A3 , - FSSP_DATAID_A4 , - FSSP_DATAID_AZIMUTH , + FSSP_DATAID_HEADING, + FSSP_DATAID_FPV, + FSSP_DATAID_PITCH, + FSSP_DATAID_ROLL, + FSSP_DATAID_ACCX, + FSSP_DATAID_ACCY, + FSSP_DATAID_ACCZ, + FSSP_DATAID_MODES, + FSSP_DATAID_GNSS, + FSSP_DATAID_HOME_DIST, + FSSP_DATAID_GPS_ALT, + FSSP_DATAID_ASPD, + // FSSP_DATAID_A3, + FSSP_DATAID_A4, + FSSP_DATAID_AZIMUTH, 0 }; @@ -467,27 +469,27 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear smartPortIdCnt++; switch (id) { - case FSSP_DATAID_VFAS : + case FSSP_DATAID_VFAS: if (isBatteryVoltageConfigured()) { uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage(); smartPortSendPackage(id, vfasVoltage); *clearToSend = false; } break; - case FSSP_DATAID_CURRENT : + case FSSP_DATAID_CURRENT: if (isAmperageConfigured()) { smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit *clearToSend = false; } break; - //case FSSP_DATAID_RPM : - case FSSP_DATAID_ALTITUDE : + //case FSSP_DATAID_RPM: + case FSSP_DATAID_ALTITUDE: if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter *clearToSend = false; } break; - case FSSP_DATAID_FUEL : + case FSSP_DATAID_FUEL: if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) { smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON *clearToSend = false; @@ -496,63 +498,71 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; } break; - //case FSSP_DATAID_ADC1 : - //case FSSP_DATAID_ADC2 : - //case FSSP_DATAID_CAP_USED : - case FSSP_DATAID_VARIO : + //case FSSP_DATAID_ADC1: + //case FSSP_DATAID_ADC2: + //case FSSP_DATAID_CAP_USED: + case FSSP_DATAID_VARIO: if (sensors(SENSOR_BARO)) { smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s *clearToSend = false; } break; - case FSSP_DATAID_HEADING : + case FSSP_DATAID_HEADING: smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg *clearToSend = false; break; - case FSSP_DATAID_PITCH : + case FSSP_DATAID_PITCH: if (telemetryConfig()->frsky_pitch_roll) { smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg *clearToSend = false; } break; - case FSSP_DATAID_ROLL : + case FSSP_DATAID_ROLL: if (telemetryConfig()->frsky_pitch_roll) { smartPortSendPackage(id, attitude.values.roll); // given in 10*deg *clearToSend = false; } break; - case FSSP_DATAID_ACCX : + case FSSP_DATAID_ACCX: if (!telemetryConfig()->frsky_pitch_roll) { smartPortSendPackage(id, lrintf(100 * acc.accADCf[X])); *clearToSend = false; } break; - case FSSP_DATAID_ACCY : + case FSSP_DATAID_ACCY: if (!telemetryConfig()->frsky_pitch_roll) { smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y])); *clearToSend = false; } break; - case FSSP_DATAID_ACCZ : + case FSSP_DATAID_ACCZ: if (!telemetryConfig()->frsky_pitch_roll) { smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z])); *clearToSend = false; } break; - case FSSP_DATAID_T1 : + case FSSP_DATAID_MODES: { + if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids) + id = FSSP_DATAID_LEGACY_MODES; + smartPortSendPackage(id, frskyGetFlightMode()); *clearToSend = false; break; } #ifdef USE_GPS - case FSSP_DATAID_T2 : - if (smartPortShouldSendGPSData()) { - smartPortSendPackage(id, frskyGetGPSState()); - *clearToSend = false; + case FSSP_DATAID_GNSS: + { + if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids) + id = FSSP_DATAID_LEGACY_GNSS; + + if (smartPortShouldSendGPSData()) { + smartPortSendPackage(id, frskyGetGPSState()); + *clearToSend = false; + } + break; } - break; - case FSSP_DATAID_SPEED : + case FSSP_DATAID_SPEED: if (smartPortShouldSendGPSData()) { //convert to knots: 1cm/s = 0.0194384449 knots //Speed should be sent in knots/1000 (GPS speed is in cm/s) @@ -561,7 +571,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; } break; - case FSSP_DATAID_LATLONG : + case FSSP_DATAID_LATLONG: if (smartPortShouldSendGPSData()) { uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude @@ -581,25 +591,25 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear *clearToSend = false; } break; - case FSSP_DATAID_HOME_DIST : + case FSSP_DATAID_HOME_DIST: if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, GPS_distanceToHome); *clearToSend = false; } break; - case FSSP_DATAID_GPS_ALT : + case FSSP_DATAID_GPS_ALT: if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, gpsSol.llh.alt); // cm *clearToSend = false; } break; - case FSSP_DATAID_FPV : + case FSSP_DATAID_FPV: if (smartPortShouldSendGPSData()) { smartPortSendPackage(id, gpsSol.groundCourse); // given in 10*deg *clearToSend = false; } break; - case FSSP_DATAID_AZIMUTH : + case FSSP_DATAID_AZIMUTH: if (smartPortShouldSendGPSData()) { int16_t h = GPS_directionToHome; if (h < 0) { @@ -614,13 +624,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear } break; #endif - case FSSP_DATAID_A4 : + case FSSP_DATAID_A4: if (isBatteryVoltageConfigured()) { smartPortSendPackage(id, getBatteryAverageCellVoltage()); *clearToSend = false; } break; - case FSSP_DATAID_ASPD : + case FSSP_DATAID_ASPD: #ifdef USE_PITOT if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1 diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d6c8e355d86..26f9631a89d 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -59,6 +59,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT, .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT, + .frsky_use_legacy_gps_mode_sensor_ids = SETTING_FRSKY_USE_LEGACY_GPS_MODE_SENSOR_IDS_DEFAULT, .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT, .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT, .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT, diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c7edad973b8..7be5e50a9ce 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -46,6 +46,7 @@ typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_inverted; // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry. uint8_t frsky_pitch_roll; + bool frsky_use_legacy_gps_mode_sensor_ids; uint8_t report_cell_voltage; uint8_t hottAlarmSoundInterval; uint8_t halfDuplex; From 64e65349b11db9c933daa51948ed70631bacce32 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 21 Apr 2024 15:16:21 +0100 Subject: [PATCH 034/175] Initial code --- src/main/io/osd.c | 143 ++++++++++++++++++++++++++++++++-------------- 1 file changed, 101 insertions(+), 42 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 56245ea04af..eb99ef917e9 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1712,6 +1712,20 @@ void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { buff[ptr] = '\0'; } +static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) { + bool elementEnabled = false; + + if (onlyCurrentLayout) + elementEnabled = (osdLayoutsConfig()->item_pos[currentLayout][elementID]) ? true : false; + else { + for (uint8_t layout = 0; layout < 4 || elementEnabled; layout++) { + elementEnabled = (osdLayoutsConfig()->item_pos[layout][elementID]) ? true : false; + } + } + + return elementEnabled; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -5122,19 +5136,21 @@ static void osdShowHDArmScreen(void) char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; uint8_t safehomeRow = 0; - uint8_t armScreenRow = 2; // Start at row 2 + uint8_t armScreenRow = 1; armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; - if (strlen(systemConfig()->craftName) > 0) { + if (osdElementEnabled(OSD_CRAFT_NAME, false) && strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - strcpy(buf2, "ARMED!"); + strcat(buf2, ": ! ARMED !"); tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); } else { - strcpy(buf, "ARMED!"); + strcpy(buf, " ! ARMED !"); } displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); + #if defined(USE_GPS) #if defined (USE_SAFE_HOME) if (posControl.safehomeState.distance) { @@ -5149,35 +5165,43 @@ static void osdShowHDArmScreen(void) #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #else strcpy(buf, "*MISSION LOADED*"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #endif } - armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { - if (osdConfig()->osd_home_position_arm_screen){ - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); - uint8_t gap = 1; - uint8_t col = strlen(buf) + strlen(buf2) + gap; - - if ((osdDisplayPort->cols %2) != (col %2)) { - gap++; - col++; - } - - col = (osdDisplayPort->cols - col) / 2; + if (osdConfig()->osd_home_position_arm_screen) { + if (osdElementEnabled(OSD_PLUS_CODE, false)) { + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0'); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } else { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + uint8_t gap = 1; + uint8_t col = strlen(buf) + strlen(buf2) + gap; + + if ((osdDisplayPort->cols %2) != (col %2)) { + gap++; + col++; + } - displayWrite(osdDisplayPort, col, armScreenRow, buf); - displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + col = (osdDisplayPort->cols - col) / 2; - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + displayWrite(osdDisplayPort, col, armScreenRow, buf); + displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } } #if defined (USE_SAFE_HOME) @@ -5191,11 +5215,14 @@ static void osdShowHDArmScreen(void) textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); } armScreenRow++; } @@ -5204,6 +5231,7 @@ static void osdShowHDArmScreen(void) if (rtcGetDateTimeLocal(&dt)) { tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); armScreenRow++; } @@ -5224,45 +5252,72 @@ static void osdShowSDArmScreen(void) char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; - // We need 12 visible rows, start row never < first fully visible row 1 - uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; + uint8_t armScreenRow = 1; uint8_t safehomeRow = 0; - strcpy(buf, "ARMED!"); + strcpy(buf, "! ARMED !"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); - safehomeRow = armScreenRow; - armScreenRow++; + memset(buf, '\0', sizeof(buf)); +#if defined(USE_GPS) +#if defined (USE_SAFE_HOME) + if (posControl.safehomeState.distance) { + safehomeRow = armScreenRow; + armScreenRow++; + } +#endif +#endif + + if (osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) + osdFormatPilotName(buf2); - if (strlen(systemConfig()->craftName) > 0) { + if (osdElementEnabled(OSD_CRAFT_NAME, false) && strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf ); + if (strlen(buf2) > 0) { + strcat(buf2, ": "); + strcat(buf2, craftNameBuf); + } else + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(craftNameBuf)) / 2, armScreenRow++, craftNameBuf ); + } + + if (strlen(buf2) > 0) { + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2 ); + memset(buf2, '\0', sizeof(buf2)); } if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #else strcpy(buf, "*MISSION LOADED*"); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); #endif } - armScreenRow++; #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { if (osdConfig()->osd_home_position_arm_screen) { - osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); - osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); - - uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; - displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); - displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); - - int digits = osdConfig()->plus_code_digits; - olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates. + if (osdElementEnabled(OSD_PLUS_CODE, false)) { + int digits = osdConfig()->plus_code_digits; + olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); + tfp_sprintf(buf2, "+CODE: %s%c", buf, '\0'); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } else { + osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat); + osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon); + + uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2; + displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf); + displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); + } } #if defined (USE_SAFE_HOME) @@ -5276,11 +5331,14 @@ static void osdShowSDArmScreen(void) textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); } #endif } else { strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); } armScreenRow++; } @@ -5289,6 +5347,7 @@ static void osdShowSDArmScreen(void) if (rtcGetDateTimeLocal(&dt)) { tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); + memset(buf, '\0', sizeof(buf)); armScreenRow++; } From 4d9e9d318ed069f5fe6bcbb6e98fa16593a1c0b9 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 23 Apr 2024 22:09:13 +0100 Subject: [PATCH 035/175] Fix element detection and tidy up --- src/main/io/osd.c | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index eb99ef917e9..da8cde972c0 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1715,11 +1715,11 @@ void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { static bool osdElementEnabled(uint8_t elementID, bool onlyCurrentLayout) { bool elementEnabled = false; - if (onlyCurrentLayout) - elementEnabled = (osdLayoutsConfig()->item_pos[currentLayout][elementID]) ? true : false; - else { - for (uint8_t layout = 0; layout < 4 || elementEnabled; layout++) { - elementEnabled = (osdLayoutsConfig()->item_pos[layout][elementID]) ? true : false; + if (onlyCurrentLayout) { + elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][elementID]); + } else { + for (uint8_t layout = 0; layout < 4 && !elementEnabled; layout++) { + elementEnabled = OSD_VISIBLE(osdLayoutsConfig()->item_pos[layout][elementID]); } } @@ -5141,25 +5141,30 @@ static void osdShowHDArmScreen(void) armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; + if (!osdConfig()->use_pilot_logo && osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) + osdFormatPilotName(buf2); + if (osdElementEnabled(OSD_CRAFT_NAME, false) && strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); - strcat(buf2, ": ! ARMED !"); - tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2); + if (strlen(buf2) > 0) { + strcat(buf2, " : "); + } + tfp_sprintf(buf, "%s%s: ! ARMED !", buf2, craftNameBuf); } else { strcpy(buf, " ! ARMED !"); } displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); + memset(buf2, '\0', sizeof(buf2)); #if defined(USE_GPS) #if defined (USE_SAFE_HOME) if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; - armScreenRow++; + armScreenRow +=2; } #endif // USE_SAFE_HOME #endif // USE_GPS - armScreenRow++; if (posControl.waypointListValid && posControl.waypointCount > 0) { #ifdef USE_MULTI_MISSION @@ -5177,6 +5182,7 @@ static void osdShowHDArmScreen(void) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { if (osdConfig()->osd_home_position_arm_screen) { + // Show pluscode if enabled on any OSD layout. Otherwise show GNSS cordinates. if (osdElementEnabled(OSD_PLUS_CODE, false)) { int digits = osdConfig()->plus_code_digits; olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf)); @@ -5224,7 +5230,6 @@ static void osdShowHDArmScreen(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); } - armScreenRow++; } #endif @@ -5262,7 +5267,7 @@ static void osdShowSDArmScreen(void) #if defined (USE_SAFE_HOME) if (posControl.safehomeState.distance) { safehomeRow = armScreenRow; - armScreenRow++; + armScreenRow += 2; } #endif #endif @@ -5282,6 +5287,7 @@ static void osdShowSDArmScreen(void) if (strlen(buf2) > 0) { displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2 ); memset(buf2, '\0', sizeof(buf2)); + armScreenRow++; } if (posControl.waypointListValid && posControl.waypointCount > 0) { @@ -5340,7 +5346,6 @@ static void osdShowSDArmScreen(void) displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); } - armScreenRow++; } #endif From de09caecfd5d5b08ec27997480f75178ccfed45d Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 15 May 2024 08:07:47 +0100 Subject: [PATCH 036/175] Add blank line after "no home position" --- src/main/io/osd.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index da8cde972c0..6e3948f61aa 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5229,6 +5229,7 @@ static void osdShowHDArmScreen(void) strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); + armScreenRow++; } } #endif @@ -5345,6 +5346,7 @@ static void osdShowSDArmScreen(void) strcpy(buf, "!NO HOME POSITION!"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); + armScreenRow++; } } #endif From e6ddb2440e8c7f8d821408ee2b3a57a83eb0d2b5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 17:51:52 +0200 Subject: [PATCH 037/175] Add setting for minimum ammount of tx buffer --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 6 ++++++ src/main/telemetry/mavlink.c | 2 +- src/main/telemetry/telemetry.c | 3 ++- src/main/telemetry/telemetry.h | 1 + 5 files changed, 20 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index da0b77f8ce1..10a4e1bfd7f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2542,6 +2542,16 @@ Rate of the extra3 message for MAVLink telemetry --- +### mavlink_min_txbuffer + +Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits. + +| Default | Min | Max | +| --- | --- | --- | +| | 0 | 100 | + +--- + ### mavlink_pos_rate Rate of the position message for MAVLink telemetry diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 52d6d63f66e..2f19a8d0a49 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3191,6 +3191,12 @@ groups: min: 1 max: 2 default_value: 2 + - name: mavlink_min_txbuffer + file: mavlink.min_txbuff + description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." + default: 100 + min: 0 + max: 100 - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 59d1ef69463..4becf6375f4 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1217,7 +1217,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) incomingRequestServed = true; } - if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= 90) { + if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) { // Only process scheduled data if we didn't serve any incoming request this cycle if (!incomingRequestServed || ( diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d6c8e355d86..d2334de891a 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -89,7 +89,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, - .version = SETTING_MAVLINK_VERSION_DEFAULT + .version = SETTING_MAVLINK_VERSION_DEFAULT, + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c7edad973b8..d74c32d77e7 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -72,6 +72,7 @@ typedef struct telemetryConfig_s { uint8_t extra2_rate; uint8_t extra3_rate; uint8_t version; + uint8_t min_txbuff; } mavlink; } telemetryConfig_t; From 6e549d69a418c133d1382e6f788c9a661b22b558 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:14:54 +0200 Subject: [PATCH 038/175] Update receiver quality statistics, if using mavlink as serialrx_provider --- src/main/telemetry/mavlink.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 4becf6375f4..67c4cf59b9a 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1116,6 +1116,13 @@ static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); txbuff_free = msg.txbuf; + + if (rxConfig()->receiverType == RX_TYPE_SERIAL && + rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { + rxLinkStatistics.uplinkRSSI = -msg.remrssi; // dbM + rxLinkStatistics.uplinkSNR = msg.noise; // dbM * 2? + rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100); // May be elrs specific + } return true; } From 309412df22bae620ad7f7c645585b6566d443ae0 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:22:36 +0200 Subject: [PATCH 039/175] Small fixes --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/telemetry/telemetry.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 10a4e1bfd7f..889cb55da17 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry | Default | Min | Max | | --- | --- | --- | -| | 0 | 100 | +| 100 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2f19a8d0a49..4ea5e73d2f2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3192,9 +3192,9 @@ groups: max: 2 default_value: 2 - name: mavlink_min_txbuffer - file: mavlink.min_txbuff + field: mavlink.min_txbuff description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." - default: 100 + default_value: 100 min: 0 max: 100 diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index d2334de891a..4200c18c6a0 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -90,7 +90,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT } ); From 3125fc08fdbcac60b5779b5acff372f21bf72501 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 10 Jul 2024 18:24:18 +0200 Subject: [PATCH 040/175] Bump pg version --- src/main/telemetry/telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 4200c18c6a0..0d846ad19a8 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -53,7 +53,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, From 44d99f3dd415064298a2a009f22d60e7e9193d65 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 11 Jul 2024 15:48:39 +0200 Subject: [PATCH 041/175] improve half duplex checks --- src/main/telemetry/mavlink.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 67c4cf59b9a..39ac3524d29 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1207,6 +1207,11 @@ static bool processMAVLinkIncomingTelemetry(void) return false; } +static bool isMAVLinkTelemetryHalfDuplex(void) { + return (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex)) + || telemetryConfig()->halfDuplex; +} + void handleMAVLinkTelemetry(timeUs_t currentTimeUs) { static bool incomingRequestServed; @@ -1226,16 +1231,10 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) { // Only process scheduled data if we didn't serve any incoming request this cycle - if (!incomingRequestServed || - ( - (rxConfig()->receiverType == RX_TYPE_SERIAL) && - (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) && - !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex) - ) - ) { + if (!incomingRequestServed || !isMAVLinkTelemetryHalfDuplex()) { processMAVLinkTelemetry(currentTimeUs); + lastMavlinkMessage = currentTimeUs; } - lastMavlinkMessage = currentTimeUs; incomingRequestServed = false; } } From edd2a67ce8cf77b22371e7e17922f05905eaaee9 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 16:14:31 +0200 Subject: [PATCH 042/175] Sync txbuff value with @MUSTARDTIGERFPV --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/telemetry/mavlink.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c588e31169c..3926fb1bb5f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry | Default | Min | Max | | --- | --- | --- | -| 100 | 0 | 100 | +| 33 | 0 | 100 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 35110159105..57511737f0e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3194,7 +3194,7 @@ groups: - name: mavlink_min_txbuffer field: mavlink.min_txbuff description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits." - default_value: 100 + default_value: 33 min: 0 max: 100 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ee372a3cfc1..1c58f36e493 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= 33; + shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); From ca22dba0ca581bd4514404308876713751fd63c9 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 16:40:21 +0200 Subject: [PATCH 043/175] Fix typo --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 1c58f36e493..0e30dc00ac6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) // Determine whether to send telemetry back based on flow control / pacing if (txbuff_valid) { // Use flow control if available - shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff; + shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff; } else { // If not, use blind frame pacing - and back off for collision avoidance if half-duplex bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage); From 7a420cbdc20c2329cd65cf99f217f57139d42be1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:03:07 +0200 Subject: [PATCH 044/175] Parse rssi/lq infor from radio_status --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 9 +++++++++ src/main/telemetry/mavlink.c | 26 ++++++++++++++++++++++---- src/main/telemetry/telemetry.c | 3 ++- src/main/telemetry/telemetry.h | 7 +++++++ 5 files changed, 50 insertions(+), 5 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 3926fb1bb5f..9f34dd5f479 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2562,6 +2562,16 @@ Rate of the position message for MAVLink telemetry --- +### mavlink_radio_type + +Mavlink radio type. Affects how RSSI and LQ are reported on OSD. + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_rc_chan_rate Rate of the RC channels message for MAVLink telemetry diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 57511737f0e..d1fafb27c10 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,9 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: mavlink_radio_type + values: ["GENERIC", "ELRS", "SIK"] + enum: mavlinkRadio_e constants: RPYL_PID_MIN: 0 @@ -3197,6 +3200,12 @@ groups: default_value: 33 min: 0 max: 100 + - name: mavlink_radio_type + description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD." + field: mavlink.radio_type + table: mavlink_radio_type + default_value: "GENERIC" + type: uint8_t - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 0e30dc00ac6..9d9babe64f7 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1126,6 +1126,27 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { return true; } +static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { + switch(telemetryConfig()->mavlink.radio_type) { + case MAVLINK_RADIO_SIK: + rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; + rxLinkStatistics.uplinkSNR = msg->noise * 1.9; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + case MAVLINK_RADIO_ELRS: + rxLinkStatistics.uplinkRSSI = -msg->remrssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100); + break; + case MAVLINK_RADIO_GENERIC: + default: + rxLinkStatistics.uplinkRSSI = msg->rssi; + rxLinkStatistics.uplinkSNR = msg->noise; + rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; + break; + } +} + static bool handleIncoming_RADIO_STATUS(void) { mavlink_radio_status_t msg; mavlink_msg_radio_status_decode(&mavRecvMsg, &msg); @@ -1134,10 +1155,7 @@ static bool handleIncoming_RADIO_STATUS(void) { if (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK) { - // TODO: decide what to do here - rxLinkStatistics.uplinkRSSI = -msg.remrssi; // dbM - rxLinkStatistics.uplinkSNR = msg.noise; // dbM * 2? - rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100); // May be elrs specific + mavlinkParseRxStats(&msg); } return true; diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 0d846ad19a8..8a8796eecb8 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -90,7 +90,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, - .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT + .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index d74c32d77e7..4ace4c5943d 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,6 +36,12 @@ typedef enum { LTM_RATE_SLOW } ltmUpdateRate_e; +typedef enum { + MAVLINK_RADIO_GENERIC, + MAVLINK_RADIO_ELRS, + MAVLINK_RADIO_SIK, +} mavlinkRadio_e; + typedef enum { SMARTPORT_FUEL_UNIT_PERCENT, SMARTPORT_FUEL_UNIT_MAH, @@ -73,6 +79,7 @@ typedef struct telemetryConfig_s { uint8_t extra3_rate; uint8_t version; uint8_t min_txbuff; + uint8_t radio_type; } mavlink; } telemetryConfig_t; From b83ddda361d63eec884ba467dbc35bf3e5f11929 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:04:58 +0200 Subject: [PATCH 045/175] Bump PG_VERSION --- src/main/telemetry/telemetry.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 8a8796eecb8..89eca3b1dd6 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -53,7 +53,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, From 3395dadf518f688f2986209a2bf976bcbbcab9d7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:07:28 +0200 Subject: [PATCH 046/175] Fix scaling --- src/main/telemetry/mavlink.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 9d9babe64f7..19bec9f62c8 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1130,7 +1130,7 @@ static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { switch(telemetryConfig()->mavlink.radio_type) { case MAVLINK_RADIO_SIK: rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; - rxLinkStatistics.uplinkSNR = msg->noise * 1.9; + rxLinkStatistics.uplinkSNR = msg->noise / 1.9; rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; break; case MAVLINK_RADIO_ELRS: From 9d69a15e4d88d7601712ba2d995308df1a8faf72 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Jul 2024 21:14:29 +0200 Subject: [PATCH 047/175] Add comment with source of dbm formula --- src/main/telemetry/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 19bec9f62c8..617c883e4f6 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1129,6 +1129,7 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) { static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) { switch(telemetryConfig()->mavlink.radio_type) { case MAVLINK_RADIO_SIK: + // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127; rxLinkStatistics.uplinkSNR = msg->noise / 1.9; rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0; From f5e209cbd787a4c07d627c2eb564d5bbff60e2a8 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 17 Jul 2024 09:01:27 +0100 Subject: [PATCH 048/175] nav_vel_z_improvement --- .../navigation/navigation_pos_estimator.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d5a342173d3..dba1209b078 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -342,6 +342,12 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } + +static bool gravityCalibrationIsValid(void) +{ + return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS; +} + #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -409,12 +415,13 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) { - if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { + if (!ARMING_FLAG(WAS_EVER_ARMED)) { zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); if (gravityCalibrationComplete()) { zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); setGravityCalibration(posEstimator.imu.calibratedGravityCMSS); + restartGravityCalibration(); // Repeat calibration until first arm to continuously update zero datum LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); } } @@ -423,8 +430,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - /* If calibration is incomplete - report zero acceleration */ - if (gravityCalibrationComplete()) { + if (gravityCalibrationIsValid()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS; @@ -432,7 +438,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) #endif posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } - else { + else { // If calibration is incomplete - report zero acceleration posEstimator.imu.accelNEU.x = 0.0f; posEstimator.imu.accelNEU.y = 0.0f; posEstimator.imu.accelNEU.z = 0.0f; @@ -521,7 +527,9 @@ static void estimationPredict(estimationContext_t * ctx) if ((ctx->newFlags & EST_Z_VALID)) { posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt; posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; - posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + if (ARMING_FLAG(WAS_EVER_ARMED)) { // Hold at zero until first armed + posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + } } /* Prediction step: XY-axis */ @@ -599,7 +607,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } if (ctx->newFlags & EST_GPS_Z_VALID) { @@ -623,7 +631,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } return correctOK; @@ -907,5 +915,5 @@ void updatePositionEstimator(void) bool navIsCalibrationComplete(void) { - return gravityCalibrationComplete(); + return gravityCalibrationIsValid(); } From 569bd14515a97e55186fde9f982a9d9d1189dddb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 23 Jul 2024 10:03:13 +0100 Subject: [PATCH 049/175] add default altitude source setting --- docs/Settings.md | 10 ++++ src/main/fc/settings.yaml | 10 +++- src/main/navigation/navigation.h | 29 +++++----- .../navigation/navigation_pos_estimator.c | 58 ++++++++++++------- .../navigation_pos_estimator_private.h | 5 ++ 5 files changed, 75 insertions(+), 37 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..9afdd789685 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1992,6 +1992,16 @@ Uncertainty value for barometric sensor [cm] --- +### inav_default_alt_sensor + +Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. + +| Default | Min | Max | +| --- | --- | --- | +| GPS | | | + +--- + ### inav_gravity_cal_tolerance Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 00b08ca698c..c65d0ffa768 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -83,7 +83,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"] - name: aux_operator values: ["OR", "AND"] @@ -198,6 +198,9 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: default_altitude_source + values: ["GPS", "BARO"] + enum: navDefaultAltitudeSensor_e constants: RPYL_PID_MIN: 0 @@ -2461,6 +2464,11 @@ groups: field: baro_epv min: 0 max: 9999 + - name: inav_default_alt_sensor + description: "Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv." + default_value: "GPS" + field: default_alt_sensor + table: default_altitude_source - name: PG_NAV_CONFIG type: navConfig_t diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..da65f0f3f57 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -231,35 +231,36 @@ typedef enum { typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; - uint8_t reset_altitude_type; // from nav_reset_type_e - uint8_t reset_home_type; // nav_reset_type_e - uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) + uint8_t reset_altitude_type; // from nav_reset_type_e + uint8_t reset_home_type; // nav_reset_type_e + uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) uint8_t allow_dead_reckoning; uint16_t max_surface_altitude; - float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements - float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements - float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements + float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements + float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements - float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes - float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements + float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes + float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements - float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements - float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements + float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements + float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements float w_xy_flow_p; float w_xy_flow_v; - float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight + float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight float w_xy_res_v; - float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - float max_eph_epv; // Max estimated position error acceptable for estimation (cm) - float baro_epv; // Baro position error + float max_eph_epv; // Max estimated position error acceptable for estimation (cm) + float baro_epv; // Baro position error + uint8_t default_alt_sensor; // default altitude sensor source #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; #endif diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index dba1209b078..e11342ff38c 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -56,7 +56,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -89,6 +89,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, + + .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT, #ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT #endif @@ -560,20 +562,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; + float wBaro = 1.0f; + float wGps = 1.0f; + + if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) { + const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); - float wBaro = 0.0f; - if (ctx->newFlags & EST_BARO_VALID) { - // Ignore baro if difference is too big, baro is probably wrong - const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; + // Fade out the non default sensor to prevent sudden jump + uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv; + const float start_epv = residualErrorEpvLimit; + const float end_epv = residualErrorEpvLimit * 2.0f; - // Fade out the baro to prevent sudden jump - const float start_epv = positionEstimationConfig()->max_eph_epv; - const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f; + // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); + + if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO + wGps = wBaro; + wBaro = 1.0f; + } } - // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS - if (wBaro > 0.01f) { + if (ctx->newFlags & EST_BARO_VALID && wBaro) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -596,21 +606,24 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); // Altitude - const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; - ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); + + ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } - if (ctx->newFlags & EST_GPS_Z_VALID) { + if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; @@ -619,16 +632,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else { // Altitude - const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; - const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z; + const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); + const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + + ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; + ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; - ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 44f0333d149..46fcb42e177 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -146,6 +146,11 @@ typedef enum { EST_Z_VALID = (1 << 6), } navPositionEstimationFlags_e; +typedef enum { + ALTITUDE_SOURCE_GPS, + ALTITUDE_SOURCE_BARO, +} navDefaultAltitudeSensor_e; + typedef struct { timeUs_t baroGroundTimeout; float baroGroundAlt; From 030b52e51d1750377af8e8ebd15fb5b2c1899e97 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 5 Aug 2024 12:22:15 +0300 Subject: [PATCH 050/175] Removed Switch Indicators and increase custom to 8 Removed switch indicators and increased the number of osd custom elements to 8. The number of osd custom elements and custom element parts can now be automatically changed using the defined numbers. MSP change Requires Configurator --- src/main/fc/fc_msp.c | 42 +++++++------ src/main/fc/settings.yaml | 53 ----------------- src/main/io/osd.c | 91 +++++++++-------------------- src/main/io/osd.h | 24 +++----- src/main/io/osd/custom_elements.c | 1 - src/main/io/osd/custom_elements.h | 2 +- src/main/msp/msp_protocol_v2_inav.h | 3 +- 7 files changed, 60 insertions(+), 156 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c8ae27f270d..e8cb5c39054 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1706,28 +1706,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_CUSTOM_OSD_ELEMENTS: - sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS); - sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1); - - for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) { - const osdCustomElement_t *customElement = osdCustomElements(i); - for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) { - sbufWriteU8(dst, customElement->part[ii].type); - sbufWriteU16(dst, customElement->part[ii].value); - } - sbufWriteU8(dst, customElement->visibility.type); - sbufWriteU16(dst, customElement->visibility.value); - for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) { - sbufWriteU8(dst, customElement->osdCustomElementText[ii]); - } + { + sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS); + sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1); + sbufWriteU8(dst, CUSTOM_ELEMENTS_PARTS); } break; +#endif default: return false; } return true; } -#endif + #ifdef USE_SAFE_HOME static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src) @@ -3349,7 +3340,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS: sbufReadU8Safe(&tmp_u8, src); - if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (MAX_CUSTOM_ELEMENTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) { + if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) { for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) { osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src); osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src); @@ -3365,7 +3356,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; - +#endif case MSP2_BETAFLIGHT_BIND: if (rxConfig()->receiverType == RX_TYPE_SERIAL) { switch (rxConfig()->serialrx_provider) { @@ -3392,7 +3383,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } return MSP_RESULT_ACK; } -#endif static const setting_t *mspReadSetting(sbuf_t *src) { @@ -3849,6 +3839,22 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu case MSP2_INAV_LOGIC_CONDITIONS_SINGLE: *ret = mspFcLogicConditionCommand(dst, src); break; + case MSP2_INAV_CUSTOM_OSD_ELEMENT: + const uint8_t idx = sbufReadU8(src); + + if (idx < MAX_CUSTOM_ELEMENTS) { + const osdCustomElement_t *customElement = osdCustomElements(idx); + for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) { + sbufWriteU8(dst, customElement->part[ii].type); + sbufWriteU16(dst, customElement->part[ii].value); + } + sbufWriteU8(dst, customElement->visibility.type); + sbufWriteU16(dst, customElement->visibility.value); + for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) { + sbufWriteU8(dst, customElement->osdCustomElementText[ii]); + } + } + break; #endif #ifdef USE_SAFE_HOME case MSP2_INAV_SAFEHOME: diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c16ce93b21..1bd1400d0a6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3706,59 +3706,6 @@ groups: min: 1000 max: 5000 default_value: 1500 - - name: osd_switch_indicator_zero_name - description: "Character to use for OSD switch incicator 0." - field: osd_switch_indicator0_name - type: string - max: 5 - default_value: "FLAP" - - name: osd_switch_indicator_one_name - description: "Character to use for OSD switch incicator 1." - field: osd_switch_indicator1_name - type: string - max: 5 - default_value: "GEAR" - - name: osd_switch_indicator_two_name - description: "Character to use for OSD switch incicator 2." - field: osd_switch_indicator2_name - type: string - max: 5 - default_value: "CAM" - - name: osd_switch_indicator_three_name - description: "Character to use for OSD switch incicator 3." - field: osd_switch_indicator3_name - type: string - max: 5 - default_value: "LIGT" - - name: osd_switch_indicator_zero_channel - description: "RC Channel to use for OSD switch indicator 0." - field: osd_switch_indicator0_channel - min: 5 - max: MAX_SUPPORTED_RC_CHANNEL_COUNT - default_value: 5 - - name: osd_switch_indicator_one_channel - description: "RC Channel to use for OSD switch indicator 1." - field: osd_switch_indicator1_channel - min: 5 - max: MAX_SUPPORTED_RC_CHANNEL_COUNT - default_value: 5 - - name: osd_switch_indicator_two_channel - description: "RC Channel to use for OSD switch indicator 2." - field: osd_switch_indicator2_channel - min: 5 - max: MAX_SUPPORTED_RC_CHANNEL_COUNT - default_value: 5 - - name: osd_switch_indicator_three_channel - description: "RC Channel to use for OSD switch indicator 3." - field: osd_switch_indicator3_channel - min: 5 - max: MAX_SUPPORTED_RC_CHANNEL_COUNT - default_value: 5 - - name: osd_switch_indicators_align_left - description: "Align text to left of switch indicators" - field: osd_switch_indicators_align_left - type: bool - default_value: ON - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d682c0299ff..77762c6a003 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -225,7 +225,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12); -PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); void osdStartedSaveProcess(void) { savingSettings = true; @@ -1610,40 +1610,6 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex) return geoWaypointIndex - posControl.startWpIndex + 1; } -void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { - int8_t ptr = 0; - - if (osdConfig()->osd_switch_indicators_align_left) { - for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { - buff[ptr] = swName[ptr]; - } - - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } - } else { - if ( rcValue < 1333) { - buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; - } else if ( rcValue > 1666) { - buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; - } else { - buff[ptr++] = SYM_SWITCH_INDICATOR_MID; - } - - for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { - buff[ptr] = swName[ptr-1]; - } - - ptr++; - } - - buff[ptr] = '\0'; -} - static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -1671,6 +1637,31 @@ static bool osdDrawSingleElement(uint8_t item) customElementDrawElement(buff, 2); break; } + case OSD_CUSTOM_ELEMENT_4: + { + customElementDrawElement(buff, 3); + break; + } + case OSD_CUSTOM_ELEMENT_5: + { + customElementDrawElement(buff, 4); + break; + } + case OSD_CUSTOM_ELEMENT_6: + { + customElementDrawElement(buff, 5); + break; + } + case OSD_CUSTOM_ELEMENT_7: + { + customElementDrawElement(buff, 6); + break; + } + case OSD_CUSTOM_ELEMENT_8: + { + customElementDrawElement(buff, 7); + break; + } case OSD_RSSI_VALUE: { uint16_t osdRssi = osdConvertRSSI(); @@ -2873,22 +2864,6 @@ static bool osdDrawSingleElement(uint8_t item) } #endif - case OSD_SWITCH_INDICATOR_0: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); - break; - - case OSD_SWITCH_INDICATOR_1: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); - break; - - case OSD_SWITCH_INDICATOR_2: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); - break; - - case OSD_SWITCH_INDICATOR_3: - osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); - break; - case OSD_PAN_SERVO_CENTRED: { int16_t panOffset = osdGetPanServoOffset(); @@ -4008,15 +3983,6 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, .mAh_precision = SETTING_OSD_MAH_PRECISION_DEFAULT, - .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, - .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, - .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, - .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, - .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, - .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, - .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, - .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, - .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -4191,11 +4157,6 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); - osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); - osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7); osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8); #if defined(USE_ESC_SENSOR) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a73db015793..2e5200a0483 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -263,10 +263,10 @@ typedef enum { OSD_AIR_MAX_SPEED, OSD_ACTIVE_PROFILE, OSD_MISSION, - OSD_SWITCH_INDICATOR_0, - OSD_SWITCH_INDICATOR_1, - OSD_SWITCH_INDICATOR_2, - OSD_SWITCH_INDICATOR_3, + OSD_CUSTOM_ELEMENT_4, + OSD_CUSTOM_ELEMENT_5, + OSD_CUSTOM_ELEMENT_6, + OSD_CUSTOM_ELEMENT_7, OSD_TPA_TIME_CONSTANT, OSD_FW_LEVEL_TRIM, OSD_GLIDE_TIME_REMAINING, @@ -286,7 +286,8 @@ typedef enum { OSD_ADSB_WARNING, //150 OSD_ADSB_INFO, OSD_BLACKBOX, - OSD_FORMATION_FLIGHT, //153 + OSD_FORMATION_FLIGHT, + OSD_CUSTOM_ELEMENT_8, // 154 OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -347,8 +348,6 @@ typedef struct osdLayoutsConfig_s { PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); -#define OSD_SWITCH_INDICATOR_NAME_LENGTH 4 - typedef struct osdConfig_s { // Alarms uint8_t rssi_alarm; // rssi % @@ -444,17 +443,8 @@ typedef struct osdConfig_s { uint8_t telemetry; // use telemetry on displayed pixel line 0 uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers. uint16_t system_msg_display_time; // system message display time for multiple messages (ms) - uint8_t mAh_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh + uint8_t mAh_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) - char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. - uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. - char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. - uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. - char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. - uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. - char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. - uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. - bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index fbd05e2be61..132b6c820af 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -138,4 +138,3 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){ } prevLength[customElementIndex] = buffSeek; } - diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index a55b010f01a..715bae5ab2b 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -21,7 +21,7 @@ #define OSD_CUSTOM_ELEMENT_TEXT_SIZE 16 #define CUSTOM_ELEMENTS_PARTS 3 -#define MAX_CUSTOM_ELEMENTS 3 +#define MAX_CUSTOM_ELEMENTS 8 typedef enum { CUSTOM_ELEMENT_TYPE_NONE = 0, diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 8b4a09e87af..02c8c979aae 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -109,7 +109,8 @@ #define MSP2_ADSB_VEHICLE_LIST 0x2090 #define MSP2_INAV_CUSTOM_OSD_ELEMENTS 0x2100 -#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101 +#define MSP2_INAV_CUSTOM_OSD_ELEMENT 0x2101 +#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102 #define MSP2_INAV_SERVO_CONFIG 0x2200 #define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file From f3c43e8c0dd6db69bae0066ae52ed9dd049d13ff Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 6 Aug 2024 18:32:41 +0300 Subject: [PATCH 051/175] Updates - Added additional numeric options and LC options to custom element types - Changed method of displaying numbers. So that negative numbers will work. --- src/main/io/osd/custom_elements.c | 111 +++++++++++++++++++++++++++--- src/main/io/osd/custom_elements.h | 26 +++++-- 2 files changed, 124 insertions(+), 13 deletions(-) diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index 132b6c820af..07ca43db4fd 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -68,26 +68,119 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu const int customPartValue = osdCustomElements(customElementIndex)->part[customElementItemIndex].value; switch (customPartType) { - case CUSTOM_ELEMENT_TYPE_GV: + case CUSTOM_ELEMENT_TYPE_GV_1: { - osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue) * (int32_t) 100, 1, 0, 0, 6, false); - return 6; + osdFormatCentiNumber(buff,(int32_t) (constrain(gvGet(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false); + return 2; + } + case CUSTOM_ELEMENT_TYPE_GV_2: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false); + return 3; + } + case CUSTOM_ELEMENT_TYPE_GV_3: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false); + return 4; + } + case CUSTOM_ELEMENT_TYPE_GV_4: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false); + return 5; } - case CUSTOM_ELEMENT_TYPE_GV_FLOAT: + case CUSTOM_ELEMENT_TYPE_GV_5: { - osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue), 1, 2, 0, 6, false); + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false); return 6; } - case CUSTOM_ELEMENT_TYPE_GV_SMALL: + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1: { - osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 1000 ) * (int32_t) 100), 1, 0, 0, 3, false); + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false); return 3; } - case CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT: + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false); + return 4; + } + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2: + { + osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -9999, 9999), 1, 2, 0, 5, false); + return 5; + } + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 10), 1, 1, 0, 5, false); + return 5; + } + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2: + { + osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -99999, 99999), 1, 2, 0, 6, false); + return 6; + } + case CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1: { - osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 100) * (int32_t) 10), 1, 1, 0, 2, false); + osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false); + return 6; + } + + case CUSTOM_ELEMENT_TYPE_LC_1: + { + osdFormatCentiNumber(buff,(int32_t) (constrain(logicConditionGetValue(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false); return 2; } + case CUSTOM_ELEMENT_TYPE_LC_2: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false); + return 3; + } + case CUSTOM_ELEMENT_TYPE_LC_3: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false); + return 4; + } + case CUSTOM_ELEMENT_TYPE_LC_4: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false); + return 5; + } + case CUSTOM_ELEMENT_TYPE_LC_5: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false); + return 6; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false); + return 3; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false); + return 4; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2: + { + osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -9999, 9999), 1, 2, 0, 5, false); + return 5; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999) * (int32_t) 10), 1, 1, 0, 5, false); + return 5; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2: + { + osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -99999, 99999), 1, 2, 0, 6, false); + return 6; + } + case CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1: + { + osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false); + return 6; + } + + case CUSTOM_ELEMENT_TYPE_ICON_GV: { *buff = (uint8_t)gvGet(customPartValue); diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index 715bae5ab2b..73425d59ad3 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -28,10 +28,28 @@ typedef enum { CUSTOM_ELEMENT_TYPE_TEXT = 1, CUSTOM_ELEMENT_TYPE_ICON_STATIC = 2, CUSTOM_ELEMENT_TYPE_ICON_GV = 3, - CUSTOM_ELEMENT_TYPE_GV = 4, - CUSTOM_ELEMENT_TYPE_GV_FLOAT = 5, - CUSTOM_ELEMENT_TYPE_GV_SMALL = 6, - CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT = 7, + CUSTOM_ELEMENT_TYPE_GV_1 = 4, + CUSTOM_ELEMENT_TYPE_GV_2 = 5, + CUSTOM_ELEMENT_TYPE_GV_3 = 6, + CUSTOM_ELEMENT_TYPE_GV_4 = 7, + CUSTOM_ELEMENT_TYPE_GV_5 = 8, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1 = 9, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 10, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 11, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 12, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 13, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 14, + CUSTOM_ELEMENT_TYPE_LC_1 = 15, + CUSTOM_ELEMENT_TYPE_LC_2 = 16, + CUSTOM_ELEMENT_TYPE_LC_3 = 17, + CUSTOM_ELEMENT_TYPE_LC_4 = 18, + CUSTOM_ELEMENT_TYPE_LC_5 = 19, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 20, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 21, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 22, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 23, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 24, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 25, } osdCustomElementType_e; typedef enum { From dad76ec9610fdbb7e53e7f9d36a068bb8f8848ce Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 6 Aug 2024 19:41:08 +0300 Subject: [PATCH 052/175] Updated docs --- docs/Settings.md | 90 ------------------------------------------------ 1 file changed, 90 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..1e7bf47381c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5152,96 +5152,6 @@ Enabling this option will show metric efficiency statistics on the post flight s --- -### osd_switch_indicator_one_channel - -RC Channel to use for OSD switch indicator 1. - -| Default | Min | Max | -| --- | --- | --- | -| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | - ---- - -### osd_switch_indicator_one_name - -Character to use for OSD switch incicator 1. - -| Default | Min | Max | -| --- | --- | --- | -| GEAR | | 5 | - ---- - -### osd_switch_indicator_three_channel - -RC Channel to use for OSD switch indicator 3. - -| Default | Min | Max | -| --- | --- | --- | -| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | - ---- - -### osd_switch_indicator_three_name - -Character to use for OSD switch incicator 3. - -| Default | Min | Max | -| --- | --- | --- | -| LIGT | | 5 | - ---- - -### osd_switch_indicator_two_channel - -RC Channel to use for OSD switch indicator 2. - -| Default | Min | Max | -| --- | --- | --- | -| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | - ---- - -### osd_switch_indicator_two_name - -Character to use for OSD switch incicator 2. - -| Default | Min | Max | -| --- | --- | --- | -| CAM | | 5 | - ---- - -### osd_switch_indicator_zero_channel - -RC Channel to use for OSD switch indicator 0. - -| Default | Min | Max | -| --- | --- | --- | -| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | - ---- - -### osd_switch_indicator_zero_name - -Character to use for OSD switch incicator 0. - -| Default | Min | Max | -| --- | --- | --- | -| FLAP | | 5 | - ---- - -### osd_switch_indicators_align_left - -Align text to left of switch indicators - -| Default | Min | Max | -| --- | --- | --- | -| ON | OFF | ON | - ---- - ### osd_system_msg_display_time System message display cycle time for multiple messages (milliseconds). From fa166c61c6e468c70d24e4fb8ed41592748df0cc Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 6 Aug 2024 19:49:22 +0300 Subject: [PATCH 053/175] Mac issues --- src/main/fc/fc_msp.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e8cb5c39054..2749f016761 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3840,18 +3840,20 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFcLogicConditionCommand(dst, src); break; case MSP2_INAV_CUSTOM_OSD_ELEMENT: - const uint8_t idx = sbufReadU8(src); + { + const uint8_t idx = sbufReadU8(src); - if (idx < MAX_CUSTOM_ELEMENTS) { - const osdCustomElement_t *customElement = osdCustomElements(idx); - for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) { - sbufWriteU8(dst, customElement->part[ii].type); - sbufWriteU16(dst, customElement->part[ii].value); - } - sbufWriteU8(dst, customElement->visibility.type); - sbufWriteU16(dst, customElement->visibility.value); - for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) { - sbufWriteU8(dst, customElement->osdCustomElementText[ii]); + if (idx < MAX_CUSTOM_ELEMENTS) { + const osdCustomElement_t *customElement = osdCustomElements(idx); + for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) { + sbufWriteU8(dst, customElement->part[ii].type); + sbufWriteU16(dst, customElement->part[ii].value); + } + sbufWriteU8(dst, customElement->visibility.type); + sbufWriteU16(dst, customElement->visibility.value); + for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) { + sbufWriteU8(dst, customElement->osdCustomElementText[ii]); + } } } break; From 70856d7baa1e89f8cb0be92ac11cb012afaa77ec Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 7 Aug 2024 23:10:33 +0300 Subject: [PATCH 054/175] Added icon from logic condition --- src/main/io/osd/custom_elements.c | 5 ++++ src/main/io/osd/custom_elements.h | 45 ++++++++++++++++--------------- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index 07ca43db4fd..df84ddd76b0 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -186,6 +186,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu *buff = (uint8_t)gvGet(customPartValue); return 1; } + case CUSTOM_ELEMENT_TYPE_ICON_LC: + { + *buff = (uint8_t)constrain(logicConditionGetValue(customPartValue), 1, 255); + return 1; + } case CUSTOM_ELEMENT_TYPE_ICON_STATIC: { *buff = (uint8_t)customPartValue; diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index 73425d59ad3..9c12ede6734 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -28,28 +28,29 @@ typedef enum { CUSTOM_ELEMENT_TYPE_TEXT = 1, CUSTOM_ELEMENT_TYPE_ICON_STATIC = 2, CUSTOM_ELEMENT_TYPE_ICON_GV = 3, - CUSTOM_ELEMENT_TYPE_GV_1 = 4, - CUSTOM_ELEMENT_TYPE_GV_2 = 5, - CUSTOM_ELEMENT_TYPE_GV_3 = 6, - CUSTOM_ELEMENT_TYPE_GV_4 = 7, - CUSTOM_ELEMENT_TYPE_GV_5 = 8, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1 = 9, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 10, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 11, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 12, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 13, - CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 14, - CUSTOM_ELEMENT_TYPE_LC_1 = 15, - CUSTOM_ELEMENT_TYPE_LC_2 = 16, - CUSTOM_ELEMENT_TYPE_LC_3 = 17, - CUSTOM_ELEMENT_TYPE_LC_4 = 18, - CUSTOM_ELEMENT_TYPE_LC_5 = 19, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 20, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 21, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 22, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 23, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 24, - CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 25, + CUSTOM_ELEMENT_TYPE_ICON_LC = 4, + CUSTOM_ELEMENT_TYPE_GV_1 = 5, + CUSTOM_ELEMENT_TYPE_GV_2 = 6, + CUSTOM_ELEMENT_TYPE_GV_3 = 7, + CUSTOM_ELEMENT_TYPE_GV_4 = 8, + CUSTOM_ELEMENT_TYPE_GV_5 = 9, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1 = 10, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1 = 11, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2 = 12, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1 = 13, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2 = 14, + CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1 = 15, + CUSTOM_ELEMENT_TYPE_LC_1 = 16, + CUSTOM_ELEMENT_TYPE_LC_2 = 17, + CUSTOM_ELEMENT_TYPE_LC_3 = 18, + CUSTOM_ELEMENT_TYPE_LC_4 = 19, + CUSTOM_ELEMENT_TYPE_LC_5 = 20, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1 = 21, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1 = 22, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2 = 23, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1 = 24, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2 = 25, + CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1 = 26, } osdCustomElementType_e; typedef enum { From 746c168deec2790cc4f164d7e00d02b62d4ab815 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 13 Aug 2024 12:14:30 +0300 Subject: [PATCH 055/175] Updates - Updated draw for pilot/craft name - Removed blink from safehome, because it looked bad --- src/main/io/osd.c | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e9ac5dd78da..bb44cbf581b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5193,26 +5193,35 @@ static void osdShowHDArmScreen(void) dateTime_t dt; char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)]; - char craftNameBuf[MAX_NAME_LENGTH]; + char craftNameBuf[MAX_NAME_LENGTH]; char versionBuf[osdDisplayPort->cols]; uint8_t safehomeRow = 0; uint8_t armScreenRow = 1; + bool showPilotOrCraftName = false; + armScreenRow = drawLogos(false, armScreenRow); armScreenRow++; - if (!osdConfig()->use_pilot_logo && osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) + if (!osdConfig()->use_pilot_logo && osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) { osdFormatPilotName(buf2); + showPilotOrCraftName = true; + } if (osdElementEnabled(OSD_CRAFT_NAME, false) && strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); if (strlen(buf2) > 0) { strcat(buf2, " : "); } + showPilotOrCraftName = true; + } + + if (showPilotOrCraftName) { tfp_sprintf(buf, "%s%s: ! ARMED !", buf2, craftNameBuf); } else { strcpy(buf, " ! ARMED !"); } + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); memset(buf, '\0', sizeof(buf)); memset(buf2, '\0', sizeof(buf2)); @@ -5320,6 +5329,7 @@ static void osdShowSDArmScreen(void) char versionBuf[osdDisplayPort->cols]; uint8_t armScreenRow = 1; uint8_t safehomeRow = 0; + bool showPilotOrCraftName = false; strcpy(buf, "! ARMED !"); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf); @@ -5333,20 +5343,23 @@ static void osdShowSDArmScreen(void) #endif #endif - if (osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) + if (osdElementEnabled(OSD_PILOT_NAME, false) && strlen(systemConfig()->pilotName) > 0) { osdFormatPilotName(buf2); + showPilotOrCraftName = true; + } if (osdElementEnabled(OSD_CRAFT_NAME, false) && strlen(systemConfig()->craftName) > 0) { osdFormatCraftName(craftNameBuf); if (strlen(buf2) > 0) { - strcat(buf2, ": "); - strcat(buf2, craftNameBuf); - } else - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(craftNameBuf)) / 2, armScreenRow++, craftNameBuf ); + strcat(buf2, " : "); + } + showPilotOrCraftName = true; } - if (strlen(buf2) > 0) { - displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf2)) / 2, armScreenRow++, buf2 ); + if (showPilotOrCraftName) { + tfp_sprintf(buf, "%s%s", buf2, craftNameBuf); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf ); + memset(buf, '\0', sizeof(buf)); memset(buf2, '\0', sizeof(buf2)); armScreenRow++; } @@ -5395,9 +5408,8 @@ static void osdShowSDArmScreen(void) osdFormatDistanceStr(buf2, posControl.safehomeState.distance); tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2); } - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; // write this message below the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr); + displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf); memset(buf, '\0', sizeof(buf)); memset(buf2, '\0', sizeof(buf2)); } From cc5917558daaa3ba2544105b195dd81c8d4b753a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Aug 2024 13:41:28 +0100 Subject: [PATCH 056/175] remove gravity calibration changes --- src/main/navigation/navigation_pos_estimator.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index e11342ff38c..84c919c8a8b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -345,11 +345,6 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } -static bool gravityCalibrationIsValid(void) -{ - return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS; -} - #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -417,13 +412,12 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) { - if (!ARMING_FLAG(WAS_EVER_ARMED)) { + if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); if (gravityCalibrationComplete()) { zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); setGravityCalibration(posEstimator.imu.calibratedGravityCMSS); - restartGravityCalibration(); // Repeat calibration until first arm to continuously update zero datum LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); } } @@ -432,7 +426,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - if (gravityCalibrationIsValid()) { + if (gravityCalibrationComplete()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS; @@ -929,5 +923,5 @@ void updatePositionEstimator(void) bool navIsCalibrationComplete(void) { - return gravityCalibrationIsValid(); + return gravityCalibrationComplete(); } From 954eb3660d484ce88cf4b4678e19f00411a0aee5 Mon Sep 17 00:00:00 2001 From: trailx <36086061+trailx@users.noreply.github.com> Date: Tue, 3 Sep 2024 09:03:25 -0400 Subject: [PATCH 057/175] Added documentation about GVARs, PIDFF Controllers, & Integer Math - Added information about integer math - Added divide-by-0 information - Added numerical limits of GVARs - Added helpful information about PIDFF controllers --- docs/Programming Framework.md | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index b2ce6fe3b93..7c2953d7165 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -24,6 +24,8 @@ INAV Programming Framework consists of: IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled "Programming". The various options shown in Configurator are described below. +**Note:** IPF uses non-floating-point math, so it only can return integers. If your programming line returns a decimal, it will be reduced an integer. So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`. + ## Logic Conditions ### CLI @@ -61,9 +63,8 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result | | 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result | | 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result | -| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result | -| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by -`Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | +| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result. NOTE: If `Operand B` = `0`, the `Divide` operation will simply return `Operand A`| +| 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | | 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | | 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | | 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | @@ -233,8 +234,15 @@ All flags are reseted on ARM and DISARM event. `gvar ` +**Note:** Global Variables (GVARs) are limited to integers between negative `-32768` and positive `32767`. + ## Programming PID +IPF makes a set of general user PIDFF controllers avaliable for use in your program. These PIDFF controllers are not tied to any roll/pitch/yaw profiles or other controls. +The output of these controllers can be used in an IPF program by using the `Programming PID` operand. +The `` of the controller is the target value for the controller to hit. The `` is the measurement of the current value. For instance, `` could be the speed you want to go, and `` is the current speed. +P, I, D, and FF values will need to be manually adjusted to determine the appropriate value for the program and controller. + `pid

` * `` - ID of PID Controller, starting from `0` From ef2d9c1084bd7883ea9522aafb63bf9648248c7c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Sep 2024 13:54:08 +0100 Subject: [PATCH 058/175] add baro v setting, clean up config calls --- docs/Settings.md | 10 ++++ src/main/fc/settings.yaml | 6 +++ src/main/navigation/navigation.h | 1 + .../navigation/navigation_pos_estimator.c | 54 +++++++++++-------- 4 files changed, 48 insertions(+), 23 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9afdd789685..c2b3247ff95 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2122,6 +2122,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i --- +### inav_w_z_baro_v + +Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. + +| Default | Min | Max | +| --- | --- | --- | +| 0.1 | 0 | 10 | + +--- + ### inav_w_z_gps_p Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 28279aff1e0..a1f1ef7ff74 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2410,6 +2410,12 @@ groups: min: 0 max: 10 default_value: 0.35 + - name: inav_w_z_baro_v + description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." + field: w_z_baro_v + min: 0 + max: 10 + default_value: 0.1 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index da65f0f3f57..24b8daa0402 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,6 +239,7 @@ typedef struct positionEstimationConfig_s { uint16_t max_surface_altitude; float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 84c919c8a8b..5bcfc1e11eb 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, + .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, @@ -478,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + if ((sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && - (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { - if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { + (posEstimator.gps.eph < max_eph_epv)) { + if (posEstimator.gps.epv < max_eph_epv) { newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; } else { @@ -505,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) newFlags |= EST_FLOW_VALID; } - if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.eph < max_eph_epv) { newFlags |= EST_XY_VALID; } - if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.epv < max_eph_epv) { newFlags |= EST_Z_VALID; } @@ -602,16 +605,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) // Altitude const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt; + ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -628,15 +632,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -716,21 +721,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) { estimationContext_t ctx; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + /* Calculate dT */ ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); posEstimator.est.lastUpdateTime = currentTimeUs; /* If IMU is not ready we can't estimate anything */ if (!isImuReady()) { - posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; - posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.est.eph = max_eph_epv + 0.001f; + posEstimator.est.epv = max_eph_epv + 0.001f; posEstimator.flags = 0; return; } /* Calculate new EPH and EPV for the case we didn't update postion */ - ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); - ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -753,12 +760,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero - if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) { + if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; } - if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) { + if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; } // Boost the corrections based on accWeight @@ -770,16 +777,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); /* Correct accelerometer bias */ - if (positionEstimationConfig()->w_acc_bias > 0.0f) { + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { /* transform error vector from NEU frame to body frame */ imuTransformVectorEarthToBody(&ctx.accBiasCorr); /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; } } From d0b5dbee4ed7a832bde6d18bf54ff9d53e9f18ff Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Sep 2024 09:41:22 +0100 Subject: [PATCH 059/175] add new settings --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_pos_estimator.c | 8 ++++---- src/main/navigation/navigation_pos_estimator_private.h | 2 ++ 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c2b3247ff95..817a6e2da28 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1994,7 +1994,7 @@ Uncertainty value for barometric sensor [cm] ### inav_default_alt_sensor -Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. +Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a1f1ef7ff74..486fc54b8c1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -199,7 +199,7 @@ tables: values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e - name: default_altitude_source - values: ["GPS", "BARO"] + values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] enum: navDefaultAltitudeSensor_e constants: @@ -2471,7 +2471,7 @@ groups: min: 0 max: 9999 - name: inav_default_alt_sensor - description: "Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv." + description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use." default_value: "GPS" field: default_alt_sensor table: default_altitude_source diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5bcfc1e11eb..8332d8a70aa 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -559,11 +559,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; - float wBaro = 1.0f; - float wGps = 1.0f; + const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f; + float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f; - if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) { - const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) { const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); // Fade out the non default sensor to prevent sudden jump diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 46fcb42e177..5cbdc81c030 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -149,6 +149,8 @@ typedef enum { typedef enum { ALTITUDE_SOURCE_GPS, ALTITUDE_SOURCE_BARO, + ALTITUDE_SOURCE_GPS_ONLY, + ALTITUDE_SOURCE_BARO_ONLY, } navDefaultAltitudeSensor_e; typedef struct { From 4f02dc0b50d33d9dc286d16bc64f31627546efc9 Mon Sep 17 00:00:00 2001 From: trailx <36086061+trailx@users.noreply.github.com> Date: Tue, 10 Sep 2024 08:24:13 -0400 Subject: [PATCH 060/175] Update Programming Framework.md Updated integer math comment based on feedback --- docs/Programming Framework.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 7c2953d7165..c2314dc627e 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -24,7 +24,7 @@ INAV Programming Framework consists of: IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled "Programming". The various options shown in Configurator are described below. -**Note:** IPF uses non-floating-point math, so it only can return integers. If your programming line returns a decimal, it will be reduced an integer. So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`. +**Note:** IPF uses integer math. If your programming line returns a decimal, it will be truncated to an integer. So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`. ## Logic Conditions From a04491f0e23ae565d4608d272359fd2bdec59811 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 10 Sep 2024 18:01:12 +0100 Subject: [PATCH 061/175] Rewrite MSP VTX support for proper integration. --- src/main/fc/fc_msp.c | 16 +- src/main/fc/fc_tasks.c | 3 - src/main/io/vtx_msp.c | 722 +++++++------------------ src/main/io/vtx_msp.h | 36 +- src/main/programming/logic_condition.c | 15 +- 5 files changed, 220 insertions(+), 572 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e7a7a4aa3a6..7993dae6eef 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2686,10 +2686,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) > 1) { uint8_t newPower = sbufReadU8(src); - uint8_t currentPower = 0; - vtxCommonGetPowerIndex(vtxDevice, ¤tPower); - if (newPower != currentPower) { - vtxCommonSetPowerByIndex(vtxDevice, newPower); + if (vtxSettingsConfig()->power != newPower) { vtxSettingsConfigMutable()->power = newPower; } @@ -2715,9 +2712,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) // API version 1.42 - extensions for non-encoded versions of the band, channel or frequency if (sbufBytesRemaining(src) >= 4) { uint8_t newBand = sbufReadU8(src); + if (vtxSettingsConfig()->band != newBand) { + vtxSettingsConfigMutable()->band = newBand; + } + const uint8_t newChannel = sbufReadU8(src); - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; + if (vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->channel = newChannel; + } } /* if (sbufBytesRemaining(src) >= 4) { @@ -3684,7 +3686,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) while (bytesCount < 80) //whole response should be less 155 bytes at worst. { bool blink1; - uint16_t lastChar; + uint16_t lastChar = 0; count = 0; while ( true ) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..fbe119c272b 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -120,9 +120,6 @@ void taskHandleSerial(timeUs_t currentTimeUs) #ifdef USE_MSP_OSD // Capture MSP Displayport messages to determine if VTX is connected mspOsdSerialProcess(mspFcProcessCommand); -#ifdef USE_VTX_MSP - mspVtxSerialProcess(mspFcProcessCommand); -#endif #endif } diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 696918e5705..13a3247eb14 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -17,157 +17,115 @@ * * If not, see . */ +/* Created by geoffsim */ -/* Created by phobos- */ - -#include #include -#include -#include -#include #include +#include #include "platform.h" #if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) -#include "build/debug.h" - -//#include "cms/cms_menu_vtx_msp.h" -#include "common/crc.h" #include "common/log.h" -#include "config/feature.h" - +#include "common/crc.h" #include "drivers/vtx_common.h" -//#include "drivers/vtx_table.h" - -#include "fc/runtime_config.h" -#include "flight/failsafe.h" - -#include "io/serial.h" -#include "io/vtx_msp.h" -#include "io/vtx_control.h" -#include "io/vtx_string.h" -#include "io/vtx_smartaudio.h" -#include "io/vtx.h" -#include "io/displayport_msp_osd.h" - #include "msp/msp_protocol.h" -#include "msp/msp_serial.h" -#include "msp/msp.h" - -//#include "pg/vtx_table.h" -#include "fc/settings.h" - -#include "rx/crsf.h" -//#include "rx/crsf_protocol.h" #include "rx/rx.h" - -#include "telemetry/msp_shared.h" - -//static uint16_t mspConfFreq = 0; -static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT; -static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT; -//static uint16_t mspConfPower = 0; -static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT; -static uint8_t mspConfPitMode = 0; -static bool mspVtxConfigChanged = false; -static timeUs_t mspVtxLastTimeUs = 0; -static bool prevLowPowerDisarmedState = false; - -static const vtxVTable_t mspVTable; // forward -static vtxDevice_t vtxMsp = { - .vTable = &mspVTable, - .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS, - .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS, - .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS, - .capability.bandNames = (char **)vtx58BandNames, - .capability.channelNames = (char **)vtx58ChannelNames, - .capability.powerNames = (char**)saPowerNames - +#include "rx/crsf.h" +#include "telemetry/crsf.h" +#include "vtx.h" +#include "displayport_msp_osd.h" +#include "vtx_string.h" +#include "vtx_msp.h" + +#define VTX_MSP_MIN_BAND (1) +#define VTX_MSP_MAX_BAND (VTX_MSP_MIN_BAND + VTX_MSP_BAND_COUNT - 1) +#define VTX_MSP_MIN_CHANNEL (1) +#define VTX_MSP_MAX_CHANNEL (VTX_MSP_MIN_CHANNEL + VTX_MSP_CHANNEL_COUNT -1) + +#define VTX_UPDATE_REQ_NONE 0x00 +#define VTX_UPDATE_REQ_FREQUENCY 0x01 +#define VTX_UPDATE_REQ_POWER 0x02 +#define VTX_UPDATE_REQ_PIT_MODE 0x04 + +typedef struct { + bool ready; + uint8_t timeouts; + uint8_t updateReqMask; + bool crsfTelemetryEnabled; + + struct { + uint8_t band; + uint8_t channel; + uint16_t freq; + uint8_t power; + uint8_t powerIndex; + uint8_t pitMode; + } request; +; +} vtxProtoState_t; + +const char * const vtxMspBandNames[VTX_MSP_BAND_COUNT + 1] = { + "-----", "A 2.4", "B 2.4", "E 2.4", "F 2.4", "R 2.4" }; -STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE; -static uint8_t mspVtxPortIdentifier = 255; +const char * vtxMspBandLetters = "-ABEFR"; -#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms - -static bool isCrsfPortConfig(const serialPortConfig_t *portConfig) -{ - return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF; -} - -static bool isLowPowerDisarmed(void) -{ - return (!ARMING_FLAG(ARMED) && !failsafeIsActive() && - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS || - (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED)))); -} - -bool isVtxConfigValid(const vtxConfig_t *cfg) -{ - LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n"); - for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) { - - if (cfg->vtxChannelActivationConditions[i].band || - (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) || - cfg->vtxChannelActivationConditions[i].auxChannelIndex || - cfg->vtxChannelActivationConditions[i].channel) { - return true; - } - } - - LOG_DEBUG(VTX, "msp Invalid Config!\r\n"); - return false; -} +const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = { + "-", "1", "2", "3", "4", "5", "6", "7", "8" +}; +const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = { + "---", "25", "200", "500", "MAX" +}; -void setMspVtxDeviceStatusReady(const int descriptor) -{ - LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n"); - UNUSED(descriptor); +const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT] = { + 25, 200, 500, 1000 +}; - mspVtxStatus = MSP_VTX_STATUS_READY; - mspVtxConfigChanged = true; -} +static serialPortIdentifier_e mspVtxPortIdentifier; +static vtxProtoState_t vtxState; +static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *); +static bool vtxMspIsReady(const vtxDevice_t *); -void prepareMspFrame(uint8_t *mspFrame) +static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { - LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n"); -/* -HDZERO parsing - fc_band_rx = msp_rx_buf[1]; - fc_channel_rx = msp_rx_buf[2]; - fc_pwr_rx = msp_rx_buf[3]; - fc_pit_rx = msp_rx_buf[4]; - fc_lp_rx = msp_rx_buf[8]; -*/ - - uint8_t pitmode = 0; - vtxCommonGetPitMode(&vtxMsp, &pitmode); - - mspFrame[0] = VTXDEV_MSP, - mspFrame[1] = vtxSettingsConfig()->band; - mspFrame[2] = vtxSettingsConfig()->channel; - mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - mspFrame[4] = pitmode; - mspFrame[5] = 0; // Freq_L - mspFrame[6] = 0; // Freq_H - mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0; - mspFrame[8] = isLowPowerDisarmed(); - mspFrame[9] = 0; // Pitmode freq Low - mspFrame[10] = 0; // pitmod freq High + LOG_DEBUG(VTX, "msp prepareMspFrame\r\n"); + //UNUSED(vtxDevice); + + // Send an MSP_VTX_V2 frame to the VTX + + mspFrame[0] = vtxMspGetDeviceType(vtxDevice); + mspFrame[1] = vtxState.request.band; + mspFrame[2] = vtxState.request.channel; + mspFrame[3] = vtxState.request.powerIndex; + mspFrame[4] = vtxState.request.pitMode; + mspFrame[5] = 0; // Freq_L + mspFrame[6] = 0; // Freq_H + mspFrame[7] = vtxMspIsReady(vtxDevice); + mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm; + mspFrame[9] = 0; // pitmode freq Low + mspFrame[10] = 0; // pitmode freq High mspFrame[11] = 0; // 1 if using vtx table - mspFrame[12] = 0; // vtx table bands or 0 - mspFrame[13] = 0; // vtx table channels or 0 - mspFrame[14] = 0; // vtx table power levels or 0 -} + mspFrame[12] = 6; // bands or 0 + mspFrame[13] = 8; // channels or 0 + mspFrame[14] = 5; // power levels or 0 + + LOG_DEBUG(VTX, "msp device [%d]\r\n", mspFrame[0]); + LOG_DEBUG(VTX, "msp band [%d]\r\n", mspFrame[1]); + LOG_DEBUG(VTX, "msp channel [%d]\r\n", mspFrame[2]); + LOG_DEBUG(VTX, "msp power [%d]\r\n", mspFrame[3]); + LOG_DEBUG(VTX, "msp freq [%d]\r\n", ((uint16_t)mspFrame[6] << 8) + mspFrame[5]); + LOG_DEBUG(VTX, "msp pitmode [%d]\r\n", mspFrame[4]); + LOG_DEBUG(VTX, "msp isready [%d]\r\n", mspFrame[7]); + LOG_DEBUG(VTX, "msp lowPower [%d]\r\n", mspFrame[8]); +} static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) { - - LOG_DEBUG(VTX, "msp CrsfPush\r\n"); + LOG_DEBUG(VTX, "msp mspCrsfPush\r\n"); #ifndef USE_TELEMETRY_CRSF UNUSED(mspCommand); UNUSED(mspFrame); @@ -201,285 +159,157 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length sbufSwitchToReader(dst, crsfFrame); - crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry + crsfRxSendTelemetryData(); // give the FC a chance to send outstanding telemetry crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); crsfRxSendTelemetryData(); #endif } -static uint16_t packetCounter = 0; - -static bool isVtxConfigChanged(void) -{ - if(mspVtxStatus == MSP_VTX_STATUS_READY) { - if (mspVtxConfigChanged == true) - return true; - - if (isLowPowerDisarmed() != prevLowPowerDisarmedState) { - LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n"); - mspVtxConfigChanged = true; - prevLowPowerDisarmedState = isLowPowerDisarmed(); - } - - if (mspConfPowerIndex != vtxSettingsConfig()->power) { - LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n"); - mspVtxConfigChanged = true; - } - - if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n"); - mspVtxConfigChanged = true; - } - - return mspVtxConfigChanged; - } - - return false; -} - static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { + LOG_DEBUG(VTX, "msp vtxMspProcess\r\n"); UNUSED(vtxDevice); - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - uint8_t frame[15]; + uint8_t mspFrame[15]; - switch (mspVtxStatus) { - case MSP_VTX_STATUS_OFFLINE: - LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n"); - // wait for MSP communication from the VTX -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif - break; - case MSP_VTX_STATUS_READY: - LOG_DEBUG(VTX, "msp MspProcess: READY\r\n"); - // send an update if stuff has changed with 200ms period - if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) { - - LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n"); - prepareMspFrame(frame); - - if (isCrsfPortConfig(portConfig)) { - mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame)); - } else { - mspPort_t *port = getMspOsdPort(); - if(port != NULL && port->port) { - LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n"); - int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE); - if (sent <= 0) { - break; - } - } + mspPort_t *mspPort = getMspOsdPort(); + unsigned lastActivity = (currentTimeUs/1000) - mspPort->lastActivityMs; + if (lastActivity > VTX_MSP_TIMEOUT) { + if (vtxState.timeouts++ > 3) { + if (vtxState.ready) { + vtxState.ready = false; } - packetCounter++; - mspVtxLastTimeUs = currentTimeUs; - mspVtxConfigChanged = false; - -#ifdef USE_CMS - //mspCmsUpdateStatusString(); -#endif } - break; - default: - mspVtxStatus = MSP_VTX_STATUS_OFFLINE; - break; + } else { // active + if (!vtxState.ready) { + vtxState.ready = true; + } } -#if 0 - DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter); - DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig)); - DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed()); -#if defined(USE_MSP_OVER_TELEMETRY) - DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#else - DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier)); -#endif -#endif + if (vtxState.ready) { + if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) { + prepareMspFrame(vtxDevice, mspFrame); + if (vtxState.crsfTelemetryEnabled) { // keep ELRS LUA up to date ? + mspCrsfPush(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame)); + } + + int sent = mspSerialPushPort(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame), mspPort, MSP_V2_NATIVE); + if (sent > 0) { + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + } + } + } } static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp GetDeviceType\r\n"); + LOG_DEBUG(VTX, "msp vtxMspGetDeviceType\r\n"); UNUSED(vtxDevice); + return VTXDEV_MSP; } static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) { - //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N"); - return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY; + LOG_DEBUG(VTX, "msp vtxMspIsReady\r\n"); + return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready; } static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetBandAndChannel\r\n"); UNUSED(vtxDevice); - if (band != mspConfBand || channel != mspConfChannel) { - LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n"); - mspVtxConfigChanged = true; + + if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) { + return; } - mspConfBand = band; - mspConfChannel = channel; + + vtxState.request.band = band; + vtxState.request.channel = channel; + vtxState.request.freq = vtx58_Bandchan2Freq(band, channel); + vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY; } static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetPowerByIndex\r\n"); UNUSED(vtxDevice); - if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS)) - { - if (index != mspConfPowerIndex) - { - LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPowerIndex = index; - } + vtxState.request.power = vtxMspPowerTable[index - 1]; + vtxState.request.powerIndex = index; + vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; } -static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff) +static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { - LOG_DEBUG(VTX, "msp SetPitMode\r\n"); + LOG_DEBUG(VTX, "msp vtxMspSetPitMode\r\n"); UNUSED(vtxDevice); - if (onoff != mspConfPitMode) { - LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n"); - mspVtxConfigChanged = true; - } - mspConfPitMode = onoff; -} -#if 0 -static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq) -{ - UNUSED(vtxDevice); - if (freq != mspConfFreq) { - mspVtxConfigChanged = true; - } - mspConfFreq = freq; + vtxState.request.pitMode = onOff; + vtxState.updateReqMask |= VTX_UPDATE_REQ_PIT_MODE; } -#endif - - - static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } - - *pBand = vtxSettingsConfig()->band; - *pChannel = vtxSettingsConfig()->channel; - - //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel); + LOG_DEBUG(VTX, "msp vtxMspGetBandAndChannel\r\n"); + UNUSED(vtxDevice); + *pBand = vtxState.request.band; + *pChannel = vtxState.request.channel; return true; } static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - if (!vtxMspIsReady(vtxDevice)) { - return false; - } - - uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; - // Special case, power not set - if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) { - *pIndex = 0; - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); - return true; - } - - *pIndex = power; + LOG_DEBUG(VTX, "msp vtxMspGetPowerIndex\r\n"); + UNUSED(vtxDevice); - //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex); + *pIndex = vtxState.request.powerIndex; return true; } -static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) +static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - LOG_DEBUG(VTX, "msp GetFreq\r\n"); - if (!vtxMspIsReady(vtxDevice)) { - return false; - } + LOG_DEBUG(VTX, "msp vtxMspGetPitMode\r\n"); + UNUSED(vtxDevice); - *pFreq = 5800; + *pOnOff = vtxState.request.pitMode; return true; } -static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) +static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - LOG_DEBUG(VTX, "msp GetPower\r\n"); - uint8_t powerIndex; - - if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) { - return false; - } - + LOG_DEBUG(VTX, "msp vtxMspGetFreq\r\n"); + UNUSED(vtxDevice); - *pIndex = powerIndex; - *pPowerMw = *pIndex; + *pFreq = vtxState.request.freq; return true; } -static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo) +static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) { - LOG_DEBUG(VTX, "msp GetOsdInfo\r\n"); - uint8_t powerIndex; - uint16_t powerMw; - uint16_t freq; - uint8_t band, channel; - - if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) { - return false; - } - - if (!vtxMspGetFreq(vtxDevice, &freq)) { - return false; - } - - if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) { - return false; - } + LOG_DEBUG(VTX, "msp vtxMspGetPower\r\n"); + UNUSED(vtxDevice); - pOsdInfo->band = band; - pOsdInfo->channel = channel; - pOsdInfo->frequency = freq; - pOsdInfo->powerIndex = powerIndex; - pOsdInfo->powerMilliwatt = powerMw; - pOsdInfo->bandLetter = vtx58BandNames[band][0]; - pOsdInfo->bandName = vtx58BandNames[band]; - pOsdInfo->channelName = vtx58ChannelNames[channel]; - pOsdInfo->powerIndexLetter = '0' + powerIndex; + *pIndex = vtxState.request.powerIndex; + *pPowerMw = vtxState.request.power; return true; } - -bool vtxMspInit(void) +static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo) { - LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); - // don't bother setting up this device if we don't have MSP vtx enabled - // Port is shared with MSP_OSD - const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD); - if (!portConfig) { - return false; - } - - mspVtxPortIdentifier = portConfig->identifier; - - // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called. -#if defined(USE_VTX_COMMON) - vtxCommonSetDevice(&vtxMsp); -#endif - - mspConfBand = vtxSettingsConfig()->band; - mspConfChannel = vtxSettingsConfig()->channel; - mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based - vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode); + LOG_DEBUG(VTX, "msp vtxMspGetOsdInfo\r\n"); + UNUSED(vtxDevice); - vtxInit(); + pOsdInfo->band = vtxState.request.band; + pOsdInfo->channel = vtxState.request.channel; + pOsdInfo->frequency = vtxState.request.freq; + pOsdInfo->powerIndex = vtxState.request.powerIndex; + pOsdInfo->powerMilliwatt = vtxState.request.power; + pOsdInfo->bandName = vtxMspBandNames[vtxState.request.band]; + pOsdInfo->bandLetter = vtxMspBandLetters[vtxState.request.band]; + pOsdInfo->channelName = vtxMspChannelNames[vtxState.request.channel]; + pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex; return true; } @@ -491,211 +321,51 @@ static const vtxVTable_t mspVTable = { .setBandAndChannel = vtxMspSetBandAndChannel, .setPowerByIndex = vtxMspSetPowerByIndex, .setPitMode = vtxMspSetPitMode, - //.setFrequency = vtxMspSetFreq, .getBandAndChannel = vtxMspGetBandAndChannel, .getPowerIndex = vtxMspGetPowerIndex, + .getPitMode = vtxMspGetPitMode, .getFrequency = vtxMspGetFreq, - //.getStatus = vtxMspGetStatus, .getPower = vtxMspGetPower, - //.serializeCustomDeviceStatus = NULL, - .getOsdInfo = vtxMspGetOsdInfo, + .getOsdInfo = vtxMspGetOsdInfo }; -static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) -{ - //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n"); - UNUSED(mspPostProcessFn); - - sbuf_t *dst = &reply->buf; - sbuf_t *src = &cmd->buf; - - const unsigned int dataSize = sbufBytesRemaining(src); - UNUSED(dst); - UNUSED(src); - - // Start initializing the reply message - reply->cmd = cmd->cmd; - reply->result = MSP_RESULT_ACK; - - vtxDevice_t *vtxDevice = vtxCommonDevice(); - if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) { - LOG_DEBUG(VTX, "msp wrong vtx\r\n"); - return MSP_RESULT_ERROR; - } - - switch (cmd->cmd) - { - case MSP_VTXTABLE_BAND: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n"); - uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice); - if (deviceType == VTXDEV_MSP) - { - /* - char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1]; - memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1); - uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS]; - const uint8_t band = sbufReadU8(src); - const uint8_t bandNameLength = sbufReadU8(src); - for (int i = 0; i < bandNameLength; i++) { - const char nameChar = sbufReadU8(src); - if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) { - bandName[i] = toupper(nameChar); - } - } - const char bandLetter = toupper(sbufReadU8(src)); - const bool isFactoryBand = (bool)sbufReadU8(src); - const uint8_t channelCount = sbufReadU8(src); - for (int i = 0; i < channelCount; i++) - { - const uint16_t frequency = sbufReadU16(src); - if (i < vtxTableConfig()->channels) - { - frequencies[i] = frequency; - } - } - */ - - setMspVtxDeviceStatusReady(1); - } - break; - } - case MSP_VTXTABLE_POWERLEVEL: - { - LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n"); - - /* - char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1]; - memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1); - const uint8_t powerLevel = sbufReadU8(src); - const uint16_t powerValue = sbufReadU16(src); - const uint8_t powerLevelLabelLength = sbufReadU8(src); - for (int i = 0; i < powerLevelLabelLength; i++) - { - const char labelChar = sbufReadU8(src); - if (i < VTX_TABLE_POWER_LABEL_LENGTH) - { - powerLevelLabel[i] = toupper(labelChar); - } - } - */ - setMspVtxDeviceStatusReady(1); - } - break; - case MSP_VTX_CONFIG: - { - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n"); - LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n"); - uint8_t pitmode = 0; - vtxCommonGetPitMode(vtxDevice, &pitmode); - - // VTXDEV_MSP, - sbufWriteU8(dst, VTXDEV_MSP); - // band; - sbufWriteU8(dst, vtxSettingsConfig()->band); - // channel; - sbufWriteU8(dst, vtxSettingsConfig()->channel); - // power; // index based - sbufWriteU8(dst, vtxSettingsConfig()->power); - // pit mode; - // Freq_L - sbufWriteU8(dst, 0); - // Freq_H - sbufWriteU8(dst, 0); - // vtx status - sbufWriteU8(dst, 1); - // lowPowerDisarm - - sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm); - // Pitmode freq Low - sbufWriteU8(dst, 0); - // pitmod freq High - sbufWriteU8(dst, 0); - // 1 if using vtx table - sbufWriteU8(dst, 0); - // vtx table bands or 0 - sbufWriteU8(dst, 0); - // vtx table channels or 0 - sbufWriteU8(dst, 0); - - setMspVtxDeviceStatusReady(1); - break; - } - case MSP_SET_VTX_CONFIG: - LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n"); - if (dataSize == 15) - { - if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN) - { - for (int i = 0; i < 15; ++i) - { - uint8_t data = sbufReadU8(src); - switch (i) - { - case 1: - vtxSettingsConfigMutable()->band = data; - break; - case 2: - vtxSettingsConfigMutable()->channel = data; - break; - case 3: - vtxSettingsConfigMutable()->power = data; - break; - case 4: - vtxCommonSetPitMode(vtxDevice, data); - break; - case 7: - // vtx ready - break; - case 8: - vtxSettingsConfigMutable()->lowPowerDisarm = data; - break; - } - } - } - - setMspVtxDeviceStatusReady(1); - break; - } - LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n"); - return MSP_RESULT_ERROR; - default: - // debug[1]++; - // debug[2] = cmd->cmd; - if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) { - LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd); - } - reply->result = MSP_RESULT_ERROR; - break; - } - - // Process DONT_REPLY flag - if (cmd->flags & MSP_FLAG_DONT_REPLY) - { - reply->result = MSP_RESULT_NO_REPLY; - } - - return reply->result; -} +static vtxDevice_t vtxMsp = { + .vTable = &mspVTable, + .capability.bandCount = VTX_MSP_MAX_BAND, + .capability.channelCount = VTX_MSP_MAX_CHANNEL, + .capability.powerCount = VTX_MSP_POWER_COUNT, + .capability.bandNames = (char **)vtxMspBandNames, + .capability.channelNames = (char **)vtxMspChannelNames, + .capability.powerNames = (char **)vtxMspPowerNames +}; -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) +bool vtxMspInit(void) { - UNUSED(mspProcessCommandFn); - // Check if VTX is ready - /* - if (mspVtxStatus != MSP_VTX_STATUS_READY) { - LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n"); - return; + LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); + const serialPortConfig_t *portConfig; + + // Shares MSP_OSD port + portConfig = findSerialPortConfig(FUNCTION_VTX_MSP); + if (!portConfig) { + return false; } - */ - mspPort_t *port = getMspOsdPort(); + portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); - if(port) { - mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand); - } + vtxState.ready = false; + vtxState.timeouts = 0; + vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; + vtxState.crsfTelemetryEnabled = crsfRxIsActive() && checkCrsfTelemetryState(); + vtxState.request.band = vtxSettingsConfig()->band; + vtxState.request.channel = vtxSettingsConfig()->channel; + vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); + vtxState.request.power = vtxSettingsConfig()->power; + uint8_t pitmode = 0; + vtxCommonGetPitMode(&vtxMsp, &pitmode); + vtxState.request.pitMode = pitmode; + vtxCommonSetDevice(&vtxMsp); + return true; } - #endif \ No newline at end of file diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h index 30ca245fed3..ceb2d5bea17 100644 --- a/src/main/io/vtx_msp.h +++ b/src/main/io/vtx_msp.h @@ -17,37 +17,17 @@ * * If not, see . */ +/* Created by geoffsim */ -#pragma once +#ifndef _VTX_MSP_H +#define _VTX_MSP_H -#include - -#include "build/build_config.h" - -#include "msp/msp_protocol.h" -#include "msp/msp_serial.h" - -typedef enum { - // Offline - device hasn't responded yet - MSP_VTX_STATUS_OFFLINE = 0, - MSP_VTX_STATUS_READY, -} mspVtxStatus_e; - -typedef struct mspPowerTable_s { - int mW; // rfpower - int16_t dbi; // valueV1 -} mspPowerTable_t; - -#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands -#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels -#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT -#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1 -#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8 -#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3 +#define VTX_MSP_TIMEOUT 250 // ms +#define VTX_MSP_BAND_COUNT 5 +#define VTX_MSP_CHANNEL_COUNT 8 +#define VTX_MSP_POWER_COUNT 4 bool vtxMspInit(void); -void setMspVtxDeviceStatusReady(const int descriptor); -void prepareMspFrame(uint8_t *mspFrame); -void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); +#endif // _VTX_MSP_H diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 594db21417c..b078e25f751 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -296,14 +296,13 @@ static int logicConditionCompute( #endif case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: #if defined(USE_VTX_CONTROL) -#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)) - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); - vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) + uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if ( newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_MAX_POWER); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; + vtxSettingsConfigMutable()->power = newpower; + return newpower; } else { return false; } From 387d51e1dfbb6dfb0f8252d78f384face876ff80 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 11 Sep 2024 20:50:58 +0100 Subject: [PATCH 062/175] Added switch indicators back in - Added switch indicators back in - Updated OSD document --- docs/OSD.md | 13 +++++- docs/Settings.md | 90 +++++++++++++++++++++++++++++++++++++++ src/main/fc/settings.yaml | 53 +++++++++++++++++++++++ src/main/io/osd.c | 63 +++++++++++++++++++++++++++ src/main/io/osd.h | 25 ++++++++--- 5 files changed, 238 insertions(+), 6 deletions(-) diff --git a/docs/OSD.md b/docs/OSD.md index 5787496687a..b492e537cef 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -177,7 +177,18 @@ Here are the OSD Elements provided by INAV. | 144 | OSD_MULTI_FUNCTION | 7.0.0 | | | 145 | OSD_ODOMETER | 7.0.0 | For this to work correctly, stats must be enabled (`set stats=ON`). Otherwise, this will show the total flight distance. | | 146 | OSD_PILOT_LOGO | 7.0.0 | | -| 147 | OSD_BLACKBOX | 8.0.0 | The element will be hidden unless blackbox recording is attempted. | +| 147 | OSD_CUSTOM_ELEMENT_1 | 7.0.0 | | +| 148 | OSD_CUSTOM_ELEMENT_2 | 7.0.0 | | +| 149 | OSD_CUSTOM_ELEMENT_3 | 7.0.0 | | +| 150 | OSD_ADSB_WARNING | 7.0.0 | | +| 151 | OSD_ADSB_INFO | 7.0.0 | | +| 152 | OSD_BLACKBOX | 8.0.0 | The element will be hidden unless blackbox recording is attempted. | +| 153 | OSD_FORMATION_FLIGHT | 8.0.0 | | +| 154 | OSD_CUSTOM_ELEMENT_4 | 8.0.0 | | +| 155 | OSD_CUSTOM_ELEMENT_5 | 8.0.0 | | +| 156 | OSD_CUSTOM_ELEMENT_6 | 8.0.0 | | +| 157 | OSD_CUSTOM_ELEMENT_7 | 8.0.0 | | +| 158 | OSD_CUSTOM_ELEMENT_8 | 8.0.0 | | # Pilot Logos diff --git a/docs/Settings.md b/docs/Settings.md index 1e7bf47381c..6487b44b6d1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5152,6 +5152,96 @@ Enabling this option will show metric efficiency statistics on the post flight s --- +### osd_switch_indicator_one_channel + +RC Channel to use for OSD switch indicator 1. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_one_name + +Character to use for OSD switch incicator 1. + +| Default | Min | Max | +| --- | --- | --- | +| GEAR | | 5 | + +--- + +### osd_switch_indicator_three_channel + +RC Channel to use for OSD switch indicator 3. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_three_name + +Character to use for OSD switch incicator 3. + +| Default | Min | Max | +| --- | --- | --- | +| LIGT | | 5 | + +--- + +### osd_switch_indicator_two_channel + +RC Channel to use for OSD switch indicator 2. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_two_name + +Character to use for OSD switch incicator 2. + +| Default | Min | Max | +| --- | --- | --- | +| CAM | | 5 | + +--- + +### osd_switch_indicator_zero_channel + +RC Channel to use for OSD switch indicator 0. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT | + +--- + +### osd_switch_indicator_zero_name + +Character to use for OSD switch incicator 0. + +| Default | Min | Max | +| --- | --- | --- | +| FLAP | | 5 | + +--- + +### osd_switch_indicators_align_left + +Align text to left of switch indicators + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + ### osd_system_msg_display_time System message display cycle time for multiple messages (milliseconds). diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1bd1400d0a6..00119727f34 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3717,6 +3717,59 @@ groups: field: highlight_djis_missing_characters default_value: ON type: bool + - name: osd_switch_indicator_zero_name + description: "Character to use for OSD switch incicator 0." + field: osd_switch_indicator0_name + type: string + max: 5 + default_value: "FLAP" + - name: osd_switch_indicator_one_name + description: "Character to use for OSD switch incicator 1." + field: osd_switch_indicator1_name + type: string + max: 5 + default_value: "GEAR" + - name: osd_switch_indicator_two_name + description: "Character to use for OSD switch incicator 2." + field: osd_switch_indicator2_name + type: string + max: 5 + default_value: "CAM" + - name: osd_switch_indicator_three_name + description: "Character to use for OSD switch incicator 3." + field: osd_switch_indicator3_name + type: string + max: 5 + default_value: "LIGT" + - name: osd_switch_indicator_zero_channel + description: "RC Channel to use for OSD switch indicator 0." + field: osd_switch_indicator0_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_one_channel + description: "RC Channel to use for OSD switch indicator 1." + field: osd_switch_indicator1_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_two_channel + description: "RC Channel to use for OSD switch indicator 2." + field: osd_switch_indicator2_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicator_three_channel + description: "RC Channel to use for OSD switch indicator 3." + field: osd_switch_indicator3_channel + min: 5 + max: MAX_SUPPORTED_RC_CHANNEL_COUNT + default_value: 5 + - name: osd_switch_indicators_align_left + description: "Align text to left of switch indicators" + field: osd_switch_indicators_align_left + type: bool + default_value: ON - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 77762c6a003..249359d4b6d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1610,6 +1610,40 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex) return geoWaypointIndex - posControl.startWpIndex + 1; } +void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { + int8_t ptr = 0; + + if (osdConfig()->osd_switch_indicators_align_left) { + for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) { + buff[ptr] = swName[ptr]; + } + + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + } else { + if ( rcValue < 1333) { + buff[ptr++] = SYM_SWITCH_INDICATOR_LOW; + } else if ( rcValue > 1666) { + buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH; + } else { + buff[ptr++] = SYM_SWITCH_INDICATOR_MID; + } + + for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) { + buff[ptr] = swName[ptr-1]; + } + + ptr++; + } + + buff[ptr] = '\0'; +} + static bool osdDrawSingleElement(uint8_t item) { uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; @@ -2863,6 +2897,21 @@ static bool osdDrawSingleElement(uint8_t item) break; } #endif + case OSD_SWITCH_INDICATOR_0: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_1: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_2: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff); + break; + + case OSD_SWITCH_INDICATOR_3: + osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff); + break; case OSD_PAN_SERVO_CENTRED: { @@ -3983,6 +4032,15 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, .mAh_precision = SETTING_OSD_MAH_PRECISION_DEFAULT, + .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, + .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, + .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, + .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT, + .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT, + .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT, + .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT, + .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT, + .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, @@ -4157,6 +4215,11 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9); + osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10); + osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7); osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8); #if defined(USE_ESC_SENSOR) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2e5200a0483..0cf93b69162 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -263,10 +263,10 @@ typedef enum { OSD_AIR_MAX_SPEED, OSD_ACTIVE_PROFILE, OSD_MISSION, - OSD_CUSTOM_ELEMENT_4, - OSD_CUSTOM_ELEMENT_5, - OSD_CUSTOM_ELEMENT_6, - OSD_CUSTOM_ELEMENT_7, + OSD_SWITCH_INDICATOR_0, + OSD_SWITCH_INDICATOR_1, + OSD_SWITCH_INDICATOR_2, + OSD_SWITCH_INDICATOR_3, OSD_TPA_TIME_CONSTANT, OSD_FW_LEVEL_TRIM, OSD_GLIDE_TIME_REMAINING, @@ -287,7 +287,11 @@ typedef enum { OSD_ADSB_INFO, OSD_BLACKBOX, OSD_FORMATION_FLIGHT, - OSD_CUSTOM_ELEMENT_8, // 154 + OSD_CUSTOM_ELEMENT_4, + OSD_CUSTOM_ELEMENT_5, + OSD_CUSTOM_ELEMENT_6, + OSD_CUSTOM_ELEMENT_7, + OSD_CUSTOM_ELEMENT_8, // 158 OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -348,6 +352,8 @@ typedef struct osdLayoutsConfig_s { PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig); +#define OSD_SWITCH_INDICATOR_NAME_LENGTH 4 + typedef struct osdConfig_s { // Alarms uint8_t rssi_alarm; // rssi % @@ -445,6 +451,15 @@ typedef struct osdConfig_s { uint16_t system_msg_display_time; // system message display time for multiple messages (ms) uint8_t mAh_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD) + char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0. + uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0. + char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1. + uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1. + char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2. + uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2. + char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3. + uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3. + bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch. bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed From cb41ee70f0eac892f0ff7c8021cbb49ac33f8ab5 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 11 Sep 2024 17:40:49 -0700 Subject: [PATCH 063/175] Update Settings.md to include units for nav_max_terrain_follow_alt Added units to the description for nav_max_terrain_follow_alt --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 6ab03e46334..a20809032ad 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3654,7 +3654,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl ### nav_max_terrain_follow_alt -Max allowed above the ground altitude for terrain following mode +Max allowed above the ground altitude for terrain following mode [cm] | Default | Min | Max | | --- | --- | --- | From 3f332ac5ef98af5ed7eff1fc741af0243ddb8038 Mon Sep 17 00:00:00 2001 From: daijoubu Date: Wed, 11 Sep 2024 18:26:00 -0700 Subject: [PATCH 064/175] Update settings.yaml Keep settings.yaml and .md in sync --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 36646efb2c0..74f525ce5f1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2692,7 +2692,7 @@ groups: - name: nav_max_terrain_follow_alt field: general.max_terrain_follow_altitude default_value: "100" - description: "Max allowed above the ground altitude for terrain following mode" + description: "Max allowed above the ground altitude for terrain following mode [cm]" max: 1000 default_value: 100 - name: nav_max_altitude From fd71bed9a2dbc2b38a4f1bfa066a2d0f57c1b4a2 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 12 Sep 2024 16:52:37 +0100 Subject: [PATCH 065/175] Restrict band/channel/power in logic conditions to the specific device capabilities. Add one to the power to allow HDZERO 0mW option. --- src/main/programming/logic_condition.c | 38 ++++++++++++-------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b078e25f751..f02cdc1fa76 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -294,12 +294,13 @@ static int logicConditionCompute( return true; break; #endif - case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: + #if defined(USE_VTX_CONTROL) -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) + case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if ( newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, VTX_SETTINGS_MAX_POWER); + if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + // HDZERO VTX max power+1 is 0mW. + newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; vtxSettingsConfigMutable()->power = newpower; return newpower; @@ -307,30 +308,25 @@ static int logicConditionCompute( return false; } break; -#else - return false; -#endif case LOGIC_CONDITION_SET_VTX_BAND: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND); - vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newband; + vtxSettingsConfigMutable()->band = newband; + return newband; } else { return false; } break; case LOGIC_CONDITION_SET_VTX_CHANNEL: - if ( - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA && - vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability) - ) { - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL); - vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newchannel = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; + vtxSettingsConfigMutable()->band = newchannel; + return newchannel; } else { return false; } From 77a0f20970c67d502c4f265b139e9190b0ea4079 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 12 Sep 2024 18:27:31 +0100 Subject: [PATCH 066/175] Move crsf include to the correct header. Include uint definitions. --- src/main/programming/logic_condition.c | 1 + src/main/rx/crsf.h | 1 + src/main/telemetry/crsf.h | 1 - 3 files changed, 2 insertions(+), 1 deletion(-) mode change 100755 => 100644 src/main/telemetry/crsf.h diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f02cdc1fa76..bdc008af8a8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -22,6 +22,7 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include #include #include "config/config_reset.h" diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 69777a0390c..da7aa9024e2 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -133,5 +133,6 @@ struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); bool crsfRxIsActive(void); +bool checkCrsfTelemetryState(void); void crsfBind(void); diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h old mode 100755 new mode 100644 index 6ffa4df3588..f738c71bf28 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,7 +24,6 @@ #define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); -bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); void crsfScheduleMspResponse(void); From 05fa7e6dee895dbcf8194f58a9514e58327289ec Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 13 Sep 2024 13:06:38 +0100 Subject: [PATCH 067/175] Fix SITL compile issues. Move telemetry function definition back to correct place. --- src/main/io/vtx_msp.c | 2 +- src/main/programming/logic_condition.c | 12 ++++++++---- src/main/rx/crsf.h | 1 - src/main/telemetry/crsf.h | 1 + 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 13a3247eb14..7868fed6717 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -355,7 +355,7 @@ bool vtxMspInit(void) vtxState.ready = false; vtxState.timeouts = 0; vtxState.updateReqMask = VTX_UPDATE_REQ_NONE; - vtxState.crsfTelemetryEnabled = crsfRxIsActive() && checkCrsfTelemetryState(); + vtxState.crsfTelemetryEnabled = crsfRxIsActive(); vtxState.request.band = vtxSettingsConfig()->band; vtxState.request.channel = vtxSettingsConfig()->channel; vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index bdc008af8a8..98ae76f8710 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -22,7 +22,6 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ -#include #include #include "config/config_reset.h" @@ -298,6 +297,7 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: + { uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { // HDZERO VTX max power+1 is 0mW. @@ -309,8 +309,9 @@ static int logicConditionCompute( return false; } break; - + } case LOGIC_CONDITION_SET_VTX_BAND: + { uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); @@ -321,17 +322,20 @@ static int logicConditionCompute( return false; } break; + } case LOGIC_CONDITION_SET_VTX_CHANNEL: + { uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newchannel = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.channelCount); + newchannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; - vtxSettingsConfigMutable()->band = newchannel; + vtxSettingsConfigMutable()->channel = newchannel; return newchannel; } else { return false; } break; + } #endif case LOGIC_CONDITION_INVERT_ROLL: LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL); diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index da7aa9024e2..69777a0390c 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -133,6 +133,5 @@ struct rxConfig_s; struct rxRuntimeConfig_s; bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); bool crsfRxIsActive(void); -bool checkCrsfTelemetryState(void); void crsfBind(void); diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index f738c71bf28..6ffa4df3588 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -24,6 +24,7 @@ #define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); +bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); void crsfScheduleMspResponse(void); From 73857e22b8793aa5405cc4d5480b99800aa81de4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Sat, 14 Sep 2024 18:56:04 +0100 Subject: [PATCH 068/175] Remove debug code. Simplify initialisation. --- src/main/io/vtx_msp.c | 22 +--------------------- 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 7868fed6717..9271f2e5eca 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -92,9 +92,6 @@ static bool vtxMspIsReady(const vtxDevice_t *); static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { - LOG_DEBUG(VTX, "msp prepareMspFrame\r\n"); - //UNUSED(vtxDevice); - // Send an MSP_VTX_V2 frame to the VTX mspFrame[0] = vtxMspGetDeviceType(vtxDevice); @@ -125,7 +122,6 @@ static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) { - LOG_DEBUG(VTX, "msp mspCrsfPush\r\n"); #ifndef USE_TELEMETRY_CRSF UNUSED(mspCommand); UNUSED(mspFrame); @@ -167,7 +163,6 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) { - LOG_DEBUG(VTX, "msp vtxMspProcess\r\n"); UNUSED(vtxDevice); uint8_t mspFrame[15]; @@ -203,21 +198,17 @@ static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs) static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice) { - LOG_DEBUG(VTX, "msp vtxMspGetDeviceType\r\n"); UNUSED(vtxDevice); - return VTXDEV_MSP; } static bool vtxMspIsReady(const vtxDevice_t *vtxDevice) { - LOG_DEBUG(VTX, "msp vtxMspIsReady\r\n"); return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready; } static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel) { - LOG_DEBUG(VTX, "msp vtxMspSetBandAndChannel\r\n"); UNUSED(vtxDevice); if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) { @@ -232,7 +223,6 @@ static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { - LOG_DEBUG(VTX, "msp vtxMspSetPowerByIndex\r\n"); UNUSED(vtxDevice); vtxState.request.power = vtxMspPowerTable[index - 1]; @@ -242,7 +232,6 @@ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) { - LOG_DEBUG(VTX, "msp vtxMspSetPitMode\r\n"); UNUSED(vtxDevice); vtxState.request.pitMode = onOff; @@ -251,7 +240,6 @@ static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff) static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel) { - LOG_DEBUG(VTX, "msp vtxMspGetBandAndChannel\r\n"); UNUSED(vtxDevice); *pBand = vtxState.request.band; @@ -261,7 +249,6 @@ static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) { - LOG_DEBUG(VTX, "msp vtxMspGetPowerIndex\r\n"); UNUSED(vtxDevice); *pIndex = vtxState.request.powerIndex; @@ -270,7 +257,6 @@ static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex) static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) { - LOG_DEBUG(VTX, "msp vtxMspGetPitMode\r\n"); UNUSED(vtxDevice); *pOnOff = vtxState.request.pitMode; @@ -279,7 +265,6 @@ static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff) static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) { - LOG_DEBUG(VTX, "msp vtxMspGetFreq\r\n"); UNUSED(vtxDevice); *pFreq = vtxState.request.freq; @@ -288,7 +273,6 @@ static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq) static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw) { - LOG_DEBUG(VTX, "msp vtxMspGetPower\r\n"); UNUSED(vtxDevice); *pIndex = vtxState.request.powerIndex; @@ -298,7 +282,6 @@ static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16 static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo) { - LOG_DEBUG(VTX, "msp vtxMspGetOsdInfo\r\n"); UNUSED(vtxDevice); pOsdInfo->band = vtxState.request.band; @@ -341,7 +324,6 @@ static vtxDevice_t vtxMsp = { bool vtxMspInit(void) { - LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__); const serialPortConfig_t *portConfig; // Shares MSP_OSD port @@ -360,9 +342,7 @@ bool vtxMspInit(void) vtxState.request.channel = vtxSettingsConfig()->channel; vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel); vtxState.request.power = vtxSettingsConfig()->power; - uint8_t pitmode = 0; - vtxCommonGetPitMode(&vtxMsp, &pitmode); - vtxState.request.pitMode = pitmode; + vtxState.request.pitMode = 0; vtxCommonSetDevice(&vtxMsp); return true; From 2f0d50362d84c3ce37f0b09068e3abe1e5b16bff Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 17 Sep 2024 13:29:11 +0100 Subject: [PATCH 069/175] Remove direct call to band and channel setting. --- src/main/fc/fc_msp.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7993dae6eef..08b7b336ae7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2675,13 +2675,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel const uint8_t newBand = (newFrequency / 8) + 1; const uint8_t newChannel = (newFrequency % 8) + 1; - - if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { - vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel); + if (vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) { + vtxSettingsConfigMutable()->band = newBand; + vtxSettingsConfigMutable()->channel = newChannel; } - - vtxSettingsConfigMutable()->band = newBand; - vtxSettingsConfigMutable()->channel = newChannel; } if (sbufBytesRemaining(src) > 1) { From 3009d7d86c3469f8a8fc9f18c4df069ff588bf1a Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Tue, 17 Sep 2024 13:30:28 +0100 Subject: [PATCH 070/175] Ensure VTX logic switches take priority over FC/VTX menu settings. --- src/main/programming/logic_condition.c | 53 ++++++++++++++------------ 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 98ae76f8710..9ca587ad9c8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -298,42 +298,45 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: { - uint8_t newpower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if (newpower != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - // HDZERO VTX max power+1 is 0mW. - newpower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newpower; - vtxSettingsConfigMutable()->power = newpower; - return newpower; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if (power != operandA || power != vtxSettingsConfig()->power) { + // HDZERO VTX max power+1 is 0mW. + power = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; + vtxSettingsConfigMutable()->power = power; + return power; + } } + return false; break; } case LOGIC_CONDITION_SET_VTX_BAND: { - uint8_t newband = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - if (newband != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newband = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newband; - vtxSettingsConfigMutable()->band = newband; - return newband; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if (band != operandA || band != vtxSettingsConfig()->band) { + band = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = band; + vtxSettingsConfigMutable()->band = band; + return band; + } } + return false; break; } case LOGIC_CONDITION_SET_VTX_CHANNEL: { - uint8_t newchannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - if (newchannel != operandA && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - newchannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newchannel; - vtxSettingsConfigMutable()->channel = newchannel; - return newchannel; - } else { - return false; + if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + uint8_t channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if (channel != operandA || channel != vtxSettingsConfig()->channel) { + channel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = channel; + vtxSettingsConfigMutable()->channel = channel; + return channel; + } } + return false; break; } #endif From c1565a77343952ebf2cad95d9d581c16044669e3 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Wed, 18 Sep 2024 05:09:52 +0200 Subject: [PATCH 071/175] fixed z position estimator error estimation --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d5a342173d3..d20087f9e32 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -617,7 +617,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p); // Accelerometer bias ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); From ce7e01400172a8837c64a542c80745858bc8caa1 Mon Sep 17 00:00:00 2001 From: flywoo Date: Wed, 18 Sep 2024 15:45:45 +0800 Subject: [PATCH 072/175] Update FLYWOOF745 config.c --- src/main/target/FLYWOOF745/config.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/FLYWOOF745/config.c b/src/main/target/FLYWOOF745/config.c index 7dfe40728dd..16cadc0231b 100644 --- a/src/main/target/FLYWOOF745/config.c +++ b/src/main/target/FLYWOOF745/config.c @@ -32,6 +32,5 @@ void targetConfiguration(void) // Make sure S1-S4 default to Motors timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; - timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS; timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS; } From f9745d7a643e3fcfd7914eef97702b69384900a1 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 15 Sep 2024 19:07:49 -0300 Subject: [PATCH 073/175] Gforce Buffe fix --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ba4571a8b4c..1b88226b9cb 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5022,7 +5022,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX) uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; - char outBuff[12]; + char outBuff[20]; const float max_gforce = accGetMeasuredMaxG(); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); From 5260d88f9c09d6d88d1e856caea62f704faed5a1 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Wed, 18 Sep 2024 10:44:34 +0800 Subject: [PATCH 074/175] tbs_lucid: mask PA13/PA14 to enable debugging --- src/main/target/TBS_LUCID_FC/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index 7a9ed60fb52..8ee8716133d 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -130,7 +130,7 @@ #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2) | BIT(12) | BIT(13)) From 79026ed027bb75a105ca28fb8b37f4db806fabf4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:31:30 +0100 Subject: [PATCH 075/175] Include MSP VTX with Smart Audio and Tramp defines. --- src/main/fc/fc_tasks.c | 2 +- src/main/fc/rc_adjustments.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index fbe119c272b..97cf005a5b6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -406,7 +406,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW)); #endif #ifdef USE_VTX_CONTROL -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) setTaskEnabled(TASK_VTXCTRL, true); #endif #endif diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 37c338fedd8..27b88800841 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -576,7 +576,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; -#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) +#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP) case ADJUSTMENT_VTX_POWER_LEVEL: { vtxDeviceCapability_t vtxDeviceCapability; From defd28358411c3a58183c5a52a8b8aef72992f9f Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:34:07 +0100 Subject: [PATCH 076/175] Allow VTX to specify number of power levels available. --- src/main/fc/fc_msp.c | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 08b7b336ae7..0601cd89a78 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2699,9 +2699,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } - // ////////////////////////////////////////////////////////// - // this code is taken from BF, it's hack for HDZERO VTX MSP frame - // API version 1.42 - this parameter kept separate since clients may already be supplying + // API version 1.42 - extension for pitmode frequency if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); //skip pitModeFreq } @@ -2719,13 +2717,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } } - /* if (sbufBytesRemaining(src) >= 4) { - sbufRead8(src); // freq_l - sbufRead8(src); // freq_h - sbufRead8(src); // band count - sbufRead8(src); // channel count - }*/ - // ////////////////////////////////////////////////////////// + if (sbufBytesRemaining(src) >= 5) { + sbufReadU16(src); // freq + sbufReadU8(src); // band count + sbufReadU8(src); // channel count + + uint8_t newPowerCount = sbufReadU8(src); + if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) { + vtxDevice->capability.powerCount = newPowerCount; + } + } } } } From f4abd1c3b7e7ad72259e6c753f34bda1024ddf76 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 20 Sep 2024 15:36:58 +0100 Subject: [PATCH 077/175] Allow FC Logic to specifiy a VTX power level of 0. --- src/main/programming/logic_condition.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 9ca587ad9c8..6501ab527c5 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -301,8 +301,7 @@ static int logicConditionCompute( if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; if (power != operandA || power != vtxSettingsConfig()->power) { - // HDZERO VTX max power+1 is 0mW. - power = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount+1); + power = constrain(operandA, 0, vtxDeviceCapability.powerCount); // Allow a power level of 0 logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; vtxSettingsConfigMutable()->power = power; return power; From 9923c306201628d6dc9c416a4122b0be07fa21d2 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 23 Sep 2024 11:55:38 +0800 Subject: [PATCH 078/175] lucid: use PC5 as adc, use PA13/PA14 as PINIO --- src/main/target/TBS_LUCID_FC/target.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index 8ee8716133d..f5bdecc0fac 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -120,17 +120,19 @@ #define USE_PINIO #define USE_PINIOBOX -#define PINIO1_PIN PC5 // Camera Control +#define PINIO1_PIN PA13 +#define PINIO2_PIN PA14 #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_CHANNEL7 #define ADC_CHANNEL_1_PIN PC0 #define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13))) +#define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2) | BIT(12) | BIT(13)) From 302eff2c8e52e08a4509e8dfff2c11f3383ebb96 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Wed, 18 Sep 2024 10:54:57 +0800 Subject: [PATCH 079/175] at32: add abilty to specify uart af --- src/main/drivers/serial_uart_at32f43x.c | 32 +++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c index 90ebe05ef82..ad4b4e0c458 100644 --- a/src/main/drivers/serial_uart_at32f43x.c +++ b/src/main/drivers/serial_uart_at32f43x.c @@ -54,7 +54,11 @@ static uartDevice_t uart1 = .dev = USART1, .rx = IO_TAG(UART1_RX_PIN), .tx = IO_TAG(UART1_TX_PIN), +#ifdef UART1_AF + .af = CONCAT(GPIO_MUX_, UART1_AF), +#else .af = GPIO_MUX_7, +#endif #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, #endif @@ -75,7 +79,11 @@ static uartDevice_t uart2 = .dev = USART2, .rx = IO_TAG(UART2_RX_PIN), .tx = IO_TAG(UART2_TX_PIN), +#ifdef UART2_AF + .af = CONCAT(GPIO_MUX_, UART2_AF), +#else .af = GPIO_MUX_7, +#endif #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, #endif @@ -96,7 +104,11 @@ static uartDevice_t uart3 = .dev = USART3, .rx = IO_TAG(UART3_RX_PIN), .tx = IO_TAG(UART3_TX_PIN), +#ifdef UART3_AF + .af = CONCAT(GPIO_MUX_, UART3_AF), +#else .af = GPIO_MUX_7, +#endif #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, #endif @@ -117,7 +129,11 @@ static uartDevice_t uart4 = .dev = UART4, .rx = IO_TAG(UART4_RX_PIN), .tx = IO_TAG(UART4_TX_PIN), +#ifdef UART4_AF + .af = CONCAT(GPIO_MUX_, UART4_AF), +#else .af = GPIO_MUX_8, +#endif #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, #endif @@ -138,7 +154,11 @@ static uartDevice_t uart5 = .dev = UART5, .rx = IO_TAG(UART5_RX_PIN), .tx = IO_TAG(UART5_TX_PIN), +#ifdef UART5_AF + .af = CONCAT(GPIO_MUX_, UART5_AF), +#else .af = GPIO_MUX_8, +#endif #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, #endif @@ -159,7 +179,11 @@ static uartDevice_t uart6 = .dev = USART6, .rx = IO_TAG(UART6_RX_PIN), .tx = IO_TAG(UART6_TX_PIN), +#ifdef UART6_AF + .af = CONCAT(GPIO_MUX_, UART6_AF), +#else .af = GPIO_MUX_8, +#endif #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, #endif @@ -180,7 +204,11 @@ static uartDevice_t uart7 = .dev = UART7, .rx = IO_TAG(UART7_RX_PIN), .tx = IO_TAG(UART7_TX_PIN), +#ifdef UART7_AF + .af = CONCAT(GPIO_MUX_, UART7_AF), +#else .af = GPIO_MUX_8, +#endif .rcc_apb1 = RCC_APB1(UART7), .irq = UART7_IRQn, .irqPriority = NVIC_PRIO_SERIALUART, @@ -198,7 +226,11 @@ static uartDevice_t uart8 = .dev = UART8, .rx = IO_TAG(UART8_RX_PIN), .tx = IO_TAG(UART8_TX_PIN), +#ifdef UART8_AF + .af = CONCAT(GPIO_MUX_, UART8_AF), +#else .af = GPIO_MUX_8, +#endif .rcc_apb1 = RCC_APB1(UART8), .irq = UART8_IRQn, .irqPriority = NVIC_PRIO_SERIALUART, From d5ab77e1a7297cfbf2104e13f9998d4234f3a425 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Wed, 18 Sep 2024 10:58:46 +0800 Subject: [PATCH 080/175] tbs_lucid: specify AF for uart2 --- src/main/target/TBS_LUCID_FC/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index f5bdecc0fac..6e755b9a1a5 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -41,6 +41,7 @@ #define UART1_RX_PIN PA10 #define USE_UART2 +#define UART2_AF 6 #define UART2_TX_PIN NONE #define UART2_RX_PIN PB0 From a591a83cf7dc3a856c05744a08c6717c25e530c9 Mon Sep 17 00:00:00 2001 From: bfmvsa Date: Thu, 26 Sep 2024 11:17:09 +0200 Subject: [PATCH 081/175] Change bus for built-in compass --- src/main/target/MICOAIR743/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 4ba27f3447e..52ebef24e1b 100644 --- a/src/main/target/MICOAIR743/target.h +++ b/src/main/target/MICOAIR743/target.h @@ -100,7 +100,7 @@ #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 +#define MAG_I2C_BUS BUS_I2C2 #define USE_MAG_ALL // *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** From 7907c8c70866fd04289437b0fdbaa3bbbd10b0d1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 26 Sep 2024 11:51:11 +0200 Subject: [PATCH 082/175] Add target for external compass --- src/main/target/MICOAIR743/CMakeLists.txt | 3 ++- src/main/target/MICOAIR743/target.h | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/target/MICOAIR743/CMakeLists.txt b/src/main/target/MICOAIR743/CMakeLists.txt index 1d381638780..6847f3daefe 100644 --- a/src/main/target/MICOAIR743/CMakeLists.txt +++ b/src/main/target/MICOAIR743/CMakeLists.txt @@ -1 +1,2 @@ -target_stm32h743xi(MICOAIR743) \ No newline at end of file +target_stm32h743xi(MICOAIR743) +target_stm32h743xi(MICOAIR743_EXTMAG) diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 52ebef24e1b..9326d6622b9 100644 --- a/src/main/target/MICOAIR743/target.h +++ b/src/main/target/MICOAIR743/target.h @@ -100,7 +100,14 @@ #define BARO_I2C_BUS BUS_I2C2 #define USE_MAG + +#ifdef MICOAIR743_EXTMAG +// External compass +#define MAG_I2C_BUS BUS_I2C2 +#else +// Onboard compass #define MAG_I2C_BUS BUS_I2C2 +#endif #define USE_MAG_ALL // *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** From 54adf5002f4358da438c8be2969054bfd965b1db Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 26 Sep 2024 11:53:36 +0200 Subject: [PATCH 083/175] Missed change --- src/main/target/MICOAIR743/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h index 9326d6622b9..526cf438d08 100644 --- a/src/main/target/MICOAIR743/target.h +++ b/src/main/target/MICOAIR743/target.h @@ -103,7 +103,7 @@ #ifdef MICOAIR743_EXTMAG // External compass -#define MAG_I2C_BUS BUS_I2C2 +#define MAG_I2C_BUS BUS_I2C1 #else // Onboard compass #define MAG_I2C_BUS BUS_I2C2 From c7d55874b88047eade4430929fad1cf5cc0bc278 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Thu, 26 Sep 2024 13:46:03 +0100 Subject: [PATCH 084/175] Allow min power of 0 (will need configurator change at some point). Set band/channel/power max values sent from VTX. Optimise VTX logic condition code. --- src/main/drivers/vtx_common.h | 4 +-- src/main/fc/fc_msp.c | 5 ++- src/main/io/vtx_msp.c | 25 +++++---------- src/main/programming/logic_condition.c | 42 +++++++++++--------------- 4 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h index 80b957c5d13..83e608dc49b 100644 --- a/src/main/drivers/vtx_common.h +++ b/src/main/drivers/vtx_common.h @@ -36,11 +36,11 @@ #define VTX_SETTINGS_POWER_COUNT 8 #define VTX_SETTINGS_DEFAULT_POWER 1 -#define VTX_SETTINGS_MIN_POWER 1 +#define VTX_SETTINGS_MIN_POWER 0 #define VTX_SETTINGS_MIN_USER_FREQ 5000 #define VTX_SETTINGS_MAX_USER_FREQ 5999 #define VTX_SETTINGS_FREQCMD -#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1) +#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER) #else diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 69552d03173..bfa57bee833 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2708,8 +2708,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } } - if (sbufBytesRemaining(src) >= 5) { + if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); // freq + } + + if (sbufBytesRemaining(src) >= 3) { sbufReadU8(src); // band count sbufReadU8(src); // channel count diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c index 9271f2e5eca..84746e0c9f3 100644 --- a/src/main/io/vtx_msp.c +++ b/src/main/io/vtx_msp.c @@ -77,11 +77,11 @@ const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = { }; const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = { - "---", "25", "200", "500", "MAX" + "0", "25", "200", "500", "MAX" }; -const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT] = { - 25, 200, 500, 1000 +const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT + 1] = { + 0, 25, 200, 500, 1000 }; static serialPortIdentifier_e mspVtxPortIdentifier; @@ -89,11 +89,11 @@ static vtxProtoState_t vtxState; static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *); static bool vtxMspIsReady(const vtxDevice_t *); +static vtxDevice_t vtxMsp; static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) { // Send an MSP_VTX_V2 frame to the VTX - mspFrame[0] = vtxMspGetDeviceType(vtxDevice); mspFrame[1] = vtxState.request.band; mspFrame[2] = vtxState.request.channel; @@ -106,18 +106,9 @@ static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame) mspFrame[9] = 0; // pitmode freq Low mspFrame[10] = 0; // pitmode freq High mspFrame[11] = 0; // 1 if using vtx table - mspFrame[12] = 6; // bands or 0 - mspFrame[13] = 8; // channels or 0 - mspFrame[14] = 5; // power levels or 0 - - LOG_DEBUG(VTX, "msp device [%d]\r\n", mspFrame[0]); - LOG_DEBUG(VTX, "msp band [%d]\r\n", mspFrame[1]); - LOG_DEBUG(VTX, "msp channel [%d]\r\n", mspFrame[2]); - LOG_DEBUG(VTX, "msp power [%d]\r\n", mspFrame[3]); - LOG_DEBUG(VTX, "msp freq [%d]\r\n", ((uint16_t)mspFrame[6] << 8) + mspFrame[5]); - LOG_DEBUG(VTX, "msp pitmode [%d]\r\n", mspFrame[4]); - LOG_DEBUG(VTX, "msp isready [%d]\r\n", mspFrame[7]); - LOG_DEBUG(VTX, "msp lowPower [%d]\r\n", mspFrame[8]); + mspFrame[12] = vtxMsp.capability.bandCount; // bands or 0 + mspFrame[13] = vtxMsp.capability.channelCount; // channels or 0 + mspFrame[14] = vtxMsp.capability.powerCount; // power levels or 0 } static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize) @@ -225,7 +216,7 @@ static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index) { UNUSED(vtxDevice); - vtxState.request.power = vtxMspPowerTable[index - 1]; + vtxState.request.power = vtxMspPowerTable[index]; vtxState.request.powerIndex = index; vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER; } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6501ab527c5..e99ab92cfe9 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -298,42 +298,36 @@ static int logicConditionCompute( #if defined(USE_VTX_CONTROL) case LOGIC_CONDITION_SET_VTX_POWER_LEVEL: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; - if (power != operandA || power != vtxSettingsConfig()->power) { - power = constrain(operandA, 0, vtxDeviceCapability.powerCount); // Allow a power level of 0 - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = power; - vtxSettingsConfigMutable()->power = power; - return power; - } + uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL]; + if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower; + vtxSettingsConfigMutable()->power = newPower; + return newPower; } return false; break; } case LOGIC_CONDITION_SET_VTX_BAND: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; - if (band != operandA || band != vtxSettingsConfig()->band) { - band = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = band; - vtxSettingsConfigMutable()->band = band; - return band; - } + uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND]; + if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; + vtxSettingsConfigMutable()->band = newBand; + return newBand; } return false; break; } case LOGIC_CONDITION_SET_VTX_CHANNEL: { - if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { - uint8_t channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; - if (channel != operandA || channel != vtxSettingsConfig()->channel) { - channel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); - logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = channel; - vtxSettingsConfigMutable()->channel = channel; - return channel; - } + uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL]; + if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { + newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); + logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; + vtxSettingsConfigMutable()->channel = newChannel; + return newChannel; } return false; break; From e92af8f08d4a31d543c6d27bb2ec669658074443 Mon Sep 17 00:00:00 2001 From: Jeff Hendrix Date: Thu, 26 Sep 2024 11:52:45 -0600 Subject: [PATCH 085/175] Add MSP command to get ESC telemetry --- src/main/fc/fc_msp.c | 12 ++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 1 + 2 files changed, 13 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f82f3cbaeaf..784e14405dc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1674,6 +1674,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; + + case MSP2_INAV_ESC_TELEM: + { + uint8_t motorCount = getMotorCount(); + sbufWriteU8(dst, motorCount); + + for (uint8_t i = 0; i < motorCount; i++){ + const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry + sbufWriteDataSafe(dst, escState, sizeof(escSensorData_t)); + } + } + break; #endif #ifdef USE_EZ_TUNE diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 02c8c979aae..465f5098536 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -89,6 +89,7 @@ #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B #define MSP2_INAV_ESC_RPM 0x2040 +#define MSP2_INAV_ESC_TELEM 0x2041 #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 From b41bcdc1b02ea870b6a0380acb1d9f4e70389b38 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Fri, 27 Sep 2024 14:15:37 +0100 Subject: [PATCH 086/175] Ensure VTX logic settings override any external changes. --- src/main/programming/logic_condition.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index e99ab92cfe9..697091ff7a0 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -302,6 +302,9 @@ static int logicConditionCompute( if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower; + if (newPower != vtxSettingsConfig()->power) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere + } vtxSettingsConfigMutable()->power = newPower; return newPower; } @@ -314,6 +317,9 @@ static int logicConditionCompute( if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; + if (newBand != vtxSettingsConfig()->band) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newBand); + } vtxSettingsConfigMutable()->band = newBand; return newBand; } @@ -326,6 +332,9 @@ static int logicConditionCompute( if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) { newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; + if (newChannel != vtxSettingsConfig()->channel) { + vtxCommonSetPowerByIndex(vtxCommonDevice(), newChannel); + } vtxSettingsConfigMutable()->channel = newChannel; return newChannel; } From cd614098584d57b607b0cb280713f774d53a3ae7 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Fri, 27 Sep 2024 19:35:31 +0100 Subject: [PATCH 087/175] Switch parameters with battery capacity unit There is a parameter for the OSD called `osd_stats_energy_unit`. This selects whether mAh or Wh are displayed on the end of flight stats screen. People are confused the when the switch the battery capacity unit to Wh from Ah, this value doesn't change. This PR will change `osd_stats_energy_unit` to match the battery capacity unit if it changes. This will be more an expected behavior. Plus it gives pilots the option to change to the other units for the stats if they want. --- src/main/fc/cli.c | 11 +++++++++++ src/main/fc/fc_msp.c | 14 ++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ee5cb289a94..0a54e489139 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3714,6 +3714,17 @@ static void cliSet(char *cmdline) } if (changeValue) { + // If changing the battery capacity unit, update the osd stats energy unit to match + if (strcmp(name, "battery_capacity_unit") == 0) { + if (batteryMetersConfig()->capacity_unit != tmp.int_value) { + if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } + } + } + cliSetIntFloatVar(val, tmp); cliPrintf("%s set to ", name); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f82f3cbaeaf..76baa4d41eb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2082,6 +2082,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2090,6 +2091,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; + } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; @@ -2121,6 +2128,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); + uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit; batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; @@ -2129,6 +2137,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; + } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) { + if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; + } else { + osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH; + } } } else return MSP_RESULT_ERROR; From 5b3b60109d7a4b4e0d5665a3a7ade6e0e3b12188 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Fri, 27 Sep 2024 20:03:38 +0100 Subject: [PATCH 088/175] Fix for SITL compiles --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 0a54e489139..5aed69c1d13 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3716,7 +3716,7 @@ static void cliSet(char *cmdline) if (changeValue) { // If changing the battery capacity unit, update the osd stats energy unit to match if (strcmp(name, "battery_capacity_unit") == 0) { - if (batteryMetersConfig()->capacity_unit != tmp.int_value) { + if (batteryMetersConfig()->capacity_unit != (uint8_t)tmp.int_value) { if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) { osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH; } else { From ac6a1de0885412c3c119e8fc92edeafc132b55f4 Mon Sep 17 00:00:00 2001 From: Geoff Sim Date: Mon, 7 Oct 2024 16:56:18 +0100 Subject: [PATCH 089/175] Fix cut/paste error in band and channel logic override. --- src/main/programming/logic_condition.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 697091ff7a0..1c19889fe47 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -318,7 +318,7 @@ static int logicConditionCompute( newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand; if (newBand != vtxSettingsConfig()->band) { - vtxCommonSetPowerByIndex(vtxCommonDevice(), newBand); + vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel); } vtxSettingsConfigMutable()->band = newBand; return newBand; @@ -333,7 +333,7 @@ static int logicConditionCompute( newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount); logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel; if (newChannel != vtxSettingsConfig()->channel) { - vtxCommonSetPowerByIndex(vtxCommonDevice(), newChannel); + vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel); } vtxSettingsConfigMutable()->channel = newChannel; return newChannel; From 05d1890ba8a2eb5db6b633d3951981fb8b7a9503 Mon Sep 17 00:00:00 2001 From: Sensei Date: Wed, 9 Oct 2024 12:23:36 -0500 Subject: [PATCH 090/175] Update Navigation.md - Remove outdated references to inav_use_gps_no_baro inav_use_gps_no_baro was removed --- docs/Navigation.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/Navigation.md b/docs/Navigation.md index 6caf4dc628f..4d7b9740d91 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -5,13 +5,13 @@ The navigation system in INAV is responsible for assisting the pilot allowing al ## NAV ALTHOLD mode - altitude hold Altitude hold requires a valid source of altitude - barometer, GPS or rangefinder. The best source is chosen automatically. -In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - quad goes up, pilot moves stick down - quad descends, you keep stick at neutral position - quad hovers. +In this mode THROTTLE stick controls climb rate (vertical velocity). When pilot moves stick up - aircraft goes up, pilot moves stick down - +aircraft descends, you keep stick at neutral position - aircraft maintains current altitude. -By default, GPS is available as an altitude source for airplanes only. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. ### CLI parameters affecting ALTHOLD mode: * *nav_use_midthr_for_althold* - when set to "0", firmware will remember where your throttle stick was when ALTHOLD was activated - this will be considered neutral position. When set to "1" - 50% throttle will be considered neutral position. -* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. + ### Related PIDs PIDs affecting altitude hold: ALT & VEL @@ -25,12 +25,12 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co ## NAV POSHOLD mode - position hold -Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless *inav_use_gps_no_baro* is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. +Position hold requires GPS, accelerometer and compass sensors. Multirotor requires barometer, unless is enabled. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From INAV 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically. ### CLI parameters affecting POSHOLD mode: * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own. -* *inav_use_gps_no_baro* - Multirotor only: enable althold based on GPS only, without baromemer installed. Default is OFF. + ### Related PIDs PIDs affecting position hold: POS, POSR @@ -42,7 +42,7 @@ PID meaning: Home for RTH is the position where vehicle was first armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. This position may also be overridden with Safehomes. RTH requires accelerometer, compass and GPS sensors. -RTH requires barometer for multirotor, unless unless *inav_use_gps_no_baro* is enabled. +RTH requires barometer for multirotor. RTH will maintain altitude during the return. When home is reached, a copter will attempt automated landing. An airplane will either loiter around the home position, or attempt an automated landing, depending on your settings. When deciding what altitude to maintain, RTH has 6 different modes of operation (controlled by *nav_rth_alt_mode* and *nav_rth_altitude* cli variables): From 8d6c2e7b5ea58f2092abcc64c0614a4e8c9a0ae5 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 14 Oct 2024 19:53:05 +0100 Subject: [PATCH 091/175] Update settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e43ad233b0c..2f5ef09d77d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -202,7 +202,7 @@ tables: values: ["GENERIC", "ELRS", "SIK"] enum: mavlinkRadio_e - name: default_altitude_source - values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] + values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"] enum: navDefaultAltitudeSensor_e constants: From a1cd050f00d2c9924b6b6447152192819b89b601 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Tue, 15 Oct 2024 12:31:16 +0100 Subject: [PATCH 092/175] Fix lockup on disarm when wh used in post-flight stats - Fixed issue - Tidied up formatting --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b88226b9cb..cdda88521b2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4789,7 +4789,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX) strcat(buff, "/"); osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false); strcat(buff, osdFormatTrimWhiteSpace(preBuff)); - tfp_sprintf(buff + strlen(buff), "%s%c", buff, SYM_WH); + tfp_sprintf(buff + strlen(buff), "%c", SYM_WH); } displayWrite(osdDisplayPort, statValX, row++, buff); @@ -4851,7 +4851,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b osdFormatCentiNumber(buff, (int32_t)((getMWhDrawn() - stats.flightStartMWh) * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); strcat(outBuff, "/"); - osdFormatCentiNumber(buff + strlen(buff), (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); strcat(outBuff, osdFormatTrimWhiteSpace(buff)); } else { strcat(outBuff, "---/---"); From 9cd1706314efdc3ea3cc0d6d3cb391f9552b84d9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 21 Oct 2024 14:44:40 +0200 Subject: [PATCH 093/175] Add PrincipioIoT F7 target --- src/main/target/PRINCIPIOTF7/CMakeLists.txt | 1 + src/main/target/PRINCIPIOTF7/config.c | 32 ++++ src/main/target/PRINCIPIOTF7/target.c | 74 ++++++++++ src/main/target/PRINCIPIOTF7/target.h | 156 ++++++++++++++++++++ 4 files changed, 263 insertions(+) create mode 100644 src/main/target/PRINCIPIOTF7/CMakeLists.txt create mode 100644 src/main/target/PRINCIPIOTF7/config.c create mode 100644 src/main/target/PRINCIPIOTF7/target.c create mode 100644 src/main/target/PRINCIPIOTF7/target.h diff --git a/src/main/target/PRINCIPIOTF7/CMakeLists.txt b/src/main/target/PRINCIPIOTF7/CMakeLists.txt new file mode 100644 index 00000000000..5988719a7ef --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(PRINCIPIOTF7) diff --git a/src/main/target/PRINCIPIOTF7/config.c b/src/main/target/PRINCIPIOTF7/config.c new file mode 100644 index 00000000000..c13aeda29b8 --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} diff --git a/src/main/target/PRINCIPIOTF7/target.c b/src/main/target/PRINCIPIOTF7/target.c new file mode 100644 index 00000000000..9268af45585 --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/target.c @@ -0,0 +1,74 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autogenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +//#include "drivers/sensor.h" + + +timerHardware_t timerHardware[] = { + // Motor 1 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + //DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 2 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + //DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + + // Motor 4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + + // Motor 5 + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + //DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 6 + DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + //DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + + // Motor 7 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + + // Motor 8 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + + // Servo 1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + + // Servo 2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 + + // LED Strip + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/PRINCIPIOTF7/target.h b/src/main/target/PRINCIPIOTF7/target.h new file mode 100644 index 00000000000..aa486d6af25 --- /dev/null +++ b/src/main/target/PRINCIPIOTF7/target.h @@ -0,0 +1,156 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + + + +#define TARGET_BOARD_IDENTIFIER "PRF7" +#define USBD_PRODUCT_STRING "PRINCIPIOTF7" + +// Beeper +#define USE_BEEPER +#define BEEPER PC13 + +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PA8 +#define LED0 PA4 +#define LED1 PB15 + +// UARTs +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_DISABLED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART2 + +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA7 +#define SPI1_MOSI_PIN PA6 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC3 +#define SPI2_MOSI_PIN PB14 + +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// MAG +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C1 + +// ADC +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 + +#define ADC_CHANNEL_2_PIN PC1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// Gyro & ACC + +// ICM42688-P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PB2 +#define ICM42605_SPI_BUS BUS_SPI1 + +// BARO +// DPS310 +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_DPS310 + +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +// Not sure which to use for the onboard chip (W25Q128JVPIQ) +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define USE_FLASH_W25M +#define W25M_SPI_BUS BUS_SPI2 +#define W25M_CS_PIN PB12 +#define USE_FLASH_W25M02G +#define W25M02G_SPI_BUS BUS_SPI2 +#define W25M02G_CS_PIN PB12 +#define USE_FLASH_W25M512 +#define W25M512_SPI_BUS BUS_SPI2 +#define W25M512_CS_PIN PB12 +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI2 +#define W25N01G_CS_PIN PB12 + +// Others + +#define MAX_PWM_OUTPUT_PORTS 10 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff From a3afa3fe7bd2fa2ea6d386d7ddd96db7ccc9467a Mon Sep 17 00:00:00 2001 From: megazar <48863881+ultrazar@users.noreply.github.com> Date: Wed, 23 Oct 2024 20:50:00 +0200 Subject: [PATCH 094/175] Added IMUTemperature read function for ICM42605 --- src/main/drivers/accgyro/accgyro_icm42605.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c index 13bc22fcee7..d0c4e3490aa 100644 --- a/src/main/drivers/accgyro/accgyro_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -62,6 +62,7 @@ #define ICM42605_RA_GYRO_DATA_X1 0x25 #define ICM42605_RA_ACCEL_DATA_X1 0x1F +#define ICM42605_RA_TEMP_DATA1 0x1D #define ICM42605_RA_INT_CONFIG 0x14 #define ICM42605_INT1_MODE_PULSED (0 << 2) @@ -321,6 +322,20 @@ static bool icm42605GyroRead(gyroDev_t *gyro) return true; } +static bool icm42605ReadTemperature(gyroDev_t *gyro, int16_t * temp) +{ + uint8_t data[2]; + + const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_TEMP_DATA1, data, 2); + if (!ack) { + return false; + } + // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 + *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10 + + return true; +} + bool icm42605GyroDetect(gyroDev_t *gyro) { gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU); @@ -340,7 +355,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro) gyro->initFn = icm42605AccAndGyroInit; gyro->readFn = icm42605GyroRead; gyro->intStatusFn = gyroCheckDataReady; - gyro->temperatureFn = NULL; + gyro->temperatureFn = icm42605ReadTemperature; gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor gyro->gyroAlign = gyro->busDev->param; From 51d98cffe5642b5e149ed0694bf4e0322c5dff40 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Tue, 1 Oct 2024 10:29:52 +0200 Subject: [PATCH 095/175] at32: add ability to specify uart rx/tx af independently --- src/main/drivers/serial_uart_at32f43x.c | 134 ++++++++++++++++++------ 1 file changed, 103 insertions(+), 31 deletions(-) diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c index ad4b4e0c458..f58e0a956aa 100644 --- a/src/main/drivers/serial_uart_at32f43x.c +++ b/src/main/drivers/serial_uart_at32f43x.c @@ -36,28 +36,37 @@ typedef struct uartDevice_s { usart_type* dev; uartPort_t port; ioTag_t rx; + uint8_t rx_af; ioTag_t tx; + uint8_t tx_af; volatile uint8_t rxBuffer[UART_RX_BUFFER_SIZE]; volatile uint8_t txBuffer[UART_TX_BUFFER_SIZE]; uint32_t rcc_ahb1; rccPeriphTag_t rcc_apb2; rccPeriphTag_t rcc_apb1; - uint8_t af; uint8_t irq; uint32_t irqPriority; bool pinSwap; } uartDevice_t; #ifdef USE_UART1 -static uartDevice_t uart1 = -{ +static uartDevice_t uart1 = { .dev = USART1, .rx = IO_TAG(UART1_RX_PIN), .tx = IO_TAG(UART1_TX_PIN), -#ifdef UART1_AF - .af = CONCAT(GPIO_MUX_, UART1_AF), +#if defined(UART1_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART1_RX_AF), +#elif defined(UART1_AF) + .rx_af = CONCAT(GPIO_MUX_, UART1_AF), +#else + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART1_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART1_TX_AF), +#elif defined(UART1_AF) + .tx_af = CONCAT(GPIO_MUX_, UART1_AF), #else - .af = GPIO_MUX_7, + .tx_af = GPIO_MUX_7, #endif #ifdef UART1_AHB1_PERIPHERALS .rcc_ahb1 = UART1_AHB1_PERIPHERALS, @@ -79,10 +88,19 @@ static uartDevice_t uart2 = .dev = USART2, .rx = IO_TAG(UART2_RX_PIN), .tx = IO_TAG(UART2_TX_PIN), -#ifdef UART2_AF - .af = CONCAT(GPIO_MUX_, UART2_AF), +#if defined(UART2_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART2_RX_AF), +#elif defined(UART2_AF) + .rx_af = CONCAT(GPIO_MUX_, UART2_AF), +#else + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART2_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART2_TX_AF), +#elif defined(UART2_AF) + .tx_af = CONCAT(GPIO_MUX_, UART2_AF), #else - .af = GPIO_MUX_7, + .tx_af = GPIO_MUX_7, #endif #ifdef UART2_AHB1_PERIPHERALS .rcc_ahb1 = UART2_AHB1_PERIPHERALS, @@ -104,10 +122,19 @@ static uartDevice_t uart3 = .dev = USART3, .rx = IO_TAG(UART3_RX_PIN), .tx = IO_TAG(UART3_TX_PIN), -#ifdef UART3_AF - .af = CONCAT(GPIO_MUX_, UART3_AF), +#if defined(UART3_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART3_RX_AF), +#elif defined(UART3_AF) + .rx_af = CONCAT(GPIO_MUX_, UART3_AF), #else - .af = GPIO_MUX_7, + .rx_af = GPIO_MUX_7, +#endif +#if defined(UART3_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART3_TX_AF), +#elif defined(UART3_AF) + .tx_af = CONCAT(GPIO_MUX_, UART3_AF), +#else + .tx_af = GPIO_MUX_7, #endif #ifdef UART3_AHB1_PERIPHERALS .rcc_ahb1 = UART3_AHB1_PERIPHERALS, @@ -129,10 +156,19 @@ static uartDevice_t uart4 = .dev = UART4, .rx = IO_TAG(UART4_RX_PIN), .tx = IO_TAG(UART4_TX_PIN), -#ifdef UART4_AF - .af = CONCAT(GPIO_MUX_, UART4_AF), +#if defined(UART4_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART4_RX_AF), +#elif defined(UART4_AF) + .rx_af = CONCAT(GPIO_MUX_, UART4_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART4_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART4_TX_AF), +#elif defined(UART4_AF) + .tx_af = CONCAT(GPIO_MUX_, UART4_AF), #else - .af = GPIO_MUX_8, + .tx_af = GPIO_MUX_8, #endif #ifdef UART4_AHB1_PERIPHERALS .rcc_ahb1 = UART4_AHB1_PERIPHERALS, @@ -154,10 +190,19 @@ static uartDevice_t uart5 = .dev = UART5, .rx = IO_TAG(UART5_RX_PIN), .tx = IO_TAG(UART5_TX_PIN), -#ifdef UART5_AF - .af = CONCAT(GPIO_MUX_, UART5_AF), +#if defined(UART5_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART5_RX_AF), +#elif defined(UART5_AF) + .rx_af = CONCAT(GPIO_MUX_, UART5_AF), #else - .af = GPIO_MUX_8, + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART5_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART5_TX_AF), +#elif defined(UART5_AF) + .tx_af = CONCAT(GPIO_MUX_, UART5_AF), +#else + .tx_af = GPIO_MUX_8, #endif #ifdef UART5_AHB1_PERIPHERALS .rcc_ahb1 = UART5_AHB1_PERIPHERALS, @@ -179,10 +224,19 @@ static uartDevice_t uart6 = .dev = USART6, .rx = IO_TAG(UART6_RX_PIN), .tx = IO_TAG(UART6_TX_PIN), -#ifdef UART6_AF - .af = CONCAT(GPIO_MUX_, UART6_AF), +#if defined(UART6_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART6_RX_AF), +#elif defined(UART6_AF) + .rx_af = CONCAT(GPIO_MUX_, UART6_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART6_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART6_TX_AF), +#elif defined(UART6_AF) + .tx_af = CONCAT(GPIO_MUX_, UART6_AF), #else - .af = GPIO_MUX_8, + .tx_af = GPIO_MUX_8, #endif #ifdef UART6_AHB1_PERIPHERALS .rcc_ahb1 = UART6_AHB1_PERIPHERALS, @@ -204,10 +258,19 @@ static uartDevice_t uart7 = .dev = UART7, .rx = IO_TAG(UART7_RX_PIN), .tx = IO_TAG(UART7_TX_PIN), -#ifdef UART7_AF - .af = CONCAT(GPIO_MUX_, UART7_AF), +#if defined(UART7_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART7_RX_AF), +#elif defined(UART7_AF) + .rx_af = CONCAT(GPIO_MUX_, UART7_AF), #else - .af = GPIO_MUX_8, + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART7_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART7_TX_AF), +#elif defined(UART7_AF) + .tx_af = CONCAT(GPIO_MUX_, UART7_AF), +#else + .tx_af = GPIO_MUX_8, #endif .rcc_apb1 = RCC_APB1(UART7), .irq = UART7_IRQn, @@ -226,10 +289,19 @@ static uartDevice_t uart8 = .dev = UART8, .rx = IO_TAG(UART8_RX_PIN), .tx = IO_TAG(UART8_TX_PIN), -#ifdef UART8_AF - .af = CONCAT(GPIO_MUX_, UART8_AF), +#if defined(UART8_RX_AF) + .rx_af = CONCAT(GPIO_MUX_, UART8_RX_AF), +#elif defined(UART8_AF) + .rx_af = CONCAT(GPIO_MUX_, UART8_AF), +#else + .rx_af = GPIO_MUX_8, +#endif +#if defined(UART8_TX_AF) + .tx_af = CONCAT(GPIO_MUX_, UART8_TX_AF), +#elif defined(UART8_AF) + .tx_af = CONCAT(GPIO_MUX_, UART8_AF), #else - .af = GPIO_MUX_8, + .tx_af = GPIO_MUX_8, #endif .rcc_apb1 = RCC_APB1(UART8), .irq = UART8_IRQn, @@ -389,22 +461,22 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, if (options & SERIAL_BIDIR) { IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(device)); if (options & SERIAL_BIDIR_PP) { - IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->tx_af); } else { IOConfigGPIOAF(tx, (options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_OD : IOCFG_AF_OD_UP, - uart->af); + uart->tx_af); } } else { if (mode & MODE_TX) { IOInit(tx, OWNER_SERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->rx_af); } if (mode & MODE_RX) { IOInit(rx, OWNER_SERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(device)); - IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af); + IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->rx_af); } } From a0a83cccd0c793420c74fa62177cd4c6c72b3276 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Tue, 1 Oct 2024 10:42:06 +0200 Subject: [PATCH 096/175] at32: re-check all serial pins and add swap/af defines as necessary --- src/main/target/NEUTRONRCF435MINI/target.h | 2 ++ src/main/target/TBS_LUCID_FC/target.h | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 6ef53abf64d..d4653ca6c25 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -141,7 +141,9 @@ #define USE_UART2 #define UART2_RX_PIN PB0 +#define UART2_RX_AF 6 #define UART2_TX_PIN PA2 +#define UART2_TX_AF 7 #define USE_UART3 #define USE_UART3_PIN_SWAP diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index 6e755b9a1a5..9baab07355d 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -42,10 +42,11 @@ #define USE_UART2 #define UART2_AF 6 -#define UART2_TX_PIN NONE +#define UART2_TX_PIN PB0 #define UART2_RX_PIN PB0 #define USE_UART3 +#define USE_UART3_PIN_SWAP #define UART3_TX_PIN PB11 #define UART3_RX_PIN PB10 From 4cdd6b9002e2bb9cb20277df8ab7ebf5a4504c8d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 24 Oct 2024 18:36:06 +0200 Subject: [PATCH 097/175] Update readme.md --- readme.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/readme.md b/readme.md index 1c313de3569..58c9dce7950 100644 --- a/readme.md +++ b/readme.md @@ -1,3 +1,11 @@ +# INAV 8.0 feature freeze + +It is that time of the year again, and the time for a new inav realease is near! + +The current plan is to have a feature freeze on **15th of November 2024**. + +For a preview of what is comming, have a look at ![milestone 8.0](https://github.com/iNavFlight/inav/milestone/43). + # INAV - navigation capable flight controller # F411 PSA From b3569812fcea76d269f07800a3e858d3dcf9dee4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 24 Oct 2024 18:47:31 +0200 Subject: [PATCH 098/175] Update readme.md --- readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/readme.md b/readme.md index 58c9dce7950..5107d4661ea 100644 --- a/readme.md +++ b/readme.md @@ -1,6 +1,6 @@ # INAV 8.0 feature freeze -It is that time of the year again, and the time for a new inav realease is near! +It is that time of the year again, and the time for a new INAV release is near! The current plan is to have a feature freeze on **15th of November 2024**. From 925c495591a9139310b630646aa37484a352029a Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Thu, 24 Oct 2024 20:23:45 +0100 Subject: [PATCH 099/175] Fix issue importing custom elements from diff --- src/main/fc/cli.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 5aed69c1d13..cbe85496f1a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2524,11 +2524,11 @@ static void osdCustom(char *cmdline){ int32_t i = args[INDEX]; if ( i >= 0 && i < MAX_CUSTOM_ELEMENTS && - args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 7 && + args[PART0_TYPE] >= 0 && args[PART0_TYPE] <= 26 && args[PART0_VALUE] >= 0 && args[PART0_VALUE] <= UINT8_MAX && - args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 7 && + args[PART1_TYPE] >= 0 && args[PART1_TYPE] <= 26 && args[PART1_VALUE] >= 0 && args[PART1_VALUE] <= UINT8_MAX && - args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 7 && + args[PART2_TYPE] >= 0 && args[PART2_TYPE] <= 26 && args[PART2_VALUE] >= 0 && args[PART2_VALUE] <= UINT8_MAX && args[VISIBILITY_TYPE] >= 0 && args[VISIBILITY_TYPE] <= 2 && args[VISIBILITY_VALUE] >= 0 && args[VISIBILITY_VALUE] <= UINT8_MAX From 16746b89dc67d7a12cb1ae4cce9f861507513aea Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Fri, 25 Oct 2024 12:54:40 +0100 Subject: [PATCH 100/175] Allow setting of altitude and distance precision --- docs/Settings.md | 22 ++++++++++++++++- src/main/fc/settings.yaml | 17 ++++++++++++- src/main/io/osd.c | 50 +++++++++++++++++++++------------------ src/main/io/osd.h | 6 +++-- 4 files changed, 68 insertions(+), 27 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ae0d3006ae6..ce4ff9c1ce4 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4582,6 +4582,26 @@ Value above which the OSD current consumption element will start blinking. Measu --- +### osd_decimals_altitude + +Number of decimals for the altitude displayed in the OSD [3-5]. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 3 | 5 | + +--- + +### osd_decimals_distance + +Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining. + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 3 | 5 | + +--- + ### osd_dist_alarm Value above which to make the OSD distance from home indicator blink (meters) @@ -4948,7 +4968,7 @@ Number of digits used for mAh precision. Currently used by mAh Used and Battery | Default | Min | Max | | --- | --- | --- | -| 4 | 4 | 6 | +| 4 | 3 | 6 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e43ad233b0c..a6e97e3811e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3566,6 +3566,20 @@ groups: field: main_voltage_decimals min: 1 max: 2 + - name: osd_decimals_altitude + description: "Number of decimals for the altitude displayed in the OSD [3-5]." + default_value: 3 + field: decimals_altitude + type: uint8_t + min: 3 + max: 5 + - name: osd_decimals_distance + description: "Number of decimals for distance displayed in the OSD [3-5]. This includes distance from home, total distance, and distance remaining." + default_value: 3 + field: decimals_distance + type: uint8_t + min: 3 + max: 5 - name: osd_coordinate_digits description: "Number of digits for the coordinates displayed in the OSD [8-11]." field: coordinate_digits @@ -3719,7 +3733,8 @@ groups: - name: osd_mah_precision description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity field: mAh_precision - min: 4 + default: 4 + min: 3 max: 6 default_value: 4 - name: osd_use_pilot_logo diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b88226b9cb..7174f0dfb7d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); void osdStartedSaveProcess(void) { @@ -259,10 +259,11 @@ bool osdIsNotMetric(void) { * prefixed by a a symbol to indicate the unit used. * @param dist Distance in centimeters */ -static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) +static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals, uint8_t digits) { - uint8_t digits = 3U; // Total number of digits (including decimal point) - uint8_t sym_index = 3U; // Position (index) at buffer of units symbol + if (digits == 0) // Total number of digits (including decimal point) + digits = 3U; + uint8_t sym_index = digits; // Position (index) at buffer of units symbol uint8_t symbol_m = SYM_DIST_M; uint8_t symbol_km = SYM_DIST_KM; uint8_t symbol_ft = SYM_DIST_FT; @@ -485,13 +486,12 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) */ void osdFormatAltitudeSymbol(char *buff, int32_t alt) { - uint8_t totalDigits = 4U; - uint8_t digits = 4U; - uint8_t symbolIndex = 4U; + uint8_t digits = osdConfig()->decimals_altitude; + uint8_t totalDigits = digits + 1; + uint8_t symbolIndex = digits + 1; uint8_t symbolKFt = SYM_ALT_KFT; if (alt >= 0) { - digits = 3U; buff[0] = ' '; } @@ -806,7 +806,7 @@ static const char * osdArmingDisabledReasonMessage(void) case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE: return OSD_MESSAGE_STR(OSD_MSG_DISABLE_NAV_FIRST); case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR: - osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0); + osdFormatDistanceSymbol(buf, distanceToFirstWP(), 0, 3); tfp_sprintf(messageBuf, "FIRST WP TOO FAR (%s)", buf); return message = messageBuf; case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR: @@ -1952,7 +1952,7 @@ static bool osdDrawSingleElement(uint8_t item) { buff[0] = SYM_HOME; uint32_t distance_to_home_cm = GPS_distanceToHome * 100; - osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0); + osdFormatDistanceSymbol(&buff[1], distance_to_home_cm, 0, osdConfig()->decimals_distance); uint16_t dist_alarm = osdConfig()->dist_alarm; if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { @@ -1963,7 +1963,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_TRIP_DIST: buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0, osdConfig()->decimals_distance); break; case OSD_BLACKBOX: @@ -2082,7 +2082,7 @@ static bool osdDrawSingleElement(uint8_t item) { if (isWaypointNavTrackingActive()) { buff[0] = SYM_CROSS_TRACK_ERROR; - osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0); + osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0, 3); } else { displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); return true; @@ -2241,7 +2241,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = '-'; buff[2] = '-'; } else { - osdFormatDistanceSymbol(buff, range, 1); + osdFormatDistanceSymbol(buff, range, 1, 3); } } break; @@ -2323,31 +2323,33 @@ static bool osdDrawSingleElement(uint8_t item) displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_FLIGHT_DIST_REMAINING); if ((!ARMING_FLAG(ARMED)) || (distanceMeters == -1)) { - buff[3] = SYM_BLANK; - buff[4] = '\0'; + buff[osdConfig()->decimals_distance] = SYM_BLANK; + buff[osdConfig()->decimals_distance + 1] = '\0'; strcpy(buff, "---"); } else if (distanceMeters == -2) { // Wind is too strong to come back with cruise throttle - buff[0] = buff[1] = buff[2] = SYM_WIND_HORIZONTAL; + for (uint8_t i = 0; i < osdConfig()->decimals_distance; i++) { + buff[i] = SYM_WIND_HORIZONTAL; + } switch ((osd_unit_e)osdConfig()->units){ case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - buff[3] = SYM_DIST_MI; + buff[osdConfig()->decimals_distance] = SYM_DIST_MI; break; case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - buff[3] = SYM_DIST_KM; + buff[osdConfig()->decimals_distance] = SYM_DIST_KM; break; case OSD_UNIT_GA: - buff[3] = SYM_DIST_NM; + buff[osdConfig()->decimals_distance] = SYM_DIST_NM; break; } - buff[4] = '\0'; + buff[osdConfig()->decimals_distance+1] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { - osdFormatDistanceSymbol(buff, distanceMeters * 100, 0); + osdFormatDistanceSymbol(buff, distanceMeters * 100, 0, osdConfig()->decimals_distance); if (distanceMeters == 0) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -2889,7 +2891,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_GLIDE_DIST; if (glideSeconds > 0) { uint32_t glideRangeCM = glideSeconds * gpsSol.groundSpeed; - osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0); + osdFormatDistanceSymbol(buff + 1, glideRangeCM, 0, 3); } else { tfp_sprintf(buff + 1, "%s%c", "---", SYM_BLANK); buff[5] = '\0'; @@ -4044,6 +4046,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT, .units = SETTING_OSD_UNITS_DEFAULT, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, + .decimals_altitude = SETTING_OSD_DECIMALS_ALTITUDE_DEFAULT, + .decimals_distance = SETTING_OSD_DECIMALS_DISTANCE_DEFAULT, .use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT, .inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT, .arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT, @@ -5779,7 +5783,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints char buf[6]; - osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0, 3); tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0cf93b69162..b0423d40eff 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -394,6 +394,8 @@ typedef struct osdConfig_s { // Preferences uint8_t main_voltage_decimals; + uint8_t decimals_altitude; + uint8_t decimals_distance; uint8_t ahi_reverse_roll; uint8_t ahi_max_pitch; uint8_t crosshairs_style; // from osd_crosshairs_style_e @@ -466,11 +468,11 @@ typedef struct osdConfig_s { #ifndef DISABLE_MSP_DJI_COMPAT bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol #endif - #ifdef USE_ADSB +#ifdef USE_ADSB uint16_t adsb_distance_warning; // in metres uint16_t adsb_distance_alert; // in metres uint16_t adsb_ignore_plane_above_me_limit; // in metres - #endif +#endif uint8_t radar_peers_display_time; // in seconds } osdConfig_t; From 5a165dee9a3eb972e921aa6ae16f72c82089e874 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Fri, 25 Oct 2024 21:24:57 +0100 Subject: [PATCH 101/175] Fix decimal places for mAh --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3ebd71f11b8..95fef520b7a 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1753,7 +1753,7 @@ static bool osdDrawSingleElement(uint8_t item) } else #endif { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, 2, mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { @@ -1795,7 +1795,7 @@ static bool osdDrawSingleElement(uint8_t item) } else #endif { - if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { + if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, 2, mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { From 81e79b731a64fa771659d5dc67996dc4ef9c0af0 Mon Sep 17 00:00:00 2001 From: TUNERC-Aria <71423100+TUNERC-Aria@users.noreply.github.com> Date: Tue, 29 Oct 2024 15:19:54 +0800 Subject: [PATCH 102/175] Update target.h Add the inverter's control pin for target TUNERCF405 --- src/main/target/TUNERCF405/target.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/target/TUNERCF405/target.h b/src/main/target/TUNERCF405/target.h index f193377c6ea..093da2fadf6 100644 --- a/src/main/target/TUNERCF405/target.h +++ b/src/main/target/TUNERCF405/target.h @@ -103,6 +103,9 @@ #define USE_UART4 #define UART4_RX_PIN PA1 #define UART4_TX_PIN PA0 +#define USE_UART_INVERTER +#define INVERTER_PIN_UART4_RX PC14 +#define INVERTER_PIN_USART4_RX PC14 #define USE_UART5 #define UART5_RX_PIN PD2 From a20064e45df235f7de1e27fca4be2e2255890302 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 30 Oct 2024 16:26:06 +0100 Subject: [PATCH 103/175] Update DPS310 driver to support SPL07-003 --- src/main/drivers/barometer/barometer_dps310.c | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c index 51e18eb12b0..663dcb27117 100644 --- a/src/main/drivers/barometer/barometer_dps310.c +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -61,6 +61,7 @@ #define DPS310_ID_REV_AND_PROD_ID (0x10) +#define SPL07_003_CHIP_ID (0x11) #define DPS310_RESET_BIT_SOFT_RST (0x09) // 0b1001 @@ -96,6 +97,8 @@ typedef struct { int16_t c20; // 16bit int16_t c21; // 16bit int16_t c30; // 16bit + int16_t c31; // 12bit + int16_t c40; // 12bit } calibrationCoefficients_t; typedef struct { @@ -105,6 +108,7 @@ typedef struct { } baroState_t; static baroState_t baroState; +static uint8_t chipId[1]; // Helper functions @@ -167,7 +171,10 @@ static bool deviceConfigure(busDevice_t * busDev) // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register. // Note: The coefficients read from the coefficient register are 2's complement numbers. - uint8_t coef[18]; + + unsigned coefficientLength = chipId[0] == SPL07_003_CHIP_ID ? 21 : 18; + uint8_t coef[coefficientLength]; + if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) { return false; } @@ -199,6 +206,17 @@ static bool deviceConfigure(busDevice_t * busDev) // 0x20 c30 [15:8] + 0x21 c30 [7:0] baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16); + if (chipId[0] == SPL07_003_CHIP_ID) { + // 0x23 c31 [3:0] + 0x22 c31 [11:4] + baroState.calib.c31 = getTwosComplement(((uint32_t)coef[18] << 4) | (((uint32_t)coef[19] >> 4) & 0x0F), 12); + + // 0x23 c40 [11:8] + 0x24 c40 [7:0] + baroState.calib.c40 = getTwosComplement((((uint32_t)coef[19] & 0x0F) << 8) | (uint32_t)coef[20], 12); + } else { + baroState.calib.c31 = 0; + baroState.calib.c40 = 0; + } + // MEAS_CFG: Make sure the device is in IDLE mode registerWriteBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_MASK, DPS310_MEAS_CFG_MEAS_IDLE); @@ -218,8 +236,12 @@ static bool deviceConfigure(busDevice_t * busDev) registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times) - const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; - registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + if (chipId[0] == SPL07_003_CHIP_ID) { + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16); + } else { + const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + } // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times) registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); @@ -265,9 +287,17 @@ static bool deviceReadMeasurement(baroDev_t *baro) const float c20 = baroState.calib.c20; const float c21 = baroState.calib.c21; const float c30 = baroState.calib.c30; + const float c31 = baroState.calib.c31; + const float c40 = baroState.calib.c40; // See section 4.9.1, How to Calculate Compensated Pressure Values, of datasheet - baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + // baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + if (chipId[0] == SPL07_003_CHIP_ID) { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * (c30 + Praw_sc * c40))) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * (c21 + Praw_sc * c31)); + } else { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + } + const float c0 = baroState.calib.c0; const float c1 = baroState.calib.c1; @@ -299,13 +329,11 @@ static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *tempera static bool deviceDetect(busDevice_t * busDev) { for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { - uint8_t chipId[1]; - delay(100); bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1); - if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) { + if (ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID)) { return true; } }; From 9b4d863c6be1aad5d3b305289b62dcf4ae4bee89 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 30 Oct 2024 16:26:06 +0100 Subject: [PATCH 104/175] Update DPS310 driver to support SPL07-003 --- src/main/drivers/barometer/barometer_dps310.c | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c index 51e18eb12b0..663dcb27117 100644 --- a/src/main/drivers/barometer/barometer_dps310.c +++ b/src/main/drivers/barometer/barometer_dps310.c @@ -61,6 +61,7 @@ #define DPS310_ID_REV_AND_PROD_ID (0x10) +#define SPL07_003_CHIP_ID (0x11) #define DPS310_RESET_BIT_SOFT_RST (0x09) // 0b1001 @@ -96,6 +97,8 @@ typedef struct { int16_t c20; // 16bit int16_t c21; // 16bit int16_t c30; // 16bit + int16_t c31; // 12bit + int16_t c40; // 12bit } calibrationCoefficients_t; typedef struct { @@ -105,6 +108,7 @@ typedef struct { } baroState_t; static baroState_t baroState; +static uint8_t chipId[1]; // Helper functions @@ -167,7 +171,10 @@ static bool deviceConfigure(busDevice_t * busDev) // 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register. // Note: The coefficients read from the coefficient register are 2's complement numbers. - uint8_t coef[18]; + + unsigned coefficientLength = chipId[0] == SPL07_003_CHIP_ID ? 21 : 18; + uint8_t coef[coefficientLength]; + if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) { return false; } @@ -199,6 +206,17 @@ static bool deviceConfigure(busDevice_t * busDev) // 0x20 c30 [15:8] + 0x21 c30 [7:0] baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16); + if (chipId[0] == SPL07_003_CHIP_ID) { + // 0x23 c31 [3:0] + 0x22 c31 [11:4] + baroState.calib.c31 = getTwosComplement(((uint32_t)coef[18] << 4) | (((uint32_t)coef[19] >> 4) & 0x0F), 12); + + // 0x23 c40 [11:8] + 0x24 c40 [7:0] + baroState.calib.c40 = getTwosComplement((((uint32_t)coef[19] & 0x0F) << 8) | (uint32_t)coef[20], 12); + } else { + baroState.calib.c31 = 0; + baroState.calib.c40 = 0; + } + // MEAS_CFG: Make sure the device is in IDLE mode registerWriteBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_MASK, DPS310_MEAS_CFG_MEAS_IDLE); @@ -218,8 +236,12 @@ static bool deviceConfigure(busDevice_t * busDev) registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16); // TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times) - const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; - registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + if (chipId[0] == SPL07_003_CHIP_ID) { + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16); + } else { + const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE; + registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE); + } // CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times) registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT); @@ -265,9 +287,17 @@ static bool deviceReadMeasurement(baroDev_t *baro) const float c20 = baroState.calib.c20; const float c21 = baroState.calib.c21; const float c30 = baroState.calib.c30; + const float c31 = baroState.calib.c31; + const float c40 = baroState.calib.c40; // See section 4.9.1, How to Calculate Compensated Pressure Values, of datasheet - baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + // baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + if (chipId[0] == SPL07_003_CHIP_ID) { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * (c30 + Praw_sc * c40))) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * (c21 + Praw_sc * c31)); + } else { + baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21); + } + const float c0 = baroState.calib.c0; const float c1 = baroState.calib.c1; @@ -299,13 +329,11 @@ static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *tempera static bool deviceDetect(busDevice_t * busDev) { for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) { - uint8_t chipId[1]; - delay(100); bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1); - if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) { + if (ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID)) { return true; } }; From ced14fe93a1a413af383c45295e132dfbc87e4c1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 1 Nov 2024 12:25:53 +0100 Subject: [PATCH 105/175] Add SpeedyBee F405 AIO target --- .vscode/c_cpp_properties.json | 3 +- .../target/SPEEDYBEEF405AIO/CMakeLists.txt | 1 + src/main/target/SPEEDYBEEF405AIO/config.c | 33 ++++ src/main/target/SPEEDYBEEF405AIO/target.c | 39 +++++ src/main/target/SPEEDYBEEF405AIO/target.h | 162 ++++++++++++++++++ src/main/target/SPEEDYBEEF405MINI/target.h | 2 - 6 files changed, 237 insertions(+), 3 deletions(-) create mode 100644 src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt create mode 100644 src/main/target/SPEEDYBEEF405AIO/config.c create mode 100644 src/main/target/SPEEDYBEEF405AIO/target.c create mode 100644 src/main/target/SPEEDYBEEF405AIO/target.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6e7d914b25a..760ac8127b0 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -57,7 +57,8 @@ "USE_SDCARD_SDIO", "USE_SDCARD", "USE_Q_TUNE", - "USE_GYRO_FFT_FILTER" + "USE_GYRO_FFT_FILTER", + "USE_BARO_DPS310" ], "configurationProvider": "ms-vscode.cmake-tools" } diff --git a/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt b/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt new file mode 100644 index 00000000000..5feb5864896 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SPEEDYBEEF405AIO HSE_MHZ 8) diff --git a/src/main/target/SPEEDYBEEF405AIO/config.c b/src/main/target/SPEEDYBEEF405AIO/config.c new file mode 100644 index 00000000000..e73067ef729 --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight. + * + * 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 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_RX_SERIAL; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; +} diff --git a/src/main/target/SPEEDYBEEF405AIO/target.c b/src/main/target/SPEEDYBEEF405AIO/target.c new file mode 100644 index 00000000000..9d1ae18ca7c --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S1 + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPEEDYBEEF405AIO/target.h b/src/main/target/SPEEDYBEEF405AIO/target.h new file mode 100644 index 00000000000..a6abdfcc37f --- /dev/null +++ b/src/main/target/SPEEDYBEEF405AIO/target.h @@ -0,0 +1,162 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SF4A" +#define USBD_PRODUCT_STRING "SPEEDYBEEF405AIO" + +#define LED0 PD7 + +#define BEEPER PD11 +#define BEEPER_INVERTED + +/* + * SPI Buses + */ +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C + */ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 +#define USE_I2C_PULLUP + +/* + * Serial + */ +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN NONE +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +/* + * Gyro + */ +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +/* + * Other + */ +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_CS_PIN PD5 +#define MAX7456_SPI_BUS BUS_SPI3 + +/* + * Blackbox + */ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 254 +#define CURRENT_METER_OFFSET 0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 4 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h index 1f25899ed92..c20ee9aa0ff 100644 --- a/src/main/target/SPEEDYBEEF405MINI/target.h +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -101,8 +101,6 @@ */ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 #define USE_BARO_DPS310 #define USE_MAG From ec46d9ee917e9b4781a4bf6cac186c8bbdf1a7f0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sat, 2 Nov 2024 10:51:48 -0300 Subject: [PATCH 106/175] Inital commit --- .vscode/c_cpp_properties.json | 16 +- .vscode/settings.json | 8 +- .vscode/tasks.json | 61 +- src/main/CMakeLists.txt | 1 + src/main/common/vector.h | 83 + src/main/config/parameter_group_ids.h | 5 +- src/main/fc/cli.c | 258 ++- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 8 + src/main/fc/fc_msp.c | 98 + src/main/fc/fc_tasks.c | 23 + src/main/fc/runtime_config.h | 9 +- src/main/fc/settings.yaml | 52 + src/main/io/osd.c | 119 +- src/main/io/osd.h | 20 + src/main/io/osd_dji_hd.c | 2 + src/main/msp/msp_protocol_v2_inav.h | 7 +- src/main/navigation/navigation.c | 261 ++- src/main/navigation/navigation.h | 113 ++ src/main/navigation/navigation_fixedwing.c | 6 + src/main/navigation/navigation_geozone.c | 1598 +++++++++++++++++ .../navigation_geozone_calculations.h | 480 +++++ src/main/navigation/navigation_private.h | 31 + src/main/scheduler/scheduler.h | 4 + src/main/target/common.h | 1 + 25 files changed, 3243 insertions(+), 23 deletions(-) create mode 100755 src/main/navigation/navigation_geozone.c create mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3a8d8d1c8ca..35984a9998c 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -4,7 +4,8 @@ "name": "Linux", "includePath": [ "${workspaceRoot}", - "${workspaceRoot}/src/main/**" + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/debug_SITL/src/main/target/SITL/SITL/*" ], "browse": { "limitSymbolsToIncludedHeaders": false, @@ -12,10 +13,14 @@ "${workspaceRoot}/**" ] }, - "intelliSenseMode": "msvc-x64", + "intelliSenseMode": "gcc-arm", "cStandard": "c11", "cppStandard": "c++17", "defines": [ + "uint16_t=unsigned short", + "int16_t=short", + "uint32_t=unsigned int", + "MCU_FLASH_SIZE=1024", "USE_OSD", "USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_2", @@ -35,9 +40,10 @@ "USE_PCF8574", "USE_ESC_SENSOR", "USE_ADAPTIVE_FILTER", - "MCU_FLASH_SIZE 1024", - ], - "configurationProvider": "ms-vscode.cmake-tools" + "SITL_BUILD", + "USE_GEOZONE" + ] + //"configurationProvider": "ms-vscode.cmake-tools" } ], "version": 4 diff --git a/.vscode/settings.json b/.vscode/settings.json index d1eb2357c02..f5f2ba120c9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,10 +4,14 @@ "cmath": "c", "ranges": "c", "navigation.h": "c", - "rth_trackback.h": "c" + "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", - "bus.h": "c" + "bus.h": "c", + "controlrate_profile.h": "c", + "settings.h": "c", + "settings_generated.h": "c", + "parameter_group.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 7be2cd3d176..9c109097adc 100755 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,9 +4,9 @@ "version": "2.0.0", "tasks": [ { - "label": "Build AOCODARCH7DUAL", + "label": "Build Matek F722-SE", "type": "shell", - "command": "make AOCODARCH7DUAL", + "command": "make MATEKF722SE", "group": "build", "problemMatcher": [], "options": { @@ -14,12 +14,11 @@ } }, { - "label": "Build AOCODARCH7DUAL", + "label": "Build Matek F722", "type": "shell", - "command": "make AOCODARCH7DUAL", + "command": "make MATEKF722", "group": { - "kind": "build", - "isDefault": true + "kind": "build" }, "problemMatcher": [], "options": { @@ -36,6 +35,56 @@ "options": { "cwd": "${workspaceFolder}/build" } + }, + { + "label": "CMAKE Build SITL", + "type": "shell", + "command": "mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "CMAKE Debug SITL", + "type": "shell", + "command": "mkdir -p debug_SITL && cd debug_SITL && cmake -DCMAKE_BUILD_TYPE=Debug -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "CMAKE Release SITL", + "type": "shell", + "command": "mkdir -p release_SITL && cd release_SITL && cmake -DCMAKE_BUILD_TYPE=Release -DSITL=ON ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}" + } + }, + { + "label": "Debug SITL", + "type": "shell", + "command": "make", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/debug_SITL" + } + }, + { + "label": "Release SITL", + "type": "shell", + "command": "make", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/release_SITL" + } } ] } \ No newline at end of file diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..7f188974cd9 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -576,6 +576,7 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h navigation/rth_trackback.c diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 449a0973b3d..737c29d56a7 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -29,6 +29,13 @@ typedef union { }; } fpVector3_t; +typedef union { + float v[2]; + struct { + float x,y; + }; +} fpVector2_t; + typedef struct { float m[3][3]; } fpMat3_t; @@ -116,3 +123,79 @@ static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t *result = ab; return result; } + +static inline fpVector3_t* vectorSub(fpVector3_t* result, const fpVector3_t* a, const fpVector3_t* b) +{ + fpVector3_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + ab.z = a->z - b->z; + + *result = ab; + return result; +} + +static inline float vectorDotProduct(const fpVector3_t* a, const fpVector3_t* b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +static inline float vector2NormSquared(const fpVector2_t * v) +{ + return sq(v->x) + sq(v->y); +} + +static inline fpVector2_t* vector2Normalize(fpVector2_t* result, const fpVector2_t* v) +{ + float sqa = vector2NormSquared(v); + float length = sqrtf(sqa); + if (length != 0) { + result->x = v->x / length; + result->y = v->y / length; + } else { + result->x = 0; + result->y = 0; + } + return result; +} + + +static inline fpVector2_t* vector2Sub(fpVector2_t* result, const fpVector2_t* a, const fpVector2_t* b) +{ + fpVector2_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + + *result = ab; + return result; +} + +static inline fpVector2_t * vector2Add(fpVector2_t * result, const fpVector2_t * a, const fpVector2_t * b) +{ + fpVector2_t ab; + + ab.x = a->x + b->x; + ab.y = a->y + b->y; + + *result = ab; + return result; +} + + +static inline float vector2DotProduct(const fpVector2_t* a, const fpVector2_t* b) +{ + return a->x * b->x + a->y * b->y; +} + +static inline fpVector2_t * vector2Scale(fpVector2_t * result, const fpVector2_t * a, const float b) +{ + fpVector2_t ab; + + ab.x = a->x * b; + ab.y = a->y * b; + + *result = ab; + return result; +} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b4062201c72..26715ab3cec 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -129,7 +129,10 @@ #define PG_GIMBAL_CONFIG 1039 #define PG_GIMBAL_SERIAL_CONFIG 1040 #define PG_HEADTRACKER_CONFIG 1041 -#define PG_INAV_END PG_HEADTRACKER_CONFIG +#define PG_GEOZONE_CONFIG 1042 +#define PG_GEOZONES 1043 +#define PG_GEOZONE_VERTICES 1044 +#define PG_INAV_END PG_GEOZONE_VERTICES // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ee5cb289a94..87db75cb509 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -152,7 +152,7 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", - "", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", @@ -1561,6 +1561,251 @@ static void cliSafeHomes(char *cmdline) } #endif + +#if defined(USE_GEOZONE) +static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) +{ + const char *format = "geozone %u %u %u %d %d %u"; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultGeoZone) { + equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].shape == defaultGeoZone->shape + && geoZone[i].type == defaultGeoZone->type + && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude + && geoZone[i].minAltitude == defaultGeoZone->minAltitude; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction); + } +} + +static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices) +{ + const char *format = "geozone vertex %d %u %d %d"; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultVertices) { + equalsDefault = vertices[i].idx == defaultVertices->idx + && vertices[i].lat == defaultVertices->lat + && vertices[i].lon == defaultVertices->lon + && vertices[i].zoneId == defaultVertices->zoneId; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon); + } + + if (!defaultVertices) { + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + } +} + +static void cliGeozone(char* cmdLine) +{ + if (isEmpty(cmdLine)) { + printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL); + } else if (sl_strcasecmp(cmdLine, "vertex") == 0) { + printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL); + } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) { + const char* ptr = &cmdLine[12]; + uint8_t zoneId = 0, idx = 0; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + } else { + geozoneResetVertices(-1, -1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + idx = fastA2I(ptr); + } else { + geozoneResetVertices(zoneId, -1); + return; + } + + if (argumentCount != 2) { + cliShowParseError(); + return; + } + + geozoneResetVertices(zoneId, idx); + + } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) { + int32_t lat = 0, lon = 0; + int8_t zoneId = 0; + int16_t vertexIdx = -1; + uint8_t vertexZoneIdx = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + if (zoneId < 0) { + return; + } + + if (zoneId >= MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + vertexZoneIdx = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lat = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + } + + if (argumentCount != 4) { + cliShowParseError(); + return; + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx) { + geoZoneVerticesMutable(i)->lat = lat; + geoZoneVerticesMutable(i)->lon = lon; + return; + } + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexIdx = i; + break; + } + } + + if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) { + cliPrintError("Maximum number of vertices reached."); + return; + } + + geoZoneVerticesMutable(vertexIdx)->lat = lat; + geoZoneVerticesMutable(vertexIdx)->lon = lon; + geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; + geoZonesConfigMutable(zoneId)->vertexCount++; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0; + int32_t minAltitude = 0, maxAltitude = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + idx = fastA2I(ptr); + if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + isPolygon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + isInclusive = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + minAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + maxAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + fenceAction = fastA2I(ptr); + if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) { + cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + } + + if (argumentCount != 6) { + cliShowParseError(); + return; + } + + if (isPolygon) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON; + } else { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + } + + if (isInclusive) { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE; + } else { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + } + + geoZonesConfigMutable(idx)->maxAltitude = maxAltitude; + geoZonesConfigMutable(idx)->minAltitude = minAltitude; + geoZonesConfigMutable(idx)->fenceAction = fenceAction; + } +} +#endif + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) { @@ -4167,6 +4412,14 @@ static void printConfig(const char *cmdline, bool doDiff) printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0)); #endif +#if defined(USE_GEOZONE) + cliPrintHashLine("geozone"); + printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0)); + + cliPrintHashLine("geozone vertices"); + printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0)); +#endif + cliPrintHashLine("features"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -4550,6 +4803,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), +#ifdef USE_GEOZONE + CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone), +#endif #ifdef USE_GPS CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites), diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ddb6a826b83..17c8ded8c1e 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -36,7 +36,7 @@ typedef enum { FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, - FEATURE_UNUSED_12 = 1 << 4, //was FEATURE_MOTOR_STOP + FEATURE_GEOZONE = 1 << 4, //was FEATURE_MOTOR_STOP FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f7603eee692..ba29a03d5e0 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ static void updateArmingStatus(void) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + if (geozoneIsInsideNFZ()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e7a7a4aa3a6..201f2b11bfc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1768,6 +1768,53 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + sbufWriteU8(dst, idx); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + +static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t zoneId = sbufReadU8(src); + const uint8_t vertexId = sbufReadU8(src); + if (zoneId < MAX_GEOZONES_IN_CONFIG) { + int8_t vertexIdx = geozoneGetVertexIdx(zoneId, vertexId); + if (vertexIdx >= 0) { + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId); + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon); + if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1); + if (vertexRadiusIdx >= 0) { + sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat); + } else { + return MSP_RESULT_ERROR; + } + } + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } +} +#endif + static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); if (idx < MAX_LOGIC_CONDITIONS) { @@ -3301,6 +3348,49 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 13) { + uint8_t geozoneId; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + + geozoneResetVertices(geozoneId, -1); + geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); + } else { + return MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_GEOZONE_VERTEX: + if (dataSize == 10 || dataSize == 14) { + uint8_t geozoneId = 0; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + uint8_t vertexId = sbufReadU8(src); + int32_t lat = sbufReadU32(src); + int32_t lon = sbufReadU32(src); + if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) { + return MSP_RESULT_ERROR; + } + + if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) { + return MSP_RESULT_ERROR; + } + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + #ifdef USE_EZ_TUNE case MSP2_INAV_EZ_TUNE_SET: @@ -3862,6 +3952,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFwApproachOutCommand(dst, src); break; #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_GEOZONE: + *ret = mspFcGeozoneOutCommand(dst, src); + break; + case MSP2_INAV_GEOZONE_VERTEX: + *ret = mspFcGeozoneVerteciesOutCommand(dst, src); + break; +#endif #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..df335ccb428 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -339,6 +339,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#if defined(USE_GEOZONE) && defined (USE_GPS) +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -453,6 +462,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#if defined(USE_GEOZONE) && defined (USE_GPS) + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -740,4 +754,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4d..637ea867e64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -24,6 +24,7 @@ typedef enum { SIMULATOR_MODE_HITL = (1 << 4), SIMULATOR_MODE_SITL = (1 << 5), + ARMING_DISABLED_GEOZONE = (1 << 6), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), @@ -49,8 +50,8 @@ typedef enum { ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_LANDING_DETECTED = (1 << 30), - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | - ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | @@ -65,7 +66,8 @@ typedef enum { // situations where we might just need the motors to spin so the // aircraft can move (even unpredictably) and get unstuck (e.g. // crashed into a high tree). -#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \ +#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_GEOZONE \ + | ARMING_DISABLED_NOT_LEVEL \ | ARMING_DISABLED_NAVIGATION_UNSAFE \ | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ @@ -106,6 +108,7 @@ typedef enum { SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), NAV_FW_AUTOLAND = (1 << 18), + NAV_SEND_TO = (1 << 19), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74f525ce5f1..8cc9b1232dd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -198,6 +198,11 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: fence_action + values: ["NONE", "AVOID", "POS_HOLD", "RTH"] + enum: fenceAction_e + - name: geozone_rth_no_way_home + values: [RTH, EMRG_LAND] constants: RPYL_PID_MIN: 0 @@ -3776,6 +3781,7 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source + condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource @@ -4314,3 +4320,49 @@ groups: field: roll_ratio min: 0 max: 5 + - name: PG_GEOZONE_CONFIG + type: geozone_config_t + headers: ["navigation/navigation.h"] + condition: USE_GEOZONE && USE_GPS + members: + - name: geozone_detection_distance + description: "Distance from which a geozone is detected" + default_value: 50000 + field: fenceDetectionDistance + min: 0 + max: 1000000 + - name: geozone_avoid_altitude_range + description: "Altitude range in which an attempt is made to avoid a geozone upwards" + default_value: 5000 + field: avoidAltitudeRange + min: 0 + max: 1000000 + - name: geozone_safe_altitude_distance + description: "Vertical distance that must be maintained to the upper and lower limits of the zone." + default_value: 1000 + field: safeAltitudeDistance + min: 0 + max: 10000 + - name: geozone_safehome_as_inclusive + description: "Treat nearest safehome as inclusive geozone" + type: bool + field: nearestSafeHomeAsInclusivZone + default_value: OFF + - name: geozone_safehome_zone_action + description: "Fence action for safehome zone" + default_value: "NONE" + field: safeHomeFenceAction + table: fence_action + type: uint8_t + - name: geozone_mr_stop_distance + description: "Distance in which multirotors stops before the border" + default_value: 15000 + min: 0 + max: 100000 + field: copterFenceStopDistance + - name: geozone_no_way_home_action + description: "Action if RTH with active geozones is unable to calculate a course to home" + default_value: RTH + field: noWayHomeAction + table: geozone_rth_no_way_home + type: uint8_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fff22479f06..efcfa394f65 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,6 +866,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + case ARMING_DISABLED_GEOZONE: + return OSD_MESSAGE_STR(OSD_MSG_NFZ); // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -2336,6 +2338,11 @@ static bool osdDrawSingleElement(uint8_t item) if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else +#endif +#ifdef USE_GEOZONE + if (FLIGHT_MODE(NAV_SEND_TO)) + p = "AUTO"; + else #endif if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; @@ -3797,6 +3804,52 @@ static bool osdDrawSingleElement(uint8_t item) clearMultiFunction = true; break; } +#if defined(USE_GEOZONE) + case OSD_COURSE_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction); + } else { + if (isGeozoneActive()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr); + } + break; + } + + case OSD_H_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0); + tfp_sprintf(buff, "FD %s", buff2 ); + } else { + strcpy(buff, "FD ---"); + } + } + break; + + case OSD_V_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone)); + tfp_sprintf(buff, "FD%s", buff2); + displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr); + } else { + strcpy(buff, "FD ---"); + } + + break; + } +#endif default: return false; @@ -4998,7 +5051,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX) uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX) { char buff[12]; - char outBuff[12]; + char outBuff[20]; const float max_gforce = accGetMeasuredMaxG(); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); @@ -5784,6 +5837,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = safehomeMessage; } #endif + +#ifdef USE_GEOZONE + if (geozone.avoidInRTHInProgress) { + messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH; + } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); @@ -5846,6 +5905,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_LOITER: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index a73db015793..fd0f4028bb5 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -132,6 +132,19 @@ #define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif +#if defined(USE_GEOZONE) +#define OSD_MSG_NFZ "NO FLY ZONE" +#define OSD_MSG_LEAVING_FZ "LEAVING FZ IN %s" +#define OSD_MSG_OUTSIDE_FZ "OUTSIDE FZ" +#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s" +#define OSD_MSG_AVOIDING_FB "AVOIDING FENCE BREACH" +#define OSD_MSG_RETURN_TO_ZONE "RETURN TO FZ" +#define OSD_MSG_FLYOUT_NFZ "FLY OUT NFZ" +#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT" +#define OSD_MSG_AVOID_ZONES_RTH "AVOIDING NO FLY ZONES" +#define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -287,6 +300,9 @@ typedef enum { OSD_ADSB_INFO, OSD_BLACKBOX, OSD_FORMATION_FLIGHT, //153 + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -467,6 +483,10 @@ typedef struct osdConfig_s { uint16_t adsb_ignore_plane_above_me_limit; // in metres #endif uint8_t radar_peers_display_time; // in seconds +#ifdef USE_GEOZONE + uint8_t geozoneDistanceWarning; // Distance to fence or action + bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b018d8f9068..c0120c7faa1 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -525,6 +525,8 @@ static char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message + case ARMING_DISABLED_GEOZONE: + return OSD_MESSAGE_STR("NO FLY ZONE"); case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; case ARMING_DISABLED_CMS_MENU: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 8b4a09e87af..b8c808cb904 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -112,4 +112,9 @@ #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2101 #define MSP2_INAV_SERVO_CONFIG 0x2200 -#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file +#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 + +#define MSP2_INAV_GEOZONE 0x2210 +#define MSP2_INAV_SET_GEOZONE 0x2211 +#define MSP2_INAV_GEOZONE_VERTEX 0x2212 +#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4ae4cf228db..bbf8e7910f0 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState); #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState); +#endif static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, /** CRUISE_HOLD mode ************************************************/ @@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -1178,6 +1189,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, #endif + +#ifdef USE_GEOZONE + [NAV_STATE_SEND_TO_INITALIZE] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_SEND_TO_IN_PROGESS] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, + [NAV_STATE_SEND_TO_FINISHED] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_FINISHED, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, +#endif }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1444,6 +1512,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // If we have valid position sensor or configured to ignore it's loss at initial stage - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) { // Prepare controllers +#ifdef USE_GEOZONE + geozoneResetRTH(); + geozoneSetupRTH(); +#endif resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL setupAltitudeController(); @@ -1610,6 +1682,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { +#ifdef USE_GEOZONE + // Check for NFZ in our way + int8_t wpCount = geozoneCheckForNFZAtCourse(true); + if (wpCount > 0) { + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + return NAV_FSM_EVENT_NONE; + } else if (geozone.avoidInRTHInProgress) { + if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) { + if (geoZoneIsLastRthWaypoint()) { + // Already at Home? + fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_SUCCESS; + } + + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; + } else { + geozoneAdvanceRthAvoidWaypoint(); + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + return NAV_FSM_EVENT_NONE; + } + } + setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + return NAV_FSM_EVENT_NONE; + } else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } else { +#endif fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { @@ -1626,6 +1728,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); return NAV_FSM_EVENT_NONE; } +#ifdef USE_GEOZONE + } +#endif } /* Position sensor failure timeout - land */ else if (checkForPositionSensorTimeout()) { @@ -1797,6 +1902,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } +#ifdef USE_GEOZONE + geozoneResetRTH(); +#endif + // Prevent I-terms growing when already landed pidResetErrorAccumulators(); return NAV_FSM_EVENT_NONE; @@ -2452,6 +2561,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga } #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + resetPositionController(); + resetAltitudeController(false); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + posControl.sendTo.startTime = millis(); + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately + if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) { + posControl.sendTo.lockSticks = false; + } + + if (!posControl.sendTo.lockSticks && areSticksDeflected()) { + abortSendTo(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) { + if (posControl.sendTo.altitudeTargetRange > 0) { + if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) { + return NAV_FSM_EVENT_SUCCESS; + } else { + return NAV_FSM_EVENT_NONE; + } + } + return NAV_FSM_EVENT_SUCCESS; + } + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState) +{ + UNUSED(previousState); + posControl.sendTo.lockSticks = false; + posControl.flags.sendToActive = false; + return NAV_FSM_EVENT_NONE; +} +#endif + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2539,6 +2706,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) switch (mode) { case RTH_HOME_ENROUTE_INITIAL: posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; +#ifdef USE_GEOZONE + if (geozone.currentzoneMaxAltitude > 0) { + posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z); + } +#endif break; case RTH_HOME_ENROUTE_PROPORTIONAL: @@ -2552,6 +2724,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; } } +#ifdef USE_GEOZONE + if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance; + } +#endif break; case RTH_HOME_ENROUTE_FINAL: @@ -2757,6 +2934,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs; } +/*----------------------------------------------------------- + * Calculates 2D distance between two points + *-----------------------------------------------------------*/ +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos) +{ + const float deltaX = destinationPos->x - startPos->x; + const float deltaY = destinationPos->y - startPos->y; + + return calc_length_pythagorean_2D(deltaX, deltaY); +} + /*----------------------------------------------------------- * Calculates distance and bearing to destination point *-----------------------------------------------------------*/ @@ -2944,6 +3132,11 @@ static void updateDesiredRTHAltitude(void) posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } +#if defined(USE_GEOZONE) + if (geozone.homeHasMaxAltitue) { + posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude); + } +#endif } /*----------------------------------------------------------- @@ -3154,6 +3347,9 @@ void updateHomePosition(void) setHome = true; break; } +#if defined(USE_GEOZONE) && defined (USE_GPS) + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3409,7 +3605,14 @@ bool isProbablyStillFlying(void) * Z-position controller *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) -{ +{ +#ifdef USE_GEOZONE + if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + return 0.0f; + } +#endif + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; @@ -4095,6 +4298,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif return; } @@ -4145,7 +4352,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4296,6 +4503,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4769,6 +4984,14 @@ void navigationInit(void) #ifdef USE_MULTI_MISSION posControl.multiMissionCount = 0; #endif + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; + posControl.sendTo.lockSticks = false; + posControl.sendTo.lockStickTime = 0; + posControl.sendTo.startTime = 0; + posControl.sendTo.targetRange = 0; +#endif /* Set initial surface invalid */ posControl.actualState.surfaceMin = -1.0f; @@ -4851,6 +5074,40 @@ rthState_e getStateOfForcedRTH(void) } } + +#ifdef USE_GEOZONE +// "Send to" is not to intended to be activated by user, only by external event +void activateSendTo(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.sendToActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortSendTo(void) +{ + posControl.flags.sendToActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void activateForcedPosHold(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortForcedPosHold(void) +{ + posControl.flags.forcedPosholdActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} +#endif + /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..6e2ca8fbcbc 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,119 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 + +typedef enum { + GEOZONE_MESSAGE_STATE_NONE, + GEOZONE_MESSAGE_STATE_NFZ, + GEOZONE_MESSAGE_STATE_LEAVING_FZ, + GEOZONE_MESSAGE_STATE_OUTSIDE_FZ, + GEOZONE_MESSAGE_STATE_ENTERING_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_FB, + GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, + GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, + GEOZONE_MESSAGE_STATE_LOITER +} geozoneMessageState_e; + +enum fenceAction_e { + GEOFENCE_ACTION_NONE, + GEOFENCE_ACTION_AVOID, + GEOFENCE_ACTION_POS_HOLD, + GEOFENCE_ACTION_RTH, +}; + +enum noWayHomeAction { + NO_WAY_HOME_ACTION_RTH, + NO_WAY_HOME_ACTION_EMRG_LAND, +}; + +#define GEOZONE_SHAPE_CIRCULAR 0 +#define GEOZONE_SHAPE_POLYGON 1 + +#define GEOZONE_TYPE_EXCLUSIVE 0 +#define GEOZONE_TYPE_INCLUSIVE 1 + +typedef struct geoZoneConfig_s +{ + uint8_t shape; + uint8_t type; + int32_t minAltitude; + int32_t maxAltitude; + uint8_t fenceAction; + uint8_t vertexCount; +} geoZoneConfig_t; + +typedef struct geozone_config_s +{ + uint32_t fenceDetectionDistance; + uint16_t avoidAltitudeRange; + uint16_t safeAltitudeDistance; + bool nearestSafeHomeAsInclusivZone; + uint8_t safeHomeFenceAction; + uint32_t copterFenceStopDistance; + uint8_t noWayHomeAction; +} geozone_config_t; + +typedef struct vertexConfig_s +{ + int8_t zoneId; + uint8_t idx; + int32_t lat; + int32_t lon; +} vertexConfig_t; + +PG_DECLARE(geozone_config_t, geoZoneConfig); +PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig); +PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices); + +typedef struct geozone_s { + bool insideFz; + bool insideNfz; + uint32_t distanceToZoneBorder3d; + int32_t vertDistanceToZoneBorder; + geozoneMessageState_e messageState; + int32_t directionToNearestZone; + int32_t distanceHorToNearestZone; + int32_t distanceVertToNearestZone; + int32_t zoneInfo; + int32_t currentzoneMaxAltitude; + int32_t currentzoneMinAltitude; + bool sticksLocked; + int8_t loiterDir; + bool avoidInRTHInProgress; + int32_t maxHomeAltitude; + bool homeHasMaxAltitue; +} geozone_t; + +extern geozone_t geozone; + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon); +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId); +bool isGeozoneActive(void); +uint8_t geozoneGetUsedVerticesCount(void); +void geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsInsideNFZ(void); +void geozoneAdvanceRthAvoidWaypoint(void); +int8_t geozoneCheckForNFZAtCourse(bool isRTH); +bool geoZoneIsLastRthWaypoint(void); +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void); +void geozoneSetupRTH(void); +void geozoneResetRTH(void); +void geozoneUpdateMaxHomeAltitude(void); +uint32_t geozoneGetDetectionDistance(void); + +void activateSendTo(void); +void abortSendTo(void); +void activateForcedPosHold(void); +void abortForcedPosHold(void); + +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ff38b2e17e5..87165cd7d6f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -250,6 +250,12 @@ static int8_t loiterDirection(void) { dir *= -1; } +#ifdef USE_GEOZONE + if (geozone.loiterDir != 0) { + dir = geozone.loiterDir; + } +#endif + return dir; } diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c new file mode 100755 index 00000000000..8a2505a0ff9 --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,1598 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "common/utils.h" +#include "common/vector.h" +#include "common/printf.h" + +#include "config/config_reset.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#if defined(USE_GEOZONE) && defined (USE_GPS) + +#include "navigation_geozone_calculations.h" + +#define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) +#define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home + +#define MAX_DISTANCE_FLY_OVER_POINTS 50000 +#define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) +#define POS_DETECTION_DISTANCE 7500 +#define STICK_LOCK_MIN_TIME 1500 +#define AVOID_TIMEOUT 30000 +#define MAX_LOCAL_VERTICES 128 +#define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m +#define STICK_MOVE_THRESHOULD 40 +#define MAX_RTH_WAYPOINTS (MAX_VERTICES / 2) +#define GEOZONE_INACTIVE INT8_MAX +#define RTH_OVERRIDE_TIMEOUT 1000 + +#define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) + +typedef enum { + GEOZONE_ACTION_STATE_NONE, + GEOZONE_ACTION_STATE_AVOIDING, + GEOZONE_ACTION_STATE_AVOIDING_UPWARD, + GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE, + GEOZONE_ACTION_STATE_RETURN_TO_FZ, + GEOZONE_ACTION_STATE_FLYOUT_NFZ, + GEOZONE_ACTION_STATE_POSHOLD, + GEOZONE_ACTION_STATE_RTH +} geozoneActionState_e; + +typedef struct geoZoneRuntimeConfig_s +{ + geoZoneConfig_t config; + bool enable; + bool isInfZone; + uint32_t radius; + fpVector2_t *verticesLocal; +} geoZoneRuntimeConfig_t; + +typedef struct pathPoint_s pathPoint_t; +struct pathPoint_s { + fpVector3_t point; + pathPoint_t* prev; + float distance; + bool visited; +}; + +static bool isInitalised = false; +static geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; +static fpVector2_t verticesLocal[MAX_VERTICES]; +static uint8_t currentZoneCount = 0; + +static bool isAtLeastOneInclusiveZoneActive = false; +static geoZoneRuntimeConfig_t activeGeoZones[MAX_GEOZONES]; +static uint8_t activeGeoZonesCount = 0; +static geoZoneConfig_t safeHomeGeozoneConfig; +static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; +static timeMs_t actionStartTime = 0; +static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; +static fpVector3_t avoidingPoint; +static bool geozoneIsEnabled = false; +static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS]; +static uint8_t rthWaypointIndex = 0; +static int8_t rthWaypointCount = 0; +static bool aboveOrUnderZone = false; +static timeMs_t rthOverrideStartTime; +static bool noZoneRTH = false; +static bool rthHomeSwitchLastState = false; +static bool lockRTZ = false; + +geozone_t geozone; + +PG_REGISTER_WITH_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, PG_GEOZONE_CONFIG, 0); + +PG_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, + .fenceDetectionDistance = SETTING_GEOZONE_DETECTION_DISTANCE_DEFAULT, + .avoidAltitudeRange = SETTING_GEOZONE_AVOID_ALTITUDE_RANGE_DEFAULT, + .safeAltitudeDistance = SETTING_GEOZONE_SAFE_ALTITUDE_DISTANCE_DEFAULT, + .nearestSafeHomeAsInclusivZone = SETTING_GEOZONE_SAFEHOME_AS_INCLUSIVE_DEFAULT, + .copterFenceStopDistance = SETTING_GEOZONE_MR_STOP_DISTANCE_DEFAULT, + .noWayHomeAction = SETTING_GEOZONE_NO_WAY_HOME_ACTION_DEFAULT +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig, PG_GEOZONES, 1); + +void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) +{ + for (int i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + RESET_CONFIG(geoZoneConfig_t, &instance[i], + .shape = GEOZONE_TYPE_EXCLUSIVE, + .type = GEOZONE_SHAPE_CIRCULAR, + .minAltitude = 0, + .maxAltitude = 0, + .fenceAction = GEOFENCE_ACTION_NONE, + .vertexCount = 0 + ); + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices, PG_GEOZONE_VERTICES, 0); + +void pgResetFn_geoZoneVertices(vertexConfig_t *instance) +{ + for (int i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + RESET_CONFIG(vertexConfig_t, &instance[i], + .zoneId = -1, + .idx = 0, + .lat = 0, + .lon = 0 + ); + } +} + +uint8_t geozoneGetUsedVerticesCount(void) +{ + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + count += geoZonesConfig(i)->vertexCount; + } + return count; +} + +void geozoneResetVertices(const int8_t zoneId, const int16_t idx) +{ + if (zoneId < 0 && idx < 0) { + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + } + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx < 0) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx >= 0 && idx < MAX_VERTICES_IN_CONFIG) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == idx) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + break; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount--; + } + } +} + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon) +{ + if (vertexId < MAX_VERTICES_IN_CONFIG) + { + int16_t vertexConfigIdx = -1; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexConfigIdx = i; + break; + } + } + if (vertexConfigIdx >= 0) { + geoZoneVerticesMutable(vertexConfigIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexConfigIdx)->idx = vertexId; + geoZoneVerticesMutable(vertexConfigIdx)->lat = lat; + geoZoneVerticesMutable(vertexConfigIdx)->lon = lon; + return true; + } + } + return false; +} + +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) +{ + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexId) { + return i; + } + } + return -1; +} + +static bool areSticksDeflectdFromChannelValue(void) +{ + return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; +} + +static bool isNonGeozoneModeFromBoxInput(void) +{ + return !(IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)); +} + +static bool isPointOnBorder(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos) +{ + fpVector2_t *prev = &zone->verticesLocal[zone->config.vertexCount -1]; + fpVector2_t *current; + bool isOnBorder = false; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + current = &zone->verticesLocal[i]; + if (isPointOnLine2(prev, current, (fpVector2_t*)pos)) { + isOnBorder = true; + break; + } + prev = current; + } + + if (isOnBorder) { + return (pos->z >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos->z <= zone->config.maxAltitude; + } + + return isOnBorder; +} + +static bool isInZoneAltitudeRange(geoZoneRuntimeConfig_t *zone, const float pos) +{ + return (pos >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos <= zone->config.maxAltitude; +} + +static bool isInGeozone(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos, bool ignoreAltitude) +{ + if (activeGeoZonesCount == 0) { + return false; + } + + bool isIn2D = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + isIn2D = isPointInPloygon((fpVector2_t*)pos, zone->verticesLocal, zone->config.vertexCount) || isPointOnBorder(zone, pos); + } else { // cylindric + isIn2D = isPointInCircle((fpVector2_t*)pos, &zone->verticesLocal[0], zone->radius); + } + + if (isIn2D && !ignoreAltitude) { + return isInZoneAltitudeRange(zone, pos->z); + } + + return isIn2D; +} + +static bool isPointInAnyOtherZone(const geoZoneRuntimeConfig_t *zone, uint8_t type, const fpVector3_t *pos) +{ + bool isInZone = false; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (zone != &activeGeoZones[i] && activeGeoZones[i].config.type == type && isInGeozone(&activeGeoZones[i], pos, false)) { + isInZone = true; + break; + } + } + return isInZone; +} + +static uint8_t getZonesForPos(geoZoneRuntimeConfig_t *zones[], const fpVector3_t *pos, const bool ignoreAltitude) +{ + uint8_t count = 0; + for (int i = 0; i < activeGeoZonesCount; i++) { + if (isInGeozone(&activeGeoZones[i], pos, ignoreAltitude)) { + zones[count++] = &activeGeoZones[i]; + } + } + return count; +} + +static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignoreAltitude) +{ + return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); +} + +static int geoZoneRTComp(const void *a, const void *b) +{ + geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; + geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; + + if (zoneA->enable == zoneB->enable) { + return 0; + } else if (zoneA->enable) { + return -1; + } else { + return 1; + } +} + +// in cm and cms/s +static uint32_t calcTime(const int32_t distance, const int32_t speed) +{ + if (speed <= 0) { + return 0; + } + + return distance / speed; +} + +static void calcPreviewPoint(fpVector3_t *target, const int32_t distance) +{ + calculateFarAwayTarget(target, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), distance); + target->z = getEstimatedActualPosition(Z) + calcTime(geoZoneConfig()->fenceDetectionDistance, gpsSol.groundSpeed) * getEstimatedActualVelocity(Z); +} + +static bool calcIntersectionForZone(fpVector3_t *intersection, float *distance, geoZoneRuntimeConfig_t *zone, const fpVector3_t *start, const fpVector3_t *end) +{ + bool hasIntersection = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + if (calcLine3dPolygonIntersection( + intersection, + distance, + start, + end, + zone->verticesLocal, + zone->config.vertexCount, + zone->config.minAltitude, + zone->config.maxAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + fpVector3_t circleCenter = { .x = zone->verticesLocal[0].x, .y = zone->verticesLocal[0].y, .z = zone->config.minAltitude }; + if (calcLineCylinderIntersection( + intersection, + distance, + start, + end, + &circleCenter, + zone->radius, + zone->config.maxAltitude - zone->config.minAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } + + if (hasIntersection && isPointOnLine3(start, end, intersection)){ + return true; + } + *distance = -1; + return false; +} + +static int32_t calcBouceCoursePolygon(const int32_t course, const fpVector2_t* pos, const fpVector2_t *intersect, const fpVector2_t* p1, const fpVector2_t* p2) +{ + int32_t newCourse = 0; + int32_t angelp1 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p1, pos, intersect)); + int32_t angelp2 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p2, pos, intersect)); + if (angelp1 < angelp2) { + if (isPointRightFromLine(pos, intersect, p1)) { + newCourse = course - 2 * angelp1; + } + else { + newCourse = course + 2 * angelp1; + } + } + else { + if (isPointRightFromLine(pos, intersect, p2)) { + newCourse = course - 2 * angelp2; + } + else { + newCourse = course + 2 * angelp2; + } + } + return wrap_36000(newCourse); +} + +static int32_t calcBouceCourseCircle(const int32_t course, const fpVector2_t* pos, const fpVector2_t* intersect, const fpVector2_t* mid) +{ + int32_t newCourse = 0; + int32_t angel = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(mid, pos, intersect)); + if (isPointRightFromLine(pos, mid, intersect)) { + newCourse = course + 2 * (angel - 9000); + } + else { + newCourse = course - 2 * (angel - 9000); + } + return wrap_36000(newCourse); +} + +static bool findNearestIntersectionZone(geoZoneRuntimeConfig_t **intersectZone, fpVector3_t *intersection, float *distance, const float detectionDistance, const fpVector3_t *start, const fpVector3_t *end) +{ + geoZoneRuntimeConfig_t *zone = NULL; + fpVector3_t intersect; + float distanceToZone = FLT_MAX; + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + float currentDistance = FLT_MAX; + if (!calcIntersectionForZone( + ¤tIntersect, + ¤tDistance, + &activeGeoZones[i], + start, + end)) { + continue; + } + + if (currentDistance < distanceToZone) { + distanceToZone = currentDistance; + zone = &activeGeoZones[i]; + intersect = currentIntersect; + } + } + + if (zone && distanceToZone < detectionDistance) { + *intersectZone = zone; + *distance = distanceToZone; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + + return false; +} + +static bool isPointDirectReachable(const fpVector3_t* start, const fpVector3_t *point) +{ + float currentDistance = 0; + bool pointIsInOverlappingZone = false, pointIsInExclusiveZone = false, hasIntersection = false; + + /* + if (start->x == point->x && start->y == point->y) { + return false; + } + */ + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + + if (!calcIntersectionForZone(¤tIntersect, ¤tDistance, &activeGeoZones[i], start, point)) { + continue; + } + hasIntersection = true; + + // Intersct a exclusive Zone? + geoZoneRuntimeConfig_t *intersectZones[MAX_GEOZONES]; + uint8_t intersectZonesCount = getZonesForPos(intersectZones, ¤tIntersect, false); + for (uint8_t j = 0; j < intersectZonesCount; j++) { + if (intersectZones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE ) { + pointIsInExclusiveZone = true; + break; + } + } + + if (pointIsInExclusiveZone) { + break; + } + + // We targeting a exit point (in min two zones) + if (intersectZonesCount < 2) { + break; + } + + geoZoneRuntimeConfig_t *startZones[MAX_GEOZONES]; + uint8_t startZonesCount = getZonesForPos(startZones, start, false); + geoZoneRuntimeConfig_t *endZones[MAX_GEOZONES]; + uint8_t endZonesCount = getZonesForPos(endZones, point, false); + + for (uint8_t j = 0; j < intersectZonesCount; j++) { + for (uint8_t k = 0; k < startZonesCount; k++) { + for (uint8_t l = 0; l < endZonesCount; l++) { + if (intersectZones[j] == startZones[k] && intersectZones[j] == endZones[l]) { + pointIsInOverlappingZone = true; + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + } + + return !pointIsInExclusiveZone && (!hasIntersection || pointIsInOverlappingZone); +} + +uint32_t geozoneGetDetectionDistance(void) +{ + uint32_t detctionDistance = 0; + if (STATE(AIRPLANE)) { + detctionDistance = navConfig()->fw.loiter_radius * 1.5; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t intersect; + bool found = false; + fpVector2_t *p1 = &zone->verticesLocal[zone->config.vertexCount - 1]; + fpVector2_t *p2; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + p2 = &zone->verticesLocal[i]; + if (calcLineLineIntersection(&intersect, p1, p2, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)prevPoint, true)) { + found = true; + break; + } + p1 = p2; + } + + if (!found) { + return -1; + } + course = calcBouceCoursePolygon(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &intersect, p1, p2); + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + course = calcBouceCourseCircle(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)intersection, &zone->verticesLocal[0]); + } + return course; +} + +static bool initPathPoint(pathPoint_t *pathPoints, const fpVector3_t pos, uint8_t *idx) +{ + if (*idx + 1 >= MAX_PATH_PONITS) { + return false; + } + + pathPoints[*idx].distance = FLT_MAX; + pathPoints[*idx].visited = false; + pathPoints[(*idx)++].point = pos; + + return true; +} + +static bool isPosInGreenAlt(geoZoneRuntimeConfig_t *zones[], const uint8_t zoneCount, const float alt) +{ + bool isInNfz = false, isInFz = false; + for (uint8_t j = 0; j < zoneCount; j++) { + if(isInZoneAltitudeRange(zones[j], alt)){ + isInFz = zones[j]->config.type == GEOZONE_TYPE_INCLUSIVE; + isInNfz = zones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE; + } + } + return !isInNfz && (!isAtLeastOneInclusiveZoneActive || isInFz); +} + +static bool checkPathPointOrSetAlt(fpVector3_t *pos) +{ + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, pos, true); + + if (zoneCount == 0) { + return !isAtLeastOneInclusiveZoneActive; + } + + if (zoneCount == 1 && zones[0]->config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + + bool isInExclusice = false; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(zones[i], pos, false)) { + isInExclusice = true; + if (zones[i]->config.minAltitude != 0) { + float min = zones[i]->config.minAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + if (isPosInGreenAlt(zones, zoneCount, min)) { + pos->z = min; + return true; + } + } + + if (!zones[i]->isInfZone || zones[i]->config.maxAltitude < INT32_MAX) { + float max = zones[i]->config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + if(isPosInGreenAlt(zones, zoneCount, max)) { + pos->z = max; + return true; + } + } + } + } + + return !isInExclusice; +} + +// Return value: 0 - Target direct reachable; -1 No way; >= 1 Waypoints to target +#define CIRCLE_POLY_SIDES 6 +static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fpVector3_t* target) +{ + pathPoint_t pathPoints[MAX_PATH_PONITS]; + uint8_t pathPointCount = 0, waypointCount = 0; + fpVector3_t start = *point; + + if (isPointDirectReachable(&start, target)) { + return 0; + } + + float offset = geozoneGetDetectionDistance(); + if (geozone.distanceVertToNearestZone <= offset) { + int bearing = wrap_36000(geozone.directionToNearestZone + 18000); + start.x += offset * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + start.y += offset * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + } + + pathPoints[pathPointCount].visited = true; + pathPoints[pathPointCount].distance = 0; + pathPoints[pathPointCount++].point = start; + + for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { + fpVector2_t *verticesZone; + fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; + uint8_t verticesZoneCount; + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + verticesZone = activeGeoZones[i].verticesLocal; + verticesZoneCount = activeGeoZones[i].config.vertexCount; + } else { + generatePolygonFromCircle(verticesCirclePoly, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius, CIRCLE_POLY_SIDES); + verticesZone = verticesCirclePoly; + verticesZoneCount = CIRCLE_POLY_SIDES; + } + + fpVector2_t safeZone[MAX_VERTICES]; + offset = geozoneGetDetectionDistance() * 2 / 3; + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + offset *= -1; + } + + float zMin = start.z, zMax = 0; + if (!isInZoneAltitudeRange(&activeGeoZones[i], start.z) && activeGeoZones[i].config.minAltitude > 0) { + zMin = activeGeoZones[i].config.minAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + } else if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + generateOffsetPolygon(safeZone, verticesZone, verticesZoneCount, offset); + fpVector2_t *prev = &safeZone[verticesZoneCount - 1]; + fpVector2_t *current; + for (uint8_t j = 0; j < verticesZoneCount; j++) { + current = &safeZone[j]; + + if (zMax > 0 ) { + fpVector3_t max = { .x = current->x, .y = current->y, .z = zMax }; + if (checkPathPointOrSetAlt(&max)) { + if (!initPathPoint(pathPoints, max, &pathPointCount)) { + return -1; + } + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + // Set some "fly over points" + float dist = calculateDistance2(prev, current); + if (dist > MAX_DISTANCE_FLY_OVER_POINTS) { + uint8_t sectionCount = (uint8_t)(dist / MAX_DISTANCE_FLY_OVER_POINTS); + float dist = MAX_DISTANCE_FLY_OVER_POINTS; + for (uint8_t k = 0; k < sectionCount; k++) { + fpVector3_t flyOverPoint; + calcPointOnLine((fpVector2_t*)&flyOverPoint, prev, current, dist); + fpVector3_t maxFo = { .x = flyOverPoint.x, .y = flyOverPoint.y, .z = zMax }; + if (checkPathPointOrSetAlt(&maxFo)) { + if (!initPathPoint(pathPoints, maxFo, &pathPointCount)) { + return -1; + } + } + dist += MAX_DISTANCE_FLY_OVER_POINTS; + } + } + } + } + + if (zMin > 0) { + fpVector3_t min = { .x = current->x, .y = current->y, .z = zMin }; + if (checkPathPointOrSetAlt(&min)) { + if (!initPathPoint(pathPoints, min, &pathPointCount)) { + return -1; + } + } + + } + prev = current; + } + } + + if (!initPathPoint(pathPoints, *target, &pathPointCount)) { + return -1; + } + + // Dijkstra + pathPoint_t *current = pathPoints; + while (!pathPoints[pathPointCount - 1].visited) { + pathPoint_t *next = current; + float min = FLT_MAX; + for (uint16_t i = 1; i < pathPointCount; i++) { + + float currentDist = FLT_MAX; + if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { + float dist2D = calculateDistance2((fpVector2_t*)¤t->point, (fpVector2_t*)&pathPoints[i].point); + float distAlt = ABS(current->point.z - pathPoints[i].point.z); + currentDist = current->distance + dist2D + 2 * distAlt; + } + + if (currentDist < pathPoints[i].distance && !pathPoints[i].visited) { + pathPoints[i].distance = currentDist; + pathPoints[i].prev = current; + } + if (min > pathPoints[i].distance && !pathPoints[i].visited) { + min = pathPoints[i].distance; + next = &pathPoints[i]; + } + } + + if (min == FLT_MAX) { + return -1; + } + + current = next; + current->visited = true; + } + + waypointCount = 0; + current = &pathPoints[pathPointCount - 1]; + while (current != pathPoints) { + waypointCount++; + current = current->prev; + } + // Don't set home to the WP list + current = pathPoints[pathPointCount - 1].prev; + uint8_t i = waypointCount - 2; + while (current != pathPoints) { + waypoints[i] = current->point; + current = current->prev; + i--; + } + return waypointCount - 1; +} + +static void updateCurrentZones(void) +{ + currentZoneCount = getCurrentZones(currentZones, false); + geozone.insideNfz = false; + geozone.insideFz = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE) { + geozone.insideNfz = true; + } + if (currentZones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + geozone.insideFz = true; + } + } +} + +static void updateZoneInfos(void) +{ + float nearestDistanceToBorder = FLT_MAX; + fpVector3_t nearestBorderPoint; + aboveOrUnderZone = false; + + geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; + uint8_t currentZoneCount = getCurrentZones(currentZones, true); + int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; + + if (currentZoneCount == 1) { + currentMaxAltitude = currentZones[0]->config.maxAltitude; + currentMinAltitude = currentZones[0]->config.minAltitude; + + if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { + nearestHorInclZone = currentZones[0]; + } + + } else if (currentZoneCount >= 2) { + + geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; + float distAbove = FLT_MAX, distBelow = FLT_MAX; + geoZoneRuntimeConfig_t *current = NULL; + for (uint8_t i = 0; i < currentZoneCount; i++) { + current = currentZones[i]; + + if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { + currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); + currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + } + + if (current->config.minAltitude > getEstimatedActualPosition(Z)) { + float dist = current->config.maxAltitude - getEstimatedActualPosition(Z); + if (dist < distAbove) { + aboveZone = current; + distAbove = dist; + } + } + + if (current->config.maxAltitude < getEstimatedActualPosition(Z)) { + float dist = getEstimatedActualPosition(Z) - current->config.maxAltitude; + if (dist < distBelow) { + belowZone = current; + distBelow = dist; + } + } + } + + if (aboveZone) { + if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); + nearestHorInclZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorInclZone = belowZone; + } else { + currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); + } + } + } + + if (currentMinAltitude == INT32_MAX) { + currentMinAltitude = 0; + } + + if (currentMaxAltitude == INT32_MIN) { + currentMaxAltitude = 0; + } + + if (currentMaxAltitude == INT32_MAX && currentMinAltitude != 0) { + geozone.distanceVertToNearestZone = ABS(currentMinAltitude - getEstimatedActualPosition(Z)); + } else if (currentMinAltitude == 0 && currentMaxAltitude != 0) { + geozone.distanceVertToNearestZone = currentMaxAltitude - getEstimatedActualPosition(Z); + } else if (currentMinAltitude != 0 && currentMaxAltitude > 0) { + int32_t distToMin = currentMinAltitude - getEstimatedActualPosition(Z); + int32_t distToMax = currentMaxAltitude - getEstimatedActualPosition(Z); + if (getEstimatedActualPosition(Z) > currentMinAltitude && getEstimatedActualPosition(Z) < currentMaxAltitude) { + if (ABS(distToMin) < ABS(currentMaxAltitude - currentMinAltitude) / 2 ) { + geozone.distanceVertToNearestZone = distToMin; + } else { + geozone.distanceVertToNearestZone = distToMax; + } + } else { + geozone.distanceVertToNearestZone = MIN(ABS(distToMin), distToMax); + } + } else { + geozone.distanceVertToNearestZone = 0; + } + + if (currentZoneCount == 0) { + geozone.currentzoneMaxAltitude = 0; + geozone.currentzoneMinAltitude = 0; + } else { + + if (getEstimatedActualPosition(Z) < currentMinAltitude || getEstimatedActualPosition(Z) > currentMaxAltitude) { + aboveOrUnderZone = true; + } + + if (currentMaxAltitude > 0) { + geozone.currentzoneMaxAltitude = currentMaxAltitude - geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMaxAltitude = 0; + } + + if (currentMinAltitude > 0) { + geozone.currentzoneMinAltitude = currentMinAltitude + geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMinAltitude = 0; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) + { + // Ignore NFZ for now, we want back fo the FZ, we will check for NFZ later at RTH + if (currentZoneCount == 0 && isAtLeastOneInclusiveZoneActive && activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + continue; + } + + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t* prev = &activeGeoZones[i].verticesLocal[activeGeoZones[i].config.vertexCount - 1]; + fpVector2_t* current = NULL; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + current = &activeGeoZones[i].verticesLocal[j]; + fpVector2_t pos = calcNearestPointOnLine(prev, current, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos); + float dist = calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &pos); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + nearestBorderPoint.x = pos.x; + nearestBorderPoint.y = pos.y; + nearestBorderPoint.z = getEstimatedActualPosition(Z); + geozone.directionToNearestZone = calculateBearingToDestination(&nearestBorderPoint); + geozone.distanceHorToNearestZone = roundf(nearestDistanceToBorder); + nearestInclusiveZone = &activeGeoZones[i]; + } + prev = current; + } + } else if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + float dist = fabsf(calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0]) - activeGeoZones[i].radius); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + int32_t directionToBorder = calculateBearingToDestination((fpVector3_t*)&activeGeoZones[i].verticesLocal[0]); + + if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { + directionToBorder = wrap_36000(directionToBorder + 18000); + } + + geozone.directionToNearestZone = directionToBorder; + geozone.distanceHorToNearestZone = roundf(dist); + nearestInclusiveZone = &activeGeoZones[i]; + } + } + + if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorInclZone; + geozone.distanceHorToNearestZone = 0; + } + } +} + +void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) +{ + // Actions only for assisted modes now + if (isNonGeozoneModeFromBoxInput() || geozone.avoidInRTHInProgress || (calculateDistanceToDestination(intersection) > geozoneGetDetectionDistance())) { + return; + } + + int32_t alt = roundf(intersection->z); + if (alt == zone->config.maxAltitude || alt == zone->config.minAltitude) { + return; + } + + fpVector3_t prevPoint; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + + if (zone->config.fenceAction == GEOFENCE_ACTION_AVOID) { + bool avoidFzStep = false; + float fzStepAlt = 0; + if (zone->config.type == GEOZONE_TYPE_INCLUSIVE) { + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES_IN_CONFIG]; + uint8_t zoneCount = getZonesForPos(zones, intersection, true); + + float maxAlt = FLT_MAX; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE && zones[i]->config.minAltitude > intersection->z) { + float alt = zones[i]->config.minAltitude; + if (alt < maxAlt) { + maxAlt = alt; + } + } + } + + if (maxAlt < FLT_MAX) { + fzStepAlt = maxAlt + geoZoneConfig()->safeAltitudeDistance * 2; + avoidFzStep = true; + } + } + // We can move upwards + if ((zone->config.type == GEOZONE_TYPE_EXCLUSIVE && geozone.zoneInfo > 0 && (geozone.zoneInfo < geoZoneConfig()->avoidAltitudeRange)) || avoidFzStep) { + + calculateFarAwayTarget(&posControl.sendTo.targetPos, posControl.actualState.cog, geoZoneConfig()->fenceDetectionDistance * 2); + if (avoidFzStep) { + posControl.sendTo.targetPos.z = fzStepAlt; + } else { + posControl.sendTo.targetPos.z = zone->config.maxAltitude + geoZoneConfig()->safeAltitudeDistance * 2; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING_UPWARD; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } else { + // Calc new course + avoidCourse = calcBounceCourseForZone(zone, &prevPoint, intersection); + + if (avoidCourse == -1) { + return; + } + + calculateFarAwayTarget(&posControl.sendTo.targetPos, avoidCourse, geoZoneConfig()->fenceDetectionDistance * 2); // Its too far, mode will be abort if we are on the right course + + // Check for min/max altitude + if (geozone.currentzoneMaxAltitude > 0 && getEstimatedActualPosition(Z) > geozone.currentzoneMaxAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 0.25; + } else if (geozone.currentzoneMinAltitude != 0 && getEstimatedActualPosition(Z) < geozone.currentzoneMinAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 0.25; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = AVOID_TIMEOUT; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } + } else if (zone->config.fenceAction == GEOFENCE_ACTION_POS_HOLD) { + actionState = GEOZONE_ACTION_STATE_POSHOLD; + + if (STATE(AIRPLANE)) { + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector3_t refPoint; + int32_t course = calcBounceCourseForZone(zone, &prevPoint, intersection); + calculateFarAwayTarget(&refPoint, course, geoZoneConfig()->fenceDetectionDistance * 2); + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, (fpVector2_t*)&refPoint)) { + geozone.loiterDir = 1; // Right + } else { + geozone.loiterDir = -1; // Left + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, &zone->verticesLocal[0])) { + geozone.loiterDir = -1; // Left + } else { + geozone.loiterDir = 1; // Right + } + } + } + + geozone.sticksLocked = true; + activateForcedPosHold(); + actionStartTime = millis(); + } else if (zone->config.fenceAction == GEOFENCE_ACTION_RTH) { + actionState = GEOZONE_ACTION_STATE_RTH; + geozone.sticksLocked = true; + activateForcedRTH(); + actionStartTime = millis(); + } +} + +static void endFenceAction(void) +{ + posControl.sendTo.lockSticks = false; + geozone.sticksLocked = false; + geozone.sticksLocked = 0; + + switch (actionState) { + case GEOZONE_ACTION_STATE_AVOIDING: + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + abortSendTo(); + break; + case GEOZONE_ACTION_STATE_POSHOLD: + abortForcedPosHold(); + break; + case GEOZONE_ACTION_STATE_RTH: + abortForcedRTH(); + break; + default: + break; + } + + actionState = GEOZONE_ACTION_STATE_NONE; + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)){ + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_Z); + } + + abortSendTo(); +} + +static void geoZoneInit(void) +{ + activeGeoZonesCount = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) + { + if (geoZonesConfig(i)->vertexCount > 0) { + memcpy(&activeGeoZones[activeGeoZonesCount].config, geoZonesConfig(i), sizeof(geoZoneRuntimeConfig_t)); + if (activeGeoZones[i].config.maxAltitude == 0) { + activeGeoZones[i].config.maxAltitude = INT32_MAX; + } + + activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0; + + if (!STATE(AIRPLANE) && activeGeoZones[i].config.fenceAction == GEOFENCE_ACTION_AVOID) { + activeGeoZones[i].config.fenceAction = GEOFENCE_ACTION_POS_HOLD; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + } + + if (activeGeoZonesCount > 0) { + // Covert geozone vertices to local + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + gpsLocation_t vertexLoc; + fpVector3_t posLocal3; + + if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) { + if (geoZonesConfig(geoZoneVertices(i)->zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) { + activeGeoZones[geoZoneVertices(i)->zoneId].radius = geoZoneVertices(i)->lat; + activeGeoZones[geoZoneVertices(i)->zoneId].config.vertexCount = 1; + continue; + } + + vertexLoc.lat = geoZoneVertices(i)->lat; + vertexLoc.lon = geoZoneVertices(i)->lon; + geoConvertGeodeticToLocal(&posLocal3, &posControl.gpsOrigin, &vertexLoc, GEO_ALT_ABSOLUTE); + + uint8_t vertexIdx = 0; + for (uint8_t j = 0; j < geoZoneVertices(i)->zoneId; j++) { + vertexIdx += activeGeoZones[j].config.vertexCount; + } + vertexIdx += geoZoneVertices(i)->idx; + + verticesLocal[vertexIdx].x = posLocal3.x; + verticesLocal[vertexIdx].y = posLocal3.y; + + if (geoZoneVertices(i)->idx == 0) { + activeGeoZones[geoZoneVertices(i)->zoneId].verticesLocal = &verticesLocal[vertexIdx]; + } + } + } + } + + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) + { + safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; + safeHomeGeozoneConfig.type = GEOZONE_TYPE_INCLUSIVE; + safeHomeGeozoneConfig.fenceAction = geoZoneConfig()->safeHomeFenceAction; + safeHomeGeozoneConfig.maxAltitude = 0; + safeHomeGeozoneConfig.minAltitude = 0; + safeHomeGeozoneConfig.vertexCount = 1; + + activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; + activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; + activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZonesCount++; + } + + updateCurrentZones(); + uint8_t newActiveZoneCount = activeGeoZonesCount; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (!geozone.insideFz) { + // Deactivate all inclusive geozones with distance > GEOZONE_INCLUSE_IGNORE_DISTANCE + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && !isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + fpVector2_t pos2 = { .x = getEstimatedActualPosition(X), .y = getEstimatedActualPosition(Y) }; + + uint32_t minDistanceToZone = INT32_MAX; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + float dist = calculateDistance2(&pos2, &activeGeoZones[i].verticesLocal[j]); + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + minDistanceToZone = dist - activeGeoZones[i].radius; + break; + } + + if (dist < minDistanceToZone) { + minDistanceToZone = dist; + } + } + if (minDistanceToZone > GEOZONE_INCLUSE_IGNORE_DISTANCE) { + activeGeoZones[i].enable = false; + newActiveZoneCount--; + } + } + } + } + + activeGeoZonesCount = newActiveZoneCount; + if (activeGeoZonesCount == 0) { + setTaskEnabled(TASK_GEOZONE, false); + geozoneIsEnabled = false; + return; + } + geozoneIsEnabled = true; + + qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + + for (int i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + isAtLeastOneInclusiveZoneActive = true; + break; + } + } +} + +// Called by Scheduler +void geozoneUpdate(timeUs_t curentTimeUs) +{ + UNUSED(curentTimeUs); + + geozone.messageState = GEOZONE_MESSAGE_STATE_NONE; + if (!isInitalised && navigationPositionEstimateIsHealthy()) { + geoZoneInit(); + isInitalised = true; + } + + if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + noZoneRTH = false; + return; + } + + // Zone RTH Override: Toggle RTH switch short + if (geozone.avoidInRTHInProgress) { + if (rthHomeSwitchLastState != IS_RC_MODE_ACTIVE(BOXNAVRTH)) { + if (millis() - rthOverrideStartTime < RTH_OVERRIDE_TIMEOUT) { + geozoneResetRTH(); + noZoneRTH = true; + } + rthOverrideStartTime = millis(); + } + rthHomeSwitchLastState = IS_RC_MODE_ACTIVE(BOXNAVRTH); + } + + updateCurrentZones(); + updateZoneInfos(); + + // User has switched to non geofence mode, end all actions and switch to mode from box input + if (isNonGeozoneModeFromBoxInput()) { + if (actionState != GEOZONE_ACTION_STATE_NONE) { + endFenceAction(); + } + lockRTZ = false; + return; + } + + switch (actionState) + { + case GEOZONE_ACTION_STATE_AVOIDING: + if (calculateDistanceToDestination(&avoidingPoint) > geozoneGetDetectionDistance()) { + posControl.sendTo.lockSticks = false; + } + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(avoidCourse, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), 500) || millis() - actionStartTime > AVOID_TIMEOUT || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE; + if ((geozone.insideFz && ABS(geozone.distanceVertToNearestZone ) > geoZoneConfig()->safeAltitudeDistance) || !posControl.flags.sendToActive) { + lockRTZ = true; + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_FLYOUT_NFZ; + if (!geozone.insideNfz || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_UPWARD: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RTH: + case GEOZONE_ACTION_STATE_POSHOLD: + geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + bool currentZoneHasAction = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + currentZoneHasAction = true; + break; + } + } + + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + + float targetAltitide = 0; + if (!geozone.insideFz) { + if (geozone.distanceVertToNearestZone < 0) { + targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + } else { + targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + } else { + targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + + fpVector3_t targetPos; + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitide; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + actionState = GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE; + actionStartTime = millis(); + activateSendTo(); + return; + } + + // RTH active: Further checks are done in RTH logic + if ((navGetCurrentStateFlags() & NAV_AUTO_RTH) || IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return; + } else if (geozone.avoidInRTHInProgress) { + geozoneResetRTH(); + } + + if (lockRTZ && (geozone.insideFz || geozone.insideNfz)) { + lockRTZ = false; + } + + if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { + + if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_OUTSIDE_FZ; + } + + if (geozone.insideNfz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_NFZ; + } + + if (!isNonGeozoneModeFromBoxInput()) { + bool flyOutNfz = false; + geoZoneRuntimeConfig_t *targetZone = nearestInclusiveZone; + + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + flyOutNfz = true; + targetZone = currentZones[i]; + break; + } + } + + if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { + int32_t targetAltitude = 0; + if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z); + } + + fpVector3_t targetPos; + if (aboveOrUnderZone) { + if (ABS(geozone.distanceVertToNearestZone) < 2000) { + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + if(geozone.distanceVertToNearestZone > 0) { + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + } + + } else { + targetPos = navGetCurrentActualPositionAndVelocity()->pos; + } + } else { + calculateFarAwayTarget(&targetPos, geozone.directionToNearestZone, geozone.distanceHorToNearestZone + geoZoneConfig()->fenceDetectionDistance / 2); + } + + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitude; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + + if (flyOutNfz) { + actionState = GEOZONE_ACTION_STATE_FLYOUT_NFZ; + } else { + actionState = GEOZONE_ACTION_STATE_RETURN_TO_FZ; + } + + activateSendTo(); + } + } + } + + + geoZoneRuntimeConfig_t *intersectZone = NULL; + fpVector3_t intersection, prevPoint; + float distanceToZone = 0; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + if (findNearestIntersectionZone(&intersectZone, &intersection, &distanceToZone, geoZoneConfig()->fenceDetectionDistance, &navGetCurrentActualPositionAndVelocity()->pos, &prevPoint)) { + if (geozone.insideFz) { + if (!isPointInAnyOtherZone(intersectZone, GEOZONE_TYPE_INCLUSIVE, &intersection)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + geozone.messageState = GEOZONE_MESSAGE_STATE_LEAVING_FZ; + performeFenceAction(intersectZone, &intersection); + } + } + + if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { + + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + + int32_t minAltitude = intersectZone->config.minAltitude; + int32_t maxAltitude = intersectZone->config.maxAltitude; + if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { + geozone.zoneInfo = INT32_MAX; + } else if (maxAltitude == INT32_MAX) { + geozone.zoneInfo = minAltitude - getEstimatedActualPosition(Z); + } else if (minAltitude == 0) { + geozone.zoneInfo = maxAltitude - getEstimatedActualPosition(Z); + } else { + int32_t distToMax = maxAltitude - getEstimatedActualPosition(Z); + int32_t distToMin = minAltitude - getEstimatedActualPosition(Z); + if (ABS(distToMin) < ABS(distToMax)) { + geozone.zoneInfo = distToMin; + } else { + geozone.zoneInfo = (distToMax); + } + } + + geozone.messageState = GEOZONE_MESSAGE_STATE_ENTERING_NFZ; + performeFenceAction(intersectZone, &intersection); + } + } +} + +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void) +{ + return &rthWaypoints[rthWaypointIndex]; +} + +void geozoneAdvanceRthAvoidWaypoint(void) +{ + if (rthWaypointIndex < rthWaypointCount) { + rthWaypointIndex++; + } +} + +bool geoZoneIsLastRthWaypoint(void) +{ + return rthWaypointIndex == rthWaypointCount - 1; +} + +void geozoneResetRTH(void) +{ + geozone.avoidInRTHInProgress = false; + rthWaypointIndex = 0; + rthWaypointCount = 0; +} + +void geozoneSetupRTH() { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +int8_t geozoneCheckForNFZAtCourse(bool isRTH) +{ + UNUSED(isRTH); + + if (geozone.avoidInRTHInProgress || noZoneRTH || !geozoneIsEnabled || !isInitalised) { + return 0; + } + + updateCurrentZones(); + + // Never mind, lets fly out of the zone on current course + if (geozone.insideNfz || (isAtLeastOneInclusiveZoneActive && !geozone.insideFz)) { + return 0; + } + + int8_t waypointCount = calcRthCourse(rthWaypoints, &navGetCurrentActualPositionAndVelocity()->pos, &posControl.rthState.homePosition.pos); + if (waypointCount > 0) { + rthWaypointCount = waypointCount; + rthWaypointIndex = 0; + geozone.avoidInRTHInProgress = true; + return 1; + } + + return waypointCount; +} + +bool isGeozoneActive(void) +{ + return activeGeoZonesCount > 0; +} + +void geozoneUpdateMaxHomeAltitude(void) { + int32_t altitude = INT32_MIN; + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, &posControl.rthState.homePosition.pos, false); + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + altitude = MAX(zones[i]->config.maxAltitude, altitude); + } + } + + if (altitude > INT32_MIN) { + geozone.maxHomeAltitude = altitude - geoZoneConfig()->safeAltitudeDistance; + geozone.homeHasMaxAltitue = true; + } else { + geozone.homeHasMaxAltitue = false; + } +} + +// Avoid arming in NFZ +bool geozoneIsInsideNFZ(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || activeGeoZonesCount == 0) { + return false; + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return true; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return false; + } + } + + // We aren't in any zone, is an inclusive one still active? + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + } + + return false; +} + +#endif diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h new file mode 100755 index 00000000000..38849719af0 --- /dev/null +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -0,0 +1,480 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#include +#include +#include + +#include "common/vector.h" +#include "navigation/navigation_private.h" + +#define K_EPSILON 1e-8 + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = line1End->x - line1Start->x; + double t1 = -(line2End->x - line2Start->x); + double r1 = line2Start->x - line1Start->x; + + double s2 = line1End->y - line1Start->y; + double t2 = -(line2End->y - line2Start->y); + double r2 = line2Start->y - line1Start->y; + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } + else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6d7b386cbde..fe277a00617 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -113,6 +113,11 @@ typedef struct navigationFlags_s { bool rthTrackbackActive; // Activation status of RTH trackback bool wpTurnSmoothingActive; // Activation status WP turn smoothing bool manualEmergLandActive; // Activation status of manual emergency landing + +#ifdef USE_GEOZONE + bool sendToActive; + bool forcedPosholdActive; +#endif } navigationFlags_t; typedef struct { @@ -160,6 +165,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, + NAV_FSM_EVENT_SWITCH_TO_SEND_TO, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -245,6 +251,10 @@ typedef enum { NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46, NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47, NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48, + + NAV_PERSISTENT_ID_SEND_TO_INITALIZE = 49, + NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES = 50, + NAV_PERSISTENT_ID_SEND_TO_FINISHED = 51 } navigationPersistentId_e; typedef enum { @@ -304,6 +314,10 @@ typedef enum { NAV_STATE_MIXERAT_IN_PROGRESS, NAV_STATE_MIXERAT_ABORT, + NAV_STATE_SEND_TO_INITALIZE, + NAV_STATE_SEND_TO_IN_PROGESS, + NAV_STATE_SEND_TO_FINISHED, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -406,6 +420,17 @@ typedef enum { RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; +#ifdef USE_GEOZONE +typedef struct navSendTo_s { + fpVector3_t targetPos; + uint16_t altitudeTargetRange; // 0 for only "2D" + uint32_t targetRange; + bool lockSticks; + uint32_t lockStickTime; + timeMs_t startTime; +} navSendTo_t; +#endif + typedef struct { fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming uint32_t distance; // distance to the nearest safehome @@ -478,6 +503,10 @@ typedef struct { fwLandState_t fwLandState; #endif +#ifdef USE_GEOZONE + navSendTo_t sendTo; // Used for Geozones +#endif + /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; @@ -502,7 +531,9 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); +void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos); +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos); bool isLandingDetected(void); void resetLandingDetector(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 176812d8f3f..5d38a6dba59 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -137,6 +137,10 @@ typedef enum { TASK_TELEMETRY_SBUS2, #endif +#if defined (USE_GEOZONE) && defined(USE_GPS) + TASK_GEOZONE, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747ce..3e035710fca 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -221,3 +221,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER +#define USE_GEOZONE From e4627d030d894234debd667cfc91c5879ad3b5e3 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 2 Nov 2024 22:11:19 +0000 Subject: [PATCH 107/175] Add stats and info MSP commands for MSP RC This is an initial, basic implementation. This can be expanded in the future to deal with multiple sublinks etc. --- src/main/fc/fc_msp.c | 36 ++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_common.h | 37 +++++++++++++++------------ src/main/rx/rx.h | 16 +++++++----- 3 files changed, 66 insertions(+), 23 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f87b7f079c7..afa5df2741d 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2914,6 +2914,42 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) return MSP_RESULT_ERROR; break; +#ifdef USE_RX_MSP + case MSP2_COMMON_SET_MSP_RC_LINK_STATS: + if (dataSize == 48) { + uint8_t sublinkID = sbufReadU8(src); // Sublink ID + sbufReadU8(src); // Valid link (Failsafe backup) + if (sublinkID == 1) { + rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); + rxLinkStatistics.downlinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkSNR = sbufReadU8(src); + } + } else + return MSP_RESULT_ERROR; + break; + + case MSP2_COMMON_SET_MSP_RC_INFO: + if (dataSize == 104) { + uint8_t sublinkID = sbufReadU8(src); + + if (sublinkID == 1) { + rxLinkStatistics.uplinkTXPower = sbufReadU8(src); + rxLinkStatistics.downlinkTXPower = sbufReadU8(src); + + for (int i = 0; i < 4 - 1; i++) { + rxLinkStatistics.band[i] = sbufReadU8(src); + } + + for (int i = 0; i < 6 - 1; i++) { + rxLinkStatistics.mode[i] = sbufReadU8(src); + } + } + } else + return MSP_RESULT_ERROR; + break; +#endif + case MSP_SET_FAILSAFE_CONFIG: if (dataSize == 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index e778a1808c9..8784b253114 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/src/main/msp/msp_protocol_v2_common.h @@ -15,22 +15,25 @@ * along with INAV. If not, see . */ -#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) -#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) -#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting -#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting +#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16)) +#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16)) +#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting +#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting + +#define MSP2_COMMON_MOTOR_MIXER 0x1005 +#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 + +#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). +#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A + +// radar commands +#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information +#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display -#define MSP2_COMMON_MOTOR_MIXER 0x1005 -#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006 +#define MSP2_COMMON_SET_MSP_RC_LINK_STATS 0x100D //in message Sets the MSP RC stats +#define MSP2_COMMON_SET_MSP_RC_INFO 0x100E //in message Sets the MSP RC info -#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..). -#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings - -#define MSP2_COMMON_SERIAL_CONFIG 0x1009 -#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A - -// radar commands -#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information -#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display - -#define MSP2_BETAFLIGHT_BIND 0x3000 +#define MSP2_BETAFLIGHT_BIND 0x3000 diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index c841838a5ea..4d8a76fecf5 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -181,12 +181,16 @@ typedef enum { } rssiSource_e; typedef struct rxLinkStatistics_s { - int16_t uplinkRSSI; // RSSI value in dBm - uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] - int8_t uplinkSNR; // The SNR of the uplink in dB - uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] - uint16_t uplinkTXPower; // power in mW - uint8_t activeAntenna; + int16_t uplinkRSSI; // RSSI value in dBm + uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100] + uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100] + int8_t uplinkSNR; // The SNR of the uplink in dB + uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz] + uint16_t uplinkTXPower; // power in mW + uint16_t downlinkTXPower; // power in mW + uint8_t activeAntenna; + char band[4]; + char mode[6]; } rxLinkStatistics_t; typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data From 66e9be42c3176fd27efffaceedb828a9b5b2592f Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 2 Nov 2024 22:15:26 +0000 Subject: [PATCH 108/175] Update fc_msp.c --- src/main/fc/fc_msp.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index afa5df2741d..62e09cf1ace 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2920,6 +2920,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { + // RSSI % rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); From a807056a560de2df0225548b5ffeeb6c96cce3c4 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 08:47:34 +0000 Subject: [PATCH 109/175] Corrected int types --- src/main/fc/fc_msp.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 62e09cf1ace..f4f144e8bd7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2916,12 +2916,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_RX_MSP case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize == 48) { + if (dataSize == 56) { uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { - // RSSI % - rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); + sbufReadU8(src); // RSSI % + rxLinkStatistics.uplinkRSSI = -sbufReadU16(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkSNR = sbufReadU8(src); @@ -2931,12 +2931,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_COMMON_SET_MSP_RC_INFO: - if (dataSize == 104) { + if (dataSize == 120) { uint8_t sublinkID = sbufReadU8(src); if (sublinkID == 1) { - rxLinkStatistics.uplinkTXPower = sbufReadU8(src); - rxLinkStatistics.downlinkTXPower = sbufReadU8(src); + rxLinkStatistics.uplinkTXPower = sbufReadU16(src); + rxLinkStatistics.downlinkTXPower = sbufReadU16(src); for (int i = 0; i < 4 - 1; i++) { rxLinkStatistics.band[i] = sbufReadU8(src); From 8a729cda495ac8e99c83319d0c458efdda89948a Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 11:34:24 +0000 Subject: [PATCH 110/175] corrected dataSize check to bytes --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f4f144e8bd7..f5cc24e7a11 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2916,7 +2916,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_RX_MSP case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize == 56) { + if (dataSize == 7) { uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { @@ -2931,7 +2931,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_COMMON_SET_MSP_RC_INFO: - if (dataSize == 120) { + if (dataSize == 15) { uint8_t sublinkID = sbufReadU8(src); if (sublinkID == 1) { From 2f51a7a7cd5629f79c87fd1710fa3acc16941050 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 11:39:02 +0000 Subject: [PATCH 111/175] Added comment to assist on hover check of variable --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f5cc24e7a11..0bba60c1825 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1807,7 +1807,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) #ifdef USE_FLASHFS static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) { - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ uint16_t readLength; const uint32_t readAddress = sbufReadU32(src); From 80c47fad20838938754ff0a2e12c136fd9c24e71 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 11:42:47 +0000 Subject: [PATCH 112/175] Ditto --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0bba60c1825..8bbf880b790 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -215,7 +215,7 @@ static void mspSerialPassthroughFn(serialPort_t *serialPort) static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ if (dataSize == 0) { // Legacy format @@ -1831,7 +1831,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t tmp_u8; uint16_t tmp_u16; - const unsigned int dataSize = sbufBytesRemaining(src); + const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */ switch (cmdMSP) { case MSP_SELECT_SETTING: From a9f327fae094b18ffb30d77a8a7eb721cb524258 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 12:21:41 +0000 Subject: [PATCH 113/175] Update fc_msp.c - Add RSSI - Allow extended versions of future MSP2_COMMON_SET_MSP_RC_LINK_STATS and MSP2_COMMON_SET_MSP_RC_INFO still work with older (from now) INAV versions. Except for the new data. --- src/main/fc/fc_msp.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8bbf880b790..ab8a3b2061c 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2916,11 +2916,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_RX_MSP case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize == 7) { + if (dataSize >= 7) { uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { - sbufReadU8(src); // RSSI % + setRSSIFromMSP(sbufReadU8(src)); // RSSI % rxLinkStatistics.uplinkRSSI = -sbufReadU16(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); @@ -2931,7 +2931,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; case MSP2_COMMON_SET_MSP_RC_INFO: - if (dataSize == 15) { + if (dataSize >= 15) { uint8_t sublinkID = sbufReadU8(src); if (sublinkID == 1) { From 4aaa12f379e18a44e038f2e2a7c0a2c14c3e415d Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 14:24:45 +0000 Subject: [PATCH 114/175] Have RSSI go from 0-100%, rather than 0-99% --- src/main/io/osd.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 41c20dda928..b6abbc30f49 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -616,11 +616,11 @@ char *osdFormatTrimWhiteSpace(char *buff) /** * Converts RSSI into a % value used by the OSD. + * Range is [0, 100] */ -static uint16_t osdConvertRSSI(void) +static uint8_t osdConvertRSSI(void) { - // change range to [0, 99] - return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); + return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 100); } static uint16_t osdGetCrsfLQ(void) @@ -1712,9 +1712,13 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_RSSI_VALUE: { - uint16_t osdRssi = osdConvertRSSI(); + uint8_t osdRssi = osdConvertRSSI(); buff[0] = SYM_RSSI; - tfp_sprintf(buff + 1, "%2d", osdRssi); + if (osdRssi < 100) + tfp_sprintf(buff + 1, "%2d", osdRssi); + else + tfp_sprintf(buff + 1, " %c", SYM_MAX); + if (osdRssi < osdConfig()->rssi_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } From 1a2659c153cdfbf1bceb1c42ebb9da6d1324bbff Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 14:30:08 +0000 Subject: [PATCH 115/175] Update osd.c --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b6abbc30f49..39f05390352 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -181,7 +181,7 @@ typedef struct statistic_s { uint16_t min_voltage; // /100 int16_t max_current; int32_t max_power; - int16_t min_rssi; + int8_t min_rssi; int16_t min_lq; // for CRSF int16_t min_rssi_dbm; // for CRSF int32_t max_altitude; @@ -4586,7 +4586,7 @@ static void osdResetStats(void) stats.max_3D_speed = 0; stats.max_air_speed = 0; stats.min_voltage = 12000; - stats.min_rssi = 99; + stats.min_rssi = 100; stats.min_lq = 300; stats.min_rssi_dbm = 0; stats.max_altitude = 0; From 823aa828912f81da916b55129d7912854308b5a9 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 14:33:13 +0000 Subject: [PATCH 116/175] Update osd.c --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 39f05390352..73ea8200330 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -181,7 +181,7 @@ typedef struct statistic_s { uint16_t min_voltage; // /100 int16_t max_current; int32_t max_power; - int8_t min_rssi; + uint8_t min_rssi; int16_t min_lq; // for CRSF int16_t min_rssi_dbm; // for CRSF int32_t max_altitude; From 6a77be500cfe655a8275c8ad922b27852975e9b7 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 15:26:22 +0000 Subject: [PATCH 117/175] Update osd.c --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 73ea8200330..d3419a65288 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1717,7 +1717,7 @@ static bool osdDrawSingleElement(uint8_t item) if (osdRssi < 100) tfp_sprintf(buff + 1, "%2d", osdRssi); else - tfp_sprintf(buff + 1, " %c", SYM_MAX); + tfp_sprintf(buff + 1, "%c ", SYM_MAX); if (osdRssi < osdConfig()->rssi_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); From 99b1a1513424b4582d1880c47463d44f438d2442 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 15:45:59 +0000 Subject: [PATCH 118/175] Fixed datasize --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ab8a3b2061c..c8eb95dd890 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2916,7 +2916,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_RX_MSP case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize >= 7) { + if (dataSize >= 8) { uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { From f16f63548208e49c90d5dd562228f4e068877b9f Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 15:50:15 +0000 Subject: [PATCH 119/175] Fix for crap added by GitHub Copilot --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c8eb95dd890..168d0817126 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2938,11 +2938,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxLinkStatistics.uplinkTXPower = sbufReadU16(src); rxLinkStatistics.downlinkTXPower = sbufReadU16(src); - for (int i = 0; i < 4 - 1; i++) { + for (int i = 0; i < 4; i++) { rxLinkStatistics.band[i] = sbufReadU8(src); } - for (int i = 0; i < 6 - 1; i++) { + for (int i = 0; i < 6; i++) { rxLinkStatistics.mode[i] = sbufReadU8(src); } } From b5b9e586a859b2f68253533125728ba2eb4995b8 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 16:22:28 +0000 Subject: [PATCH 120/175] Allow signed for SNR --- src/main/common/streambuf.c | 5 +++++ src/main/common/streambuf.h | 1 + src/main/fc/fc_msp.c | 2 +- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c index 7a4e8c8cc99..5a766423421 100644 --- a/src/main/common/streambuf.c +++ b/src/main/common/streambuf.c @@ -98,6 +98,11 @@ uint8_t sbufReadU8(sbuf_t *src) return *src->ptr++; } +int8_t sbufReadI8(sbuf_t *src) +{ + return *src->ptr++; +} + uint16_t sbufReadU16(sbuf_t *src) { uint16_t ret; diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h index 74331147da8..a2ac1f681a6 100644 --- a/src/main/common/streambuf.h +++ b/src/main/common/streambuf.h @@ -42,6 +42,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val); void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val); uint8_t sbufReadU8(sbuf_t *src); +int8_t sbufReadI8(sbuf_t *src); uint16_t sbufReadU16(sbuf_t *src); uint32_t sbufReadU32(sbuf_t *src); void sbufReadData(const sbuf_t *dst, void *data, int len); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 168d0817126..50146de8aac 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2924,7 +2924,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxLinkStatistics.uplinkRSSI = -sbufReadU16(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); - rxLinkStatistics.uplinkSNR = sbufReadU8(src); + rxLinkStatistics.uplinkSNR = sbufReadI8(src); } } else return MSP_RESULT_ERROR; From 1311e2be797f284dbe7e16d5152baeb38d2132b7 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 16:37:52 +0000 Subject: [PATCH 121/175] Allow snr_alarm to be set up to 99dB --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/io/osd.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ce4ff9c1ce4..f52da9c2c4b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5178,7 +5178,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB) | Default | Min | Max | | --- | --- | --- | -| 4 | -20 | 10 | +| 4 | -20 | 99 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fd6073fa4bd..f7ba8eb7bbd 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3376,7 +3376,7 @@ groups: default_value: 4 field: snr_alarm min: -20 - max: 10 + max: 99 - name: osd_link_quality_alarm condition: USE_SERIALRX_CRSF description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 41c20dda928..67e401f6abc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2535,7 +2535,7 @@ static bool osdDrawSingleElement(uint8_t item) } } else if (snrFiltered <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - if (snrFiltered <= -10) { + if (snrFiltered <= -10 || >= 10) { tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); } else { tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); From 7f2136c413ff144a7c398356a802a6cfb8a8d754 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 16:39:53 +0000 Subject: [PATCH 122/175] PG Bump --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 67e401f6abc..2fdf8276814 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); void osdStartedSaveProcess(void) { From 2c1afa1d5b010d74bdf5dbc843c01c24ce398245 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 16:41:10 +0000 Subject: [PATCH 123/175] Bug fix on OSD output of SNR --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2fdf8276814..2b8b634f001 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2535,7 +2535,7 @@ static bool osdDrawSingleElement(uint8_t item) } } else if (snrFiltered <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - if (snrFiltered <= -10 || >= 10) { + if (snrFiltered <= -10 || snrFiltered >= 10) { tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); } else { tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); From b48d7c8ad4e674ca412f248b1265b72b0d01a933 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 17:11:31 +0000 Subject: [PATCH 124/175] Add function to set the RSSI from MSP_RC --- src/main/fc/fc_msp.c | 2 +- src/main/rx/rx.c | 12 ++++++++++++ src/main/rx/rx.h | 1 + 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 50146de8aac..20b30a71f1f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2920,7 +2920,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) if (sublinkID == 1) { - setRSSIFromMSP(sbufReadU8(src)); // RSSI % + setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % rxLinkStatistics.uplinkRSSI = -sbufReadU16(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f941eaebe20..5743f514959 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -572,6 +572,18 @@ static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered) rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); } +void setRSSIFromMSP_RC(uint8_t newMspRssi) +{ + if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) { + activeRssiSource = RSSI_SOURCE_MSP; + } + + if (activeRssiSource == RSSI_SOURCE_MSP) { + rssi = constrain(scaleRange(newMspRssi, 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); + lastMspRssiUpdateUs = micros(); + } +} + void setRSSIFromMSP(uint8_t newMspRssi) { if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 4d8a76fecf5..64b97b172e2 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -216,6 +216,7 @@ bool isRxPulseValid(uint16_t pulseDuration); uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap); void parseRcChannels(const char *input); +void setRSSIFromMSP_RC(uint8_t newMspRssi); void setRSSIFromMSP(uint8_t newMspRssi); void updateRSSI(timeUs_t currentTimeUs); // Returns RSSI in [0, RSSI_MAX_VALUE] range. From 102875384fe6b70049294db089d7ba894296500d Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 19:24:15 +0000 Subject: [PATCH 125/175] Update - Set base sublink ID to 0 - changed RSSI dB to uint8 as 16 is not needed - Added constraint to ensure that RSSI % is between 0 and 100 --- src/main/fc/fc_msp.c | 8 ++++---- src/main/rx/rx.c | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 20b30a71f1f..1ba0d3780f4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2916,12 +2916,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_RX_MSP case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize >= 8) { + if (dataSize >= 7) { uint8_t sublinkID = sbufReadU8(src); // Sublink ID sbufReadU8(src); // Valid link (Failsafe backup) - if (sublinkID == 1) { + if (sublinkID == 0) { setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % - rxLinkStatistics.uplinkRSSI = -sbufReadU16(src); + rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); rxLinkStatistics.downlinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkSNR = sbufReadI8(src); @@ -2934,7 +2934,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize >= 15) { uint8_t sublinkID = sbufReadU8(src); - if (sublinkID == 1) { + if (sublinkID == 0) { rxLinkStatistics.uplinkTXPower = sbufReadU16(src); rxLinkStatistics.downlinkTXPower = sbufReadU16(src); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 5743f514959..00ed23ab25c 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -579,7 +579,7 @@ void setRSSIFromMSP_RC(uint8_t newMspRssi) } if (activeRssiSource == RSSI_SOURCE_MSP) { - rssi = constrain(scaleRange(newMspRssi, 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); + rssi = constrain(scaleRange(constrain(newMspRssi, 0, 100), 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE); lastMspRssiUpdateUs = micros(); } } From 313459e19c6fce4dfe71868503ff835be9a8a2aa Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 19:41:53 +0000 Subject: [PATCH 126/175] Return `MSP_RESULT_NO_REPLY` flag --- src/main/fc/fc_msp.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 1ba0d3780f4..0ef822231fe 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1861,6 +1861,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } rxMspFrameReceive(frame, channelCount); } + + return MSP_RESULT_NO_REPLY; } break; #endif @@ -2926,6 +2928,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxLinkStatistics.uplinkLQ = sbufReadU8(src); rxLinkStatistics.uplinkSNR = sbufReadI8(src); } + + return MSP_RESULT_NO_REPLY; } else return MSP_RESULT_ERROR; break; @@ -2946,6 +2950,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxLinkStatistics.mode[i] = sbufReadU8(src); } } + + return MSP_RESULT_NO_REPLY; } else return MSP_RESULT_ERROR; break; From 0655460ebef06c2c4485865af5006c322df8b9ee Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sun, 3 Nov 2024 20:13:51 +0000 Subject: [PATCH 127/175] Update fc_msp.c --- src/main/fc/fc_msp.c | 62 +++++++++++++++++++++++--------------------- 1 file changed, 32 insertions(+), 30 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0ef822231fe..3182ede9700 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2917,43 +2917,45 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #ifdef USE_RX_MSP - case MSP2_COMMON_SET_MSP_RC_LINK_STATS: - if (dataSize >= 7) { - uint8_t sublinkID = sbufReadU8(src); // Sublink ID - sbufReadU8(src); // Valid link (Failsafe backup) - if (sublinkID == 0) { - setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % - rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); - rxLinkStatistics.downlinkLQ = sbufReadU8(src); - rxLinkStatistics.uplinkLQ = sbufReadU8(src); - rxLinkStatistics.uplinkSNR = sbufReadI8(src); - } + case MSP2_COMMON_SET_MSP_RC_LINK_STATS: { + if (dataSize >= 7) { + uint8_t sublinkID = sbufReadU8(src); // Sublink ID + sbufReadU8(src); // Valid link (Failsafe backup) + if (sublinkID == 0) { + setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI % + rxLinkStatistics.uplinkRSSI = -sbufReadU8(src); + rxLinkStatistics.downlinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkLQ = sbufReadU8(src); + rxLinkStatistics.uplinkSNR = sbufReadI8(src); + } - return MSP_RESULT_NO_REPLY; - } else - return MSP_RESULT_ERROR; + return MSP_RESULT_NO_REPLY; + } else + return MSP_RESULT_ERROR; + } break; - case MSP2_COMMON_SET_MSP_RC_INFO: - if (dataSize >= 15) { - uint8_t sublinkID = sbufReadU8(src); + case MSP2_COMMON_SET_MSP_RC_INFO: { + if (dataSize >= 15) { + uint8_t sublinkID = sbufReadU8(src); - if (sublinkID == 0) { - rxLinkStatistics.uplinkTXPower = sbufReadU16(src); - rxLinkStatistics.downlinkTXPower = sbufReadU16(src); - - for (int i = 0; i < 4; i++) { - rxLinkStatistics.band[i] = sbufReadU8(src); - } + if (sublinkID == 0) { + rxLinkStatistics.uplinkTXPower = sbufReadU16(src); + rxLinkStatistics.downlinkTXPower = sbufReadU16(src); + + for (int i = 0; i < 4; i++) { + rxLinkStatistics.band[i] = sbufReadU8(src); + } - for (int i = 0; i < 6; i++) { - rxLinkStatistics.mode[i] = sbufReadU8(src); + for (int i = 0; i < 6; i++) { + rxLinkStatistics.mode[i] = sbufReadU8(src); + } } - } - return MSP_RESULT_NO_REPLY; - } else - return MSP_RESULT_ERROR; + return MSP_RESULT_NO_REPLY; + } else + return MSP_RESULT_ERROR; + } break; #endif From 5b6f691a6cf86d0f75b357be305ebd9d5ad98044 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Mon, 4 Nov 2024 20:11:12 +0000 Subject: [PATCH 128/175] Add PF and OSD code - Added new RX operands to programming framework - Added new OSD elements - Updated CRSF elements that are no longer exclusive to CRSF --- src/main/cms/cms_menu_osd.c | 8 +-- src/main/fc/settings.yaml | 10 ++-- src/main/io/osd.c | 77 ++++++++++++++++++-------- src/main/io/osd.h | 14 +++-- src/main/programming/logic_condition.c | 24 +++++--- src/main/programming/logic_condition.h | 5 +- 6 files changed, 91 insertions(+), 47 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index e99e0cc52f2..0e85d0e656d 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -147,10 +147,10 @@ static const OSD_Entry menuCrsfRxEntries[]= OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM), OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM), OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN), - OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM), - OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ), - OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB), - OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER), + OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_RSSI_DBM), + OSD_ELEMENT_ENTRY("RX LQ", OSD_LQ_UPLINK), + OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_SNR_DB), + OSD_ELEMENT_ENTRY("TX POWER", OSD_TX_POWER_UPLINK), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f7ba8eb7bbd..1960e5dac67 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3371,35 +3371,35 @@ groups: min: -550 max: 1250 - name: osd_snr_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "Value below which Crossfire SNR Alarm pops-up. (dB)" default_value: 4 field: snr_alarm min: -20 max: 99 - name: osd_link_quality_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" default_value: 70 field: link_quality_alarm min: 0 max: 100 - name: osd_rssi_dbm_alarm - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm" default_value: 0 field: rssi_dbm_alarm min: -130 max: 0 - name: osd_rssi_dbm_max - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%" default_value: -30 field: rssi_dbm_max min: -50 max: 0 - name: osd_rssi_dbm_min - condition: USE_SERIALRX_CRSF + condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%" default_value: -120 field: rssi_dbm_min diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2b8b634f001..71081843c78 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -225,7 +225,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); -PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); +PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3); void osdStartedSaveProcess(void) { savingSettings = true; @@ -2463,8 +2463,8 @@ static bool osdDrawSingleElement(uint8_t item) return true; } -#if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_RSSI_DBM: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + case OSD_RSSI_DBM: { int16_t rssi = rxLinkStatistics.uplinkRSSI; buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna @@ -2480,19 +2480,15 @@ static bool osdDrawSingleElement(uint8_t item) } break; } - case OSD_CRSF_LQ: + case OSD_LQ_UPLINK: { buff[0] = SYM_LQ; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); - switch (osdConfig()->crsf_lq_format) { - case OSD_CRSF_LQ_TYPE1: - if (!failsafeIsReceivingRxData()) { - tfp_sprintf(buff+1, "%3d", 0); - } else { - tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); - } - break; + uint8_t lqFormat = osdConfig()->crsf_lq_format; + + if (rxConfig()->receiverType == RX_TYPE_MSP) + lqFormat = OSD_CRSF_LQ_TYPE1; + + switch (lqFormat) { case OSD_CRSF_LQ_TYPE2: if (!failsafeIsReceivingRxData()) { tfp_sprintf(buff+1, "%s:%3d", " ", 0); @@ -2504,9 +2500,18 @@ static bool osdDrawSingleElement(uint8_t item) if (!failsafeIsReceivingRxData()) { tfp_sprintf(buff+1, "%3d", 0); } else { + int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300); tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); } break; + case OSD_CRSF_LQ_TYPE1: + default: + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d", 0); + } else { + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + } + break; } if (!failsafeIsReceivingRxData()) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -2516,7 +2521,24 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_SNR_DB: + case OSD_LQ_DOWNLINK: + { + buff[0] = SYM_LQ; + if (!failsafeIsReceivingRxData()) { + tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN); + } else { + tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN); + } + + if (!failsafeIsReceivingRxData()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + break; + } + + case OSD_SNR_DB: { static pt1Filter_t snrFilterState; static timeMs_t snrUpdated = 0; @@ -2544,7 +2566,7 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_TX_POWER: + case OSD_TX_POWER_UPLINK: { if (!failsafeIsReceivingRxData()) tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); @@ -2552,6 +2574,15 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); break; } + + case OSD_RX_POWER_DOWNLINK: + { + if (!failsafeIsReceivingRxData()) + tfp_sprintf(buff, "%s%c%c", " ", SYM_BLANK, SYM_BLANK); + else + tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN); + break; + } #endif case OSD_FORMATION_FLIGHT: @@ -3992,7 +4023,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, #endif -#ifdef USE_SERIALRX_CRSF +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, @@ -4143,11 +4174,13 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3); osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6); -#ifdef USE_SERIALRX_CRSF - osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12); - osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11); - osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9); - osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10); +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + osdLayoutsConfig->item_pos[0][OSD_RSSI_DBM] = OSD_POS(23, 12); + osdLayoutsConfig->item_pos[0][OSD_LQ_UPLINK] = OSD_POS(23, 10); + osdLayoutsConfig->item_pos[0][OSD_LQ_DOWNLINK] = OSD_POS(23, 11); + osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9); + osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10); + osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11); #endif osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b0423d40eff..da2c9bb015c 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -242,10 +242,10 @@ typedef enum { OSD_ESC_RPM, OSD_ESC_TEMPERATURE, OSD_AZIMUTH, - OSD_CRSF_RSSI_DBM, - OSD_CRSF_LQ, - OSD_CRSF_SNR_DB, - OSD_CRSF_TX_POWER, + OSD_RSSI_DBM, + OSD_LQ_UPLINK, + OSD_SNR_DB, + OSD_TX_POWER_UPLINK, OSD_GVAR_0, OSD_GVAR_1, OSD_GVAR_2, @@ -291,7 +291,9 @@ typedef enum { OSD_CUSTOM_ELEMENT_5, OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, - OSD_CUSTOM_ELEMENT_8, // 158 + OSD_CUSTOM_ELEMENT_8, + OSD_LQ_DOWNLINK, + OSD_RX_POWER_DOWNLINK, // 160 OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -369,7 +371,7 @@ typedef struct osdConfig_s { float gforce_alarm; float gforce_axis_alarm_min; float gforce_axis_alarm_max; -#ifdef USE_SERIALRX_CRSF +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) int8_t snr_alarm; //CRSF SNR alarm in dB int8_t link_quality_alarm; int16_t rssi_dbm_alarm; // in dBm diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 594db21417c..facc595c3f4 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -796,20 +796,28 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX); break; - case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ: - #ifdef USE_SERIALRX_CRSF + case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) return rxLinkStatistics.uplinkLQ; - #else +#else return 0; - #endif +#endif break; - case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR: - #ifdef USE_SERIALRX_CRSF +case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + return rxLinkStatistics.downlinkLQ; +#else + return 0; +#endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_SNR: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) return rxLinkStatistics.uplinkSNR; - #else +#else return 0; - #endif +#endif break; case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 74a7765be40..aaa80d51d53 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -129,8 +129,8 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26 LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27 LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29 - LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK, // 29 + LOGIC_CONDITION_OPERAND_FLIGHT_SNR, // 39 LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31 LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33 @@ -144,6 +144,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41 LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42 LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43 + LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44 } logicFlightOperands_e; typedef enum { From 4fa7acd25e055856b4bbdb1e780fa9b84a64aed9 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Mon, 4 Nov 2024 22:30:46 +0000 Subject: [PATCH 129/175] Added RX band and mode elements --- src/main/drivers/osd_symbols.h | 2 ++ src/main/io/osd.c | 11 +++++++++++ src/main/io/osd.h | 2 ++ 3 files changed, 15 insertions(+) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 90c0bc97131..fe673c93a57 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -234,6 +234,8 @@ #define SYM_AH_CH_CENTER 0x166 // 358 Crossair center #define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing #define SYM_ODOMETER 0x168 // 360 Odometer +#define SYM_RX_BAND 0x169 // 361 RX Band +#define SYM_RX_MODE 0x16A // 362 RX Mode #define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 #define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 71081843c78..a7f9f024868 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2583,6 +2583,15 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN); break; } + case OSD_RX_BAND: + displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND); + strcat(buff, rxLinkStatistics.band); + break; + + case OSD_RX_MODE: + displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE); + strcat(buff, rxLinkStatistics.mode); + break; #endif case OSD_FORMATION_FLIGHT: @@ -4181,6 +4190,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9); osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10); osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11); + osdLayoutsConfig->item_pos[0][OSD_RX_BAND] = OSD_POS(24, 12); + osdLayoutsConfig->item_pos[0][OSD_RX_MODE] = OSD_POS(24, 13); #endif osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index da2c9bb015c..c57bd62f640 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -294,6 +294,8 @@ typedef enum { OSD_CUSTOM_ELEMENT_8, OSD_LQ_DOWNLINK, OSD_RX_POWER_DOWNLINK, // 160 + OSD_RX_BAND, + OSD_RX_MODE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From 44aca2a86bee25732ae686442736927e602a2aae Mon Sep 17 00:00:00 2001 From: TUNERC-Aria <71423100+TUNERC-Aria@users.noreply.github.com> Date: Tue, 5 Nov 2024 17:42:56 +0800 Subject: [PATCH 130/175] Update target.h Removed the wrong USART4 --- src/main/target/TUNERCF405/target.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/TUNERCF405/target.h b/src/main/target/TUNERCF405/target.h index 093da2fadf6..b359545e697 100644 --- a/src/main/target/TUNERCF405/target.h +++ b/src/main/target/TUNERCF405/target.h @@ -105,7 +105,6 @@ #define UART4_TX_PIN PA0 #define USE_UART_INVERTER #define INVERTER_PIN_UART4_RX PC14 -#define INVERTER_PIN_USART4_RX PC14 #define USE_UART5 #define UART5_RX_PIN PD2 From 95f0f47bdffa4a53f1bbf0aff9aeff4dd127d9bf Mon Sep 17 00:00:00 2001 From: TUNERC-Aria <71423100+TUNERC-Aria@users.noreply.github.com> Date: Tue, 5 Nov 2024 23:59:17 +0800 Subject: [PATCH 131/175] Update uart_inverter.c f405 does not have USART4. --- src/main/drivers/uart_inverter.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/uart_inverter.c b/src/main/drivers/uart_inverter.c index b815883df79..5168831b471 100644 --- a/src/main/drivers/uart_inverter.c +++ b/src/main/drivers/uart_inverter.c @@ -145,7 +145,12 @@ void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable // UART4 #if defined(INVERTER_PIN_UART4_RX) || defined(INVERTER_PIN_UART4_TX) - if (USARTx == USART4) { +#if defined(STM32F4) + if (USARTx == UART4) +#else + if (USARTx == USART4) +#endif + { #if defined(INVERTER_PIN_UART4_RX) rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_RX)); #endif From ec014c51f5240bcb5dce4399aa7c644db7419e78 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Tue, 5 Nov 2024 20:47:17 +0000 Subject: [PATCH 132/175] Added uplink RSSI dBm to programming framework --- src/main/programming/logic_condition.c | 8 ++++++++ src/main/programming/logic_condition.h | 1 + 2 files changed, 9 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index facc595c3f4..5b7310b4b1f 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -804,6 +804,14 @@ static int logicConditionGetFlightOperandValue(int operand) { #endif break; + case LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM: +#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) + return rxLinkStatistics.uplinkRSSI; +#else + return 0; +#endif + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK: #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) return rxLinkStatistics.downlinkLQ; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index aaa80d51d53..f5653bb68cc 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -145,6 +145,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42 LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43 LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44 + LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM, // 45 } logicFlightOperands_e; typedef enum { From 24c1ed91f9cfe2e926bed9431d5b77857f56c9cd Mon Sep 17 00:00:00 2001 From: jamming Date: Thu, 7 Nov 2024 15:21:50 +0800 Subject: [PATCH 133/175] Add spa06 and spl07 baro for Kakutef4/f7 FC --- src/main/target/KAKUTEF4/target.h | 2 ++ src/main/target/KAKUTEF7/target.h | 2 ++ src/main/target/KAKUTEF7MINIV3/target.h | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index c1ce47b9e80..92cbcf13414 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -78,6 +78,8 @@ # define BARO_I2C_BUS BUS_I2C1 # define USE_BARO_MS5611 # define USE_BARO_BMP280 +# define USE_BARO_DPS310 +# define USE_BARO_SPL06 #else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP # define USE_BARO # define USE_MAG diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index f3f4edfed40..79f9d079353 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -131,6 +131,8 @@ #define USE_BARO #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 #define BARO_I2C_BUS BUS_I2C1 #define USE_MAG diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index bf485ebc82b..89ba7c632b5 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -117,6 +117,8 @@ #define BARO_I2C_BUS BUS_I2C1 #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 /* * Mag From e72354e389b2b0c9f6b4354099b6afd27bb8467c Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:27:08 -0300 Subject: [PATCH 134/175] Bugfix --- src/main/io/osd.c | 116 +++++++++--------- src/main/navigation/navigation.c | 36 ++++-- src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_geozone.c | 81 ++++++------ .../navigation_geozone_calculations.h | 3 +- 5 files changed, 129 insertions(+), 110 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index efcfa394f65..444978dc8c1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5867,6 +5867,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ @@ -5905,64 +5963,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } -#ifdef USE_GEOZONE - char buf[12], buf1[12]; - switch (geozone.messageState) { - case GEOZONE_MESSAGE_STATE_NFZ: - messages[messageCount++] = OSD_MSG_NFZ; - break; - case GEOZONE_MESSAGE_STATE_LEAVING_FZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); - tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: - messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; - break; - case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0); - if (geozone.zoneInfo == INT32_MAX) { - tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); - } else { - osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); - } - tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_FB: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: - messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: - messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: - messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_LOITER: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } -#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bbf8e7910f0..fcde4980f44 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -648,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE, } }, @@ -3606,14 +3607,15 @@ bool isProbablyStillFlying(void) *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { + + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + #ifdef USE_GEOZONE - if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || - (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { return 0.0f; } #endif - - const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4253,6 +4255,14 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6e2ca8fbcbc..1f545b7353f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,7 +131,7 @@ typedef enum { GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, - GEOZONE_MESSAGE_STATE_LOITER + GEOZONE_MESSAGE_STATE_POS_HOLD } geozoneMessageState_e; enum fenceAction_e { @@ -197,6 +197,7 @@ typedef struct geozone_s { int32_t zoneInfo; int32_t currentzoneMaxAltitude; int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; bool sticksLocked; int8_t loiterDir; bool avoidInRTHInProgress; diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 8a2505a0ff9..80b9cf4def0 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -58,7 +58,7 @@ #define MAX_DISTANCE_FLY_OVER_POINTS 50000 #define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) #define POS_DETECTION_DISTANCE 7500 -#define STICK_LOCK_MIN_TIME 1500 +#define STICK_LOCK_MIN_TIME 2500 #define AVOID_TIMEOUT 30000 #define MAX_LOCAL_VERTICES 128 #define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m @@ -109,6 +109,7 @@ static geoZoneConfig_t safeHomeGeozoneConfig; static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; static timeMs_t actionStartTime = 0; static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestHorZone = NULL; static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; static fpVector3_t avoidingPoint; static bool geozoneIsEnabled = false; @@ -650,6 +651,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp return 0; } + // Set starting point slightly away from our current position float offset = geozoneGetDetectionDistance(); if (geozone.distanceVertToNearestZone <= offset) { int bearing = wrap_36000(geozone.directionToNearestZone + 18000); @@ -660,7 +662,11 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp pathPoints[pathPointCount].visited = true; pathPoints[pathPointCount].distance = 0; pathPoints[pathPointCount++].point = start; - + + // Calculate possible waypoints + // Vertices of the zones are possible waypoints, + // inclusive zones are “reduced”, exclusive zones are “enlarged” to keep distance, + // round zones are converted into hexagons and long sides get additional points to be able to fly over zones. for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { fpVector2_t *verticesZone; fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; @@ -748,7 +754,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp while (!pathPoints[pathPointCount - 1].visited) { pathPoint_t *next = current; float min = FLT_MAX; - for (uint16_t i = 1; i < pathPointCount; i++) { + for (uint8_t i = 1; i < pathPointCount; i++) { float currentDist = FLT_MAX; if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { @@ -813,7 +819,7 @@ static void updateZoneInfos(void) fpVector3_t nearestBorderPoint; aboveOrUnderZone = false; - geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + nearestHorZone = NULL; geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; uint8_t currentZoneCount = getCurrentZones(currentZones, true); int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; @@ -821,11 +827,8 @@ static void updateZoneInfos(void) if (currentZoneCount == 1) { currentMaxAltitude = currentZones[0]->config.maxAltitude; currentMinAltitude = currentZones[0]->config.minAltitude; - - if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { - nearestHorInclZone = currentZones[0]; - } - + nearestHorZone = currentZones[0]; + } else if (currentZoneCount >= 2) { geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; @@ -837,6 +840,7 @@ static void updateZoneInfos(void) if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + nearestHorZone = current; } if (current->config.minAltitude > getEstimatedActualPosition(Z)) { @@ -859,16 +863,16 @@ static void updateZoneInfos(void) if (aboveZone) { if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); - nearestHorInclZone = aboveZone; + nearestHorZone = aboveZone; } else { currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); } } if (belowZone) { - if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); - nearestHorInclZone = belowZone; + nearestHorZone = belowZone; } else { currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); } @@ -959,18 +963,19 @@ static void updateZoneInfos(void) if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { directionToBorder = wrap_36000(directionToBorder + 18000); } - geozone.directionToNearestZone = directionToBorder; geozone.distanceHorToNearestZone = roundf(dist); nearestInclusiveZone = &activeGeoZones[i]; } } - if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { - nearestInclusiveZone = nearestHorInclZone; + if (aboveOrUnderZone && nearestHorZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorZone; geozone.distanceHorToNearestZone = 0; } } + + geozone.nearestHorZoneHasAction = nearestHorZone && nearestHorZone->config.fenceAction != GEOFENCE_ACTION_NONE; } void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) @@ -1248,7 +1253,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) isInitalised = true; } - if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + if (!ARMING_FLAG(ARMED) || !isInitalised || activeGeoZonesCount == 0) { noZoneRTH = false; return; } @@ -1267,6 +1272,10 @@ void geozoneUpdate(timeUs_t curentTimeUs) updateCurrentZones(); updateZoneInfos(); + + if (STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + return; + } // User has switched to non geofence mode, end all actions and switch to mode from box input if (isNonGeozoneModeFromBoxInput()) { @@ -1315,7 +1324,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) return; case GEOZONE_ACTION_STATE_RTH: case GEOZONE_ACTION_STATE_POSHOLD: - geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + geozone.messageState = GEOZONE_MESSAGE_STATE_POS_HOLD; if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { geozone.sticksLocked = false; } @@ -1327,25 +1336,26 @@ void geozoneUpdate(timeUs_t curentTimeUs) break; } - bool currentZoneHasAction = false; - for (uint8_t i = 0; i < currentZoneCount; i++) { - if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { - currentZoneHasAction = true; - break; - } - } - - if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && + actionState == GEOZONE_ACTION_STATE_NONE && + geozone.nearestHorZoneHasAction && + ABS(geozone.distanceVertToNearestZone) > 0 && + ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { float targetAltitide = 0; - if (!geozone.insideFz) { - if (geozone.distanceVertToNearestZone < 0) { - targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + uint32_t extraSafteyAlt = geoZoneConfig()->safeAltitudeDistance * 0.25; + if (nearestHorZone->config.type == GEOZONE_TYPE_INCLUSIVE && geozone.insideFz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMaxAltitude - extraSafteyAlt; } else { - targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + targetAltitide = geozone.currentzoneMinAltitude + extraSafteyAlt; + } + } else if (nearestHorZone->config.type == GEOZONE_TYPE_EXCLUSIVE && !geozone.insideNfz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMinAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMaxAltitude + extraSafteyAlt; } - } else { - targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; } fpVector3_t targetPos; @@ -1374,6 +1384,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) lockRTZ = false; } + // RTZ: Return to zone: if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { @@ -1457,9 +1468,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) } if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { - - geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); - + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); int32_t minAltitude = intersectZone->config.minAltitude; int32_t maxAltitude = intersectZone->config.maxAltitude; if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { @@ -1474,7 +1483,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(distToMin) < ABS(distToMax)) { geozone.zoneInfo = distToMin; } else { - geozone.zoneInfo = (distToMax); + geozone.zoneInfo = distToMax; } } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 38849719af0..d29d7deb35f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -348,8 +348,7 @@ static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* dista if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { intersect.x = i1.x; intersect.y = i1.y; - } - else { + } else { intersect.x = i2.x; intersect.y = i2.y; } From 0971756190340c94ab48585e7a454e73287b387d Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sat, 2 Nov 2024 10:51:48 -0300 Subject: [PATCH 135/175] Inital commit --- src/main/CMakeLists.txt | 1 + src/main/common/vector.h | 83 + src/main/config/parameter_group_ids.h | 5 +- src/main/fc/cli.c | 258 ++- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 8 + src/main/fc/fc_msp.c | 98 + src/main/fc/fc_tasks.c | 23 + src/main/fc/runtime_config.h | 9 +- src/main/fc/settings.yaml | 52 + src/main/io/osd.c | 117 ++ src/main/io/osd.h | 22 +- src/main/io/osd_dji_hd.c | 2 + src/main/msp/msp_protocol_v2_inav.h | 7 +- src/main/navigation/navigation.c | 261 ++- src/main/navigation/navigation.h | 113 ++ src/main/navigation/navigation_fixedwing.c | 6 + src/main/navigation/navigation_geozone.c | 1598 +++++++++++++++++ .../navigation_geozone_calculations.h | 480 +++++ src/main/navigation/navigation_private.h | 31 + src/main/scheduler/scheduler.h | 4 + src/main/target/common.h | 1 + 22 files changed, 3171 insertions(+), 10 deletions(-) create mode 100755 src/main/navigation/navigation_geozone.c create mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..7f188974cd9 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -576,6 +576,7 @@ main_sources(COMMON_SRC navigation/navigation_pos_estimator_flow.c navigation/navigation_private.h navigation/navigation_rover_boat.c + navigation/navigation_geozone.c navigation/sqrt_controller.c navigation/sqrt_controller.h navigation/rth_trackback.c diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 449a0973b3d..737c29d56a7 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -29,6 +29,13 @@ typedef union { }; } fpVector3_t; +typedef union { + float v[2]; + struct { + float x,y; + }; +} fpVector2_t; + typedef struct { float m[3][3]; } fpMat3_t; @@ -116,3 +123,79 @@ static inline fpVector3_t * vectorScale(fpVector3_t * result, const fpVector3_t *result = ab; return result; } + +static inline fpVector3_t* vectorSub(fpVector3_t* result, const fpVector3_t* a, const fpVector3_t* b) +{ + fpVector3_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + ab.z = a->z - b->z; + + *result = ab; + return result; +} + +static inline float vectorDotProduct(const fpVector3_t* a, const fpVector3_t* b) +{ + return a->x * b->x + a->y * b->y + a->z * b->z; +} + +static inline float vector2NormSquared(const fpVector2_t * v) +{ + return sq(v->x) + sq(v->y); +} + +static inline fpVector2_t* vector2Normalize(fpVector2_t* result, const fpVector2_t* v) +{ + float sqa = vector2NormSquared(v); + float length = sqrtf(sqa); + if (length != 0) { + result->x = v->x / length; + result->y = v->y / length; + } else { + result->x = 0; + result->y = 0; + } + return result; +} + + +static inline fpVector2_t* vector2Sub(fpVector2_t* result, const fpVector2_t* a, const fpVector2_t* b) +{ + fpVector2_t ab; + + ab.x = a->x - b->x; + ab.y = a->y - b->y; + + *result = ab; + return result; +} + +static inline fpVector2_t * vector2Add(fpVector2_t * result, const fpVector2_t * a, const fpVector2_t * b) +{ + fpVector2_t ab; + + ab.x = a->x + b->x; + ab.y = a->y + b->y; + + *result = ab; + return result; +} + + +static inline float vector2DotProduct(const fpVector2_t* a, const fpVector2_t* b) +{ + return a->x * b->x + a->y * b->y; +} + +static inline fpVector2_t * vector2Scale(fpVector2_t * result, const fpVector2_t * a, const float b) +{ + fpVector2_t ab; + + ab.x = a->x * b; + ab.y = a->y * b; + + *result = ab; + return result; +} diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index b4062201c72..26715ab3cec 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -129,7 +129,10 @@ #define PG_GIMBAL_CONFIG 1039 #define PG_GIMBAL_SERIAL_CONFIG 1040 #define PG_HEADTRACKER_CONFIG 1041 -#define PG_INAV_END PG_HEADTRACKER_CONFIG +#define PG_GEOZONE_CONFIG 1042 +#define PG_GEOZONES 1043 +#define PG_GEOZONE_VERTICES 1044 +#define PG_INAV_END PG_GEOZONE_VERTICES // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cbe85496f1a..d5c115c66b1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -152,7 +152,7 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", - "", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", @@ -1561,6 +1561,251 @@ static void cliSafeHomes(char *cmdline) } #endif + +#if defined(USE_GEOZONE) +static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) +{ + const char *format = "geozone %u %u %u %d %d %u"; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultGeoZone) { + equalsDefault = geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].shape == defaultGeoZone->shape + && geoZone[i].type == defaultGeoZone->type + && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude + && geoZone[i].minAltitude == defaultGeoZone->minAltitude; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction); + } + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction); + } +} + +static void printGeozoneVertices(uint8_t dumpMask, const vertexConfig_t *vertices, const vertexConfig_t *defaultVertices) +{ + const char *format = "geozone vertex %d %u %d %d"; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + bool equalsDefault = false; + if (defaultVertices) { + equalsDefault = vertices[i].idx == defaultVertices->idx + && vertices[i].lat == defaultVertices->lat + && vertices[i].lon == defaultVertices->lon + && vertices[i].zoneId == defaultVertices->zoneId; + + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultVertices[i].zoneId, defaultVertices[i].idx, defaultVertices[i].lat, defaultVertices[i].lon); + } + + cliDumpPrintLinef(dumpMask, equalsDefault, format, vertices[i].zoneId, vertices[i].idx, vertices[i].lat, vertices[i].lon); + } + + if (!defaultVertices) { + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + } +} + +static void cliGeozone(char* cmdLine) +{ + if (isEmpty(cmdLine)) { + printGeozones(DUMP_MASTER, geoZonesConfig(0), NULL); + } else if (sl_strcasecmp(cmdLine, "vertex") == 0) { + printGeozoneVertices(DUMP_MASTER, geoZoneVertices(0), NULL); + } else if (sl_strncasecmp(cmdLine, "vertex reset", 12) == 0) { + const char* ptr = &cmdLine[12]; + uint8_t zoneId = 0, idx = 0; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + } else { + geozoneResetVertices(-1, -1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + idx = fastA2I(ptr); + } else { + geozoneResetVertices(zoneId, -1); + return; + } + + if (argumentCount != 2) { + cliShowParseError(); + return; + } + + geozoneResetVertices(zoneId, idx); + + } else if (sl_strncasecmp(cmdLine, "vertex", 6) == 0) { + int32_t lat = 0, lon = 0; + int8_t zoneId = 0; + int16_t vertexIdx = -1; + uint8_t vertexZoneIdx = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + if ((ptr = nextArg(ptr))) { + zoneId = fastA2I(ptr); + if (zoneId < 0) { + return; + } + + if (zoneId >= MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + vertexZoneIdx = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lat = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + lon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + } + + if (argumentCount != 4) { + cliShowParseError(); + return; + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexZoneIdx) { + geoZoneVerticesMutable(i)->lat = lat; + geoZoneVerticesMutable(i)->lon = lon; + return; + } + } + + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexIdx = i; + break; + } + } + + if (vertexIdx < 0 || vertexIdx >= MAX_VERTICES_IN_CONFIG || vertexZoneIdx > MAX_VERTICES_IN_CONFIG) { + cliPrintError("Maximum number of vertices reached."); + return; + } + + geoZoneVerticesMutable(vertexIdx)->lat = lat; + geoZoneVerticesMutable(vertexIdx)->lon = lon; + geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; + geoZonesConfigMutable(zoneId)->vertexCount++; + + uint8_t totalVertices = geozoneGetUsedVerticesCount(); + cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + + } else { + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0; + int32_t minAltitude = 0, maxAltitude = 0; + const char* ptr = cmdLine; + uint8_t argumentCount = 1; + + idx = fastA2I(ptr); + if (idx < 0 || idx > MAX_GEOZONES_IN_CONFIG) { + cliShowArgumentRangeError("geozone index", 0, MAX_GEOZONES_IN_CONFIG - 1); + return; + } + + if ((ptr = nextArg(ptr))) { + argumentCount++; + isPolygon = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + isInclusive = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + minAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + maxAltitude = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + fenceAction = fastA2I(ptr); + if (fenceAction < 0 || fenceAction > GEOFENCE_ACTION_RTH) { + cliShowArgumentRangeError("fence action", 0, GEOFENCE_ACTION_RTH); + return; + } + } else { + cliShowParseError(); + return; + } + + if ((ptr = nextArg(ptr))){ + argumentCount++; + } + + if (argumentCount != 6) { + cliShowParseError(); + return; + } + + if (isPolygon) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_POLYGON; + } else { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + } + + if (isInclusive) { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_INCLUSIVE; + } else { + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + } + + geoZonesConfigMutable(idx)->maxAltitude = maxAltitude; + geoZonesConfigMutable(idx)->minAltitude = minAltitude; + geoZonesConfigMutable(idx)->fenceAction = fenceAction; + } +} +#endif + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) && defined(NAV_NON_VOLATILE_WAYPOINT_CLI) static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint) { @@ -4178,6 +4423,14 @@ static void printConfig(const char *cmdline, bool doDiff) printFwAutolandApproach(dumpMask, fwAutolandApproachConfig_CopyArray, fwAutolandApproachConfig(0)); #endif +#if defined(USE_GEOZONE) + cliPrintHashLine("geozone"); + printGeozones(dumpMask, geoZonesConfig_CopyArray, geoZonesConfig(0)); + + cliPrintHashLine("geozone vertices"); + printGeozoneVertices(dumpMask, geoZoneVertices_CopyArray, geoZoneVertices(0)); +#endif + cliPrintHashLine("features"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); @@ -4561,6 +4814,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("fwapproach", "Fixed Wing Approach Settings", NULL, cliFwAutolandApproach), #endif CLI_COMMAND_DEF("get", "get variable value", "[name]", cliGet), +#ifdef USE_GEOZONE + CLI_COMMAND_DEF("geozone", "get or set geo zones", NULL, cliGeozone), +#endif #ifdef USE_GPS CLI_COMMAND_DEF("gpspassthrough", "passthrough gps to serial", NULL, cliGpsPassthrough), CLI_COMMAND_DEF("gpssats", "show GPS satellites", NULL, cliUbloxPrintSatelites), diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ddb6a826b83..17c8ded8c1e 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -36,7 +36,7 @@ typedef enum { FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3, - FEATURE_UNUSED_12 = 1 << 4, //was FEATURE_MOTOR_STOP + FEATURE_GEOZONE = 1 << 4, //was FEATURE_MOTOR_STOP FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f7603eee692..ba29a03d5e0 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,6 +278,14 @@ static void updateArmingStatus(void) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + if (geozoneIsInsideNFZ()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } +#endif + /* CHECK: */ if ( sensors(SENSOR_ACC) && diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f87b7f079c7..c875587ae5e 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1771,6 +1771,53 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) +static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, geoZonesConfig(idx)->type); + sbufWriteU8(dst, geoZonesConfig(idx)->shape); + sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); + sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); + sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); + sbufWriteU8(dst, idx); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + +static mspResult_e mspFcGeozoneVerteciesOutCommand(sbuf_t *dst, sbuf_t *src) +{ + const uint8_t zoneId = sbufReadU8(src); + const uint8_t vertexId = sbufReadU8(src); + if (zoneId < MAX_GEOZONES_IN_CONFIG) { + int8_t vertexIdx = geozoneGetVertexIdx(zoneId, vertexId); + if (vertexIdx >= 0) { + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->zoneId); + sbufWriteU8(dst, geoZoneVertices(vertexIdx)->idx); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lat); + sbufWriteU32(dst, geoZoneVertices(vertexIdx)->lon); + if (geoZonesConfig(zoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + int8_t vertexRadiusIdx = geozoneGetVertexIdx(zoneId, vertexId + 1); + if (vertexRadiusIdx >= 0) { + sbufWriteU32(dst, geoZoneVertices(vertexRadiusIdx)->lat); + } else { + return MSP_RESULT_ERROR; + } + } + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } + } else { + return MSP_RESULT_ERROR; + } +} +#endif + static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); if (idx < MAX_LOGIC_CONDITIONS) { @@ -3318,6 +3365,49 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_SET_GEOZONE: + if (dataSize == 13) { + uint8_t geozoneId; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + + geozoneResetVertices(geozoneId, -1); + geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); + geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); + } else { + return MSP_RESULT_ERROR; + } + break; + case MSP2_INAV_SET_GEOZONE_VERTEX: + if (dataSize == 10 || dataSize == 14) { + uint8_t geozoneId = 0; + if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { + return MSP_RESULT_ERROR; + } + uint8_t vertexId = sbufReadU8(src); + int32_t lat = sbufReadU32(src); + int32_t lon = sbufReadU32(src); + if (!geozoneSetVertex(geozoneId, vertexId, lat, lon)) { + return MSP_RESULT_ERROR; + } + + if (geoZonesConfig(geozoneId)->shape == GEOZONE_SHAPE_CIRCULAR) { + if (!geozoneSetVertex(geozoneId, vertexId + 1, sbufReadU32(src), 0)) { + return MSP_RESULT_ERROR; + } + } + } else { + return MSP_RESULT_ERROR; + } + break; +#endif + #ifdef USE_EZ_TUNE case MSP2_INAV_EZ_TUNE_SET: @@ -3896,6 +3986,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFwApproachOutCommand(dst, src); break; #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + case MSP2_INAV_GEOZONE: + *ret = mspFcGeozoneOutCommand(dst, src); + break; + case MSP2_INAV_GEOZONE_VERTEX: + *ret = mspFcGeozoneVerteciesOutCommand(dst, src); + break; +#endif #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index afb880db526..df335ccb428 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -339,6 +339,15 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } +#if defined(USE_GEOZONE) && defined (USE_GPS) +void geozoneUpdateTask(timeUs_t currentTimeUs) +{ + if (feature(FEATURE_GEOZONE)) { + geozoneUpdate(currentTimeUs); + } +} +#endif + void fcTasksInit(void) { schedulerInit(); @@ -453,6 +462,11 @@ void fcTasksInit(void) #if defined(SITL_BUILD) serialProxyStart(); #endif + +#if defined(USE_GEOZONE) && defined (USE_GPS) + setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); +#endif + } cfTask_t cfTasks[TASK_COUNT] = { @@ -740,4 +754,13 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif +#if defined(USE_GEOZONE) && defined (USE_GPS) + [TASK_GEOZONE] = { + .taskName = "GEOZONE", + .taskFunc = geozoneUpdateTask, + .desiredPeriod = TASK_PERIOD_HZ(5), + .staticPriority = TASK_PRIORITY_MEDIUM, + }, +#endif + }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 974f90f8c4d..637ea867e64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -24,6 +24,7 @@ typedef enum { SIMULATOR_MODE_HITL = (1 << 4), SIMULATOR_MODE_SITL = (1 << 5), + ARMING_DISABLED_GEOZONE = (1 << 6), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), @@ -49,8 +50,8 @@ typedef enum { ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_LANDING_DETECTED = (1 << 30), - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | - ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | + ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | @@ -65,7 +66,8 @@ typedef enum { // situations where we might just need the motors to spin so the // aircraft can move (even unpredictably) and get unstuck (e.g. // crashed into a high tree). -#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_NOT_LEVEL \ +#define ARMING_DISABLED_EMERGENCY_OVERRIDE (ARMING_DISABLED_GEOZONE \ + | ARMING_DISABLED_NOT_LEVEL \ | ARMING_DISABLED_NAVIGATION_UNSAFE \ | ARMING_DISABLED_COMPASS_NOT_CALIBRATED \ | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED \ @@ -106,6 +108,7 @@ typedef enum { SOARING_MODE = (1 << 16), ANGLEHOLD_MODE = (1 << 17), NAV_FW_AUTOLAND = (1 << 18), + NAV_SEND_TO = (1 << 19), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fd6073fa4bd..490fab4b914 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -204,6 +204,11 @@ tables: - name: default_altitude_source values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"] enum: navDefaultAltitudeSensor_e + - name: fence_action + values: ["NONE", "AVOID", "POS_HOLD", "RTH"] + enum: fenceAction_e + - name: geozone_rth_no_way_home + values: [RTH, EMRG_LAND] constants: RPYL_PID_MIN: 0 @@ -3824,6 +3829,7 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source + condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource @@ -4362,3 +4368,49 @@ groups: field: roll_ratio min: 0 max: 5 + - name: PG_GEOZONE_CONFIG + type: geozone_config_t + headers: ["navigation/navigation.h"] + condition: USE_GEOZONE && USE_GPS + members: + - name: geozone_detection_distance + description: "Distance from which a geozone is detected" + default_value: 50000 + field: fenceDetectionDistance + min: 0 + max: 1000000 + - name: geozone_avoid_altitude_range + description: "Altitude range in which an attempt is made to avoid a geozone upwards" + default_value: 5000 + field: avoidAltitudeRange + min: 0 + max: 1000000 + - name: geozone_safe_altitude_distance + description: "Vertical distance that must be maintained to the upper and lower limits of the zone." + default_value: 1000 + field: safeAltitudeDistance + min: 0 + max: 10000 + - name: geozone_safehome_as_inclusive + description: "Treat nearest safehome as inclusive geozone" + type: bool + field: nearestSafeHomeAsInclusivZone + default_value: OFF + - name: geozone_safehome_zone_action + description: "Fence action for safehome zone" + default_value: "NONE" + field: safeHomeFenceAction + table: fence_action + type: uint8_t + - name: geozone_mr_stop_distance + description: "Distance in which multirotors stops before the border" + default_value: 15000 + min: 0 + max: 100000 + field: copterFenceStopDistance + - name: geozone_no_way_home_action + description: "Action if RTH with active geozones is unable to calculate a course to home" + default_value: RTH + field: noWayHomeAction + table: geozone_rth_no_way_home + type: uint8_t diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 41c20dda928..0307f63c1ab 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,6 +866,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + case ARMING_DISABLED_GEOZONE: + return OSD_MESSAGE_STR(OSD_MSG_NFZ); // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; @@ -2377,6 +2379,11 @@ static bool osdDrawSingleElement(uint8_t item) if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else +#endif +#ifdef USE_GEOZONE + if (FLIGHT_MODE(NAV_SEND_TO)) + p = "AUTO"; + else #endif if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; @@ -3837,6 +3844,52 @@ static bool osdDrawSingleElement(uint8_t item) clearMultiFunction = true; break; } +#if defined(USE_GEOZONE) + case OSD_COURSE_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + int16_t panHomeDirOffset = 0; + if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + panHomeDirOffset = osdGetPanServoOffset(); + } + int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); + int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset; + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction); + } else { + if (isGeozoneActive()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, '-', elemAttr); + } + break; + } + + case OSD_H_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatDistanceSymbol(buff2, geozone.distanceHorToNearestZone, 0, 3); + tfp_sprintf(buff, "FD %s", buff2 ); + } else { + strcpy(buff, "FD ---"); + } + } + break; + + case OSD_V_DIST_TO_FENCE: + { + if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { + char buff2[12]; + osdFormatAltitudeSymbol(buff2, abs(geozone.distanceVertToNearestZone)); + tfp_sprintf(buff, "FD%s", buff2); + displayWriteCharWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, geozone.distanceVertToNearestZone < 0 ? SYM_DECORATION + 4 : SYM_DECORATION, elemAttr); + } else { + strcpy(buff, "FD ---"); + } + + break; + } +#endif default: return false; @@ -5890,6 +5943,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = safehomeMessage; } #endif + +#ifdef USE_GEOZONE + if (geozone.avoidInRTHInProgress) { + messages[messageCount++] = OSD_MSG_AVOID_ZONES_RTH; + } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while armed if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) { uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis()); @@ -5952,6 +6011,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_LOITER: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b0423d40eff..69d5433e0a5 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -132,6 +132,19 @@ #define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif +#if defined(USE_GEOZONE) +#define OSD_MSG_NFZ "NO FLY ZONE" +#define OSD_MSG_LEAVING_FZ "LEAVING FZ IN %s" +#define OSD_MSG_OUTSIDE_FZ "OUTSIDE FZ" +#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s" +#define OSD_MSG_AVOIDING_FB "AVOIDING FENCE BREACH" +#define OSD_MSG_RETURN_TO_ZONE "RETURN TO FZ" +#define OSD_MSG_FLYOUT_NFZ "FLY OUT NFZ" +#define OSD_MSG_AVOIDING_ALT_BREACH "REACHED ZONE ALTITUDE LIMIT" +#define OSD_MSG_AVOID_ZONES_RTH "AVOIDING NO FLY ZONES" +#define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -291,7 +304,10 @@ typedef enum { OSD_CUSTOM_ELEMENT_5, OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, - OSD_CUSTOM_ELEMENT_8, // 158 + OSD_CUSTOM_ELEMENT_8, + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -474,6 +490,10 @@ typedef struct osdConfig_s { uint16_t adsb_ignore_plane_above_me_limit; // in metres #endif uint8_t radar_peers_display_time; // in seconds +#ifdef USE_GEOZONE + uint8_t geozoneDistanceWarning; // Distance to fence or action + bool geozoneDistanceType; // Shows a countdown timer or distance to fence/action +#endif } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig); diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index b018d8f9068..c0120c7faa1 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -525,6 +525,8 @@ static char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message + case ARMING_DISABLED_GEOZONE: + return OSD_MESSAGE_STR("NO FLY ZONE"); case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; case ARMING_DISABLED_CMS_MENU: diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 465f5098536..dcbdeb5e71f 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -114,4 +114,9 @@ #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS 0x2102 #define MSP2_INAV_SERVO_CONFIG 0x2200 -#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 \ No newline at end of file +#define MSP2_INAV_SET_SERVO_CONFIG 0x2201 + +#define MSP2_INAV_GEOZONE 0x2210 +#define MSP2_INAV_SET_GEOZONE 0x2211 +#define MSP2_INAV_GEOZONE_VERTEX 0x2212 +#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213 \ No newline at end of file diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4ae4cf228db..bbf8e7910f0 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -356,6 +356,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState); #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState); +#endif static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -377,6 +382,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -449,6 +455,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, /** CRUISE_HOLD mode ************************************************/ @@ -485,6 +492,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -507,6 +515,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -544,6 +553,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -566,6 +576,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_SEND_TO] = NAV_STATE_SEND_TO_INITALIZE, } }, @@ -1178,6 +1189,63 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, #endif + +#ifdef USE_GEOZONE + [NAV_STATE_SEND_TO_INITALIZE] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_INITALIZE, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_IN_PROGESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_SEND_TO_IN_PROGESS] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_IN_PROGESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_SEND_TO_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, + [NAV_STATE_SEND_TO_FINISHED] = { + .persistentId = NAV_PERSISTENT_ID_SEND_TO_FINISHED, + .onEntry = navOnEnteringState_NAV_STATE_SEND_TO_FINISHED, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, + .mapToFlightModes = NAV_SEND_TO, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_SEND_TO_FINISHED, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, +#endif }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1444,6 +1512,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // If we have valid position sensor or configured to ignore it's loss at initial stage - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) { // Prepare controllers +#ifdef USE_GEOZONE + geozoneResetRTH(); + geozoneSetupRTH(); +#endif resetPositionController(); resetAltitudeController(false); // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL setupAltitudeController(); @@ -1610,6 +1682,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio // If we have position sensor - continue home if ((posControl.flags.estPosStatus >= EST_USABLE)) { +#ifdef USE_GEOZONE + // Check for NFZ in our way + int8_t wpCount = geozoneCheckForNFZAtCourse(true); + if (wpCount > 0) { + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + return NAV_FSM_EVENT_NONE; + } else if (geozone.avoidInRTHInProgress) { + if (isWaypointReached(geozoneGetCurrentRthAvoidWaypoint(), &posControl.activeWaypoint.bearing)) { + if (geoZoneIsLastRthWaypoint()) { + // Already at Home? + fpVector3_t *tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_SUCCESS; + } + + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; + } else { + geozoneAdvanceRthAvoidWaypoint(); + calculateAndSetActiveWaypointToLocalPosition(geozoneGetCurrentRthAvoidWaypoint()); + return NAV_FSM_EVENT_NONE; + } + } + setDesiredPosition(geozoneGetCurrentRthAvoidWaypoint(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + return NAV_FSM_EVENT_NONE; + } else if (wpCount < 0 && geoZoneConfig()->noWayHomeAction == NO_WAY_HOME_ACTION_EMRG_LAND) { + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; + } else { +#endif fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { @@ -1626,6 +1728,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); return NAV_FSM_EVENT_NONE; } +#ifdef USE_GEOZONE + } +#endif } /* Position sensor failure timeout - land */ else if (checkForPositionSensorTimeout()) { @@ -1797,6 +1902,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME } +#ifdef USE_GEOZONE + geozoneResetRTH(); +#endif + // Prevent I-terms growing when already landed pidResetErrorAccumulators(); return NAV_FSM_EVENT_NONE; @@ -2452,6 +2561,64 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga } #endif +#ifdef USE_GEOZONE +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_INITALIZE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + if (checkForPositionSensorTimeout()) { + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + resetPositionController(); + resetAltitudeController(false); + + if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST); + } else { + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + } + + posControl.sendTo.startTime = millis(); + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // "Send to" is designed to kick in even user is making inputs, lock sticks for a short time to avoid the mode ends immediately + if (posControl.sendTo.lockSticks && millis() - posControl.sendTo.startTime > posControl.sendTo.lockStickTime) { + posControl.sendTo.lockSticks = false; + } + + if (!posControl.sendTo.lockSticks && areSticksDeflected()) { + abortSendTo(); + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + } + + if (calculateDistanceToDestination(&posControl.sendTo.targetPos) <= posControl.sendTo.targetRange ) { + if (posControl.sendTo.altitudeTargetRange > 0) { + if ((getEstimatedActualPosition(Z) > posControl.sendTo.targetPos.z - posControl.sendTo.altitudeTargetRange) && (getEstimatedActualPosition(Z) < posControl.sendTo.targetPos.z + posControl.sendTo.altitudeTargetRange)) { + return NAV_FSM_EVENT_SUCCESS; + } else { + return NAV_FSM_EVENT_NONE; + } + } + return NAV_FSM_EVENT_SUCCESS; + } + return NAV_FSM_EVENT_NONE; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_SEND_TO_FINISHED(navigationFSMState_t previousState) +{ + UNUSED(previousState); + posControl.sendTo.lockSticks = false; + posControl.flags.sendToActive = false; + return NAV_FSM_EVENT_NONE; +} +#endif + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -2539,6 +2706,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) switch (mode) { case RTH_HOME_ENROUTE_INITIAL: posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthInitialAltitude; +#ifdef USE_GEOZONE + if (geozone.currentzoneMaxAltitude > 0) { + posControl.rthState.homeTmpWaypoint.z = MIN(geozone.currentzoneMaxAltitude, posControl.rthState.homeTmpWaypoint.z); + } +#endif break; case RTH_HOME_ENROUTE_PROPORTIONAL: @@ -2552,6 +2724,11 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude; } } +#ifdef USE_GEOZONE + if (geozone.distanceVertToNearestZone < 0 && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + posControl.rthState.homeTmpWaypoint.z += geoZoneConfig()->safeAltitudeDistance; + } +#endif break; case RTH_HOME_ENROUTE_FINAL: @@ -2757,6 +2934,17 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void) return posControl.flags.isTerrainFollowEnabled ? &posControl.actualState.agl : &posControl.actualState.abs; } +/*----------------------------------------------------------- + * Calculates 2D distance between two points + *-----------------------------------------------------------*/ +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos) +{ + const float deltaX = destinationPos->x - startPos->x; + const float deltaY = destinationPos->y - startPos->y; + + return calc_length_pythagorean_2D(deltaX, deltaY); +} + /*----------------------------------------------------------- * Calculates distance and bearing to destination point *-----------------------------------------------------------*/ @@ -2944,6 +3132,11 @@ static void updateDesiredRTHAltitude(void) posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.actualState.abs.pos.z; } +#if defined(USE_GEOZONE) + if (geozone.homeHasMaxAltitue) { + posControl.rthState.rthFinalAltitude = MIN(posControl.rthState.rthFinalAltitude, geozone.maxHomeAltitude); + } +#endif } /*----------------------------------------------------------- @@ -3154,6 +3347,9 @@ void updateHomePosition(void) setHome = true; break; } +#if defined(USE_GEOZONE) && defined (USE_GPS) + geozoneUpdateMaxHomeAltitude(); +#endif } } else { @@ -3409,7 +3605,14 @@ bool isProbablyStillFlying(void) * Z-position controller *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) -{ +{ +#ifdef USE_GEOZONE + if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + return 0.0f; + } +#endif + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; @@ -4095,6 +4298,10 @@ void applyWaypointNavigationAndAltitudeHold(void) posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback resetRthTrackBack(); + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; +#endif return; } @@ -4145,7 +4352,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE | NAV_SEND_TO) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -4296,6 +4503,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } + if (posControl.flags.sendToActive) { + return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; + } + + if (posControl.flags.forcedPosholdActive) { + return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; + } + /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission * auto restarting after interruption by Manual or RTH modes. @@ -4769,6 +4984,14 @@ void navigationInit(void) #ifdef USE_MULTI_MISSION posControl.multiMissionCount = 0; #endif + +#ifdef USE_GEOZONE + posControl.flags.sendToActive = false; + posControl.sendTo.lockSticks = false; + posControl.sendTo.lockStickTime = 0; + posControl.sendTo.startTime = 0; + posControl.sendTo.targetRange = 0; +#endif /* Set initial surface invalid */ posControl.actualState.surfaceMin = -1.0f; @@ -4851,6 +5074,40 @@ rthState_e getStateOfForcedRTH(void) } } + +#ifdef USE_GEOZONE +// "Send to" is not to intended to be activated by user, only by external event +void activateSendTo(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.sendToActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortSendTo(void) +{ + posControl.flags.sendToActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void activateForcedPosHold(void) +{ + if (!geozone.avoidInRTHInProgress) { + abortFixedWingLaunch(); + posControl.flags.forcedPosholdActive = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); + } +} + +void abortForcedPosHold(void) +{ + posControl.flags.forcedPosholdActive = false; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} +#endif + /*----------------------------------------------------------- * Ability to execute Emergency Landing on external event *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 24b8daa0402..ad6d60ee06f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -116,6 +116,119 @@ void resetFwAutolandApproach(int8_t idx); #endif +#if defined(USE_GEOZONE) + +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 + +typedef enum { + GEOZONE_MESSAGE_STATE_NONE, + GEOZONE_MESSAGE_STATE_NFZ, + GEOZONE_MESSAGE_STATE_LEAVING_FZ, + GEOZONE_MESSAGE_STATE_OUTSIDE_FZ, + GEOZONE_MESSAGE_STATE_ENTERING_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_FB, + GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, + GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, + GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, + GEOZONE_MESSAGE_STATE_LOITER +} geozoneMessageState_e; + +enum fenceAction_e { + GEOFENCE_ACTION_NONE, + GEOFENCE_ACTION_AVOID, + GEOFENCE_ACTION_POS_HOLD, + GEOFENCE_ACTION_RTH, +}; + +enum noWayHomeAction { + NO_WAY_HOME_ACTION_RTH, + NO_WAY_HOME_ACTION_EMRG_LAND, +}; + +#define GEOZONE_SHAPE_CIRCULAR 0 +#define GEOZONE_SHAPE_POLYGON 1 + +#define GEOZONE_TYPE_EXCLUSIVE 0 +#define GEOZONE_TYPE_INCLUSIVE 1 + +typedef struct geoZoneConfig_s +{ + uint8_t shape; + uint8_t type; + int32_t minAltitude; + int32_t maxAltitude; + uint8_t fenceAction; + uint8_t vertexCount; +} geoZoneConfig_t; + +typedef struct geozone_config_s +{ + uint32_t fenceDetectionDistance; + uint16_t avoidAltitudeRange; + uint16_t safeAltitudeDistance; + bool nearestSafeHomeAsInclusivZone; + uint8_t safeHomeFenceAction; + uint32_t copterFenceStopDistance; + uint8_t noWayHomeAction; +} geozone_config_t; + +typedef struct vertexConfig_s +{ + int8_t zoneId; + uint8_t idx; + int32_t lat; + int32_t lon; +} vertexConfig_t; + +PG_DECLARE(geozone_config_t, geoZoneConfig); +PG_DECLARE_ARRAY(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig); +PG_DECLARE_ARRAY(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices); + +typedef struct geozone_s { + bool insideFz; + bool insideNfz; + uint32_t distanceToZoneBorder3d; + int32_t vertDistanceToZoneBorder; + geozoneMessageState_e messageState; + int32_t directionToNearestZone; + int32_t distanceHorToNearestZone; + int32_t distanceVertToNearestZone; + int32_t zoneInfo; + int32_t currentzoneMaxAltitude; + int32_t currentzoneMinAltitude; + bool sticksLocked; + int8_t loiterDir; + bool avoidInRTHInProgress; + int32_t maxHomeAltitude; + bool homeHasMaxAltitue; +} geozone_t; + +extern geozone_t geozone; + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon); +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId); +bool isGeozoneActive(void); +uint8_t geozoneGetUsedVerticesCount(void); +void geozoneResetVertices(int8_t zoneId, int16_t idx); +void geozoneUpdate(timeUs_t curentTimeUs); +bool geozoneIsInsideNFZ(void); +void geozoneAdvanceRthAvoidWaypoint(void); +int8_t geozoneCheckForNFZAtCourse(bool isRTH); +bool geoZoneIsLastRthWaypoint(void); +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void); +void geozoneSetupRTH(void); +void geozoneResetRTH(void); +void geozoneUpdateMaxHomeAltitude(void); +uint32_t geozoneGetDetectionDistance(void); + +void activateSendTo(void); +void abortSendTo(void); +void activateForcedPosHold(void); +void abortForcedPosHold(void); + +#endif + #ifndef NAV_MAX_WAYPOINTS #define NAV_MAX_WAYPOINTS 15 #endif diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index ff38b2e17e5..87165cd7d6f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -250,6 +250,12 @@ static int8_t loiterDirection(void) { dir *= -1; } +#ifdef USE_GEOZONE + if (geozone.loiterDir != 0) { + dir = geozone.loiterDir; + } +#endif + return dir; } diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c new file mode 100755 index 00000000000..8a2505a0ff9 --- /dev/null +++ b/src/main/navigation/navigation_geozone.c @@ -0,0 +1,1598 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include "common/utils.h" +#include "common/vector.h" +#include "common/printf.h" + +#include "config/config_reset.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/rc_modes.h" +#include "fc/rc_controls.h" +#include "fc/settings.h" + +#include "flight/imu.h" + +#include "rx/rx.h" + +#include "scheduler/scheduler.h" + +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#if defined(USE_GEOZONE) && defined (USE_GPS) + +#include "navigation_geozone_calculations.h" + +#define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) +#define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home + +#define MAX_DISTANCE_FLY_OVER_POINTS 50000 +#define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) +#define POS_DETECTION_DISTANCE 7500 +#define STICK_LOCK_MIN_TIME 1500 +#define AVOID_TIMEOUT 30000 +#define MAX_LOCAL_VERTICES 128 +#define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m +#define STICK_MOVE_THRESHOULD 40 +#define MAX_RTH_WAYPOINTS (MAX_VERTICES / 2) +#define GEOZONE_INACTIVE INT8_MAX +#define RTH_OVERRIDE_TIMEOUT 1000 + +#define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) + +typedef enum { + GEOZONE_ACTION_STATE_NONE, + GEOZONE_ACTION_STATE_AVOIDING, + GEOZONE_ACTION_STATE_AVOIDING_UPWARD, + GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE, + GEOZONE_ACTION_STATE_RETURN_TO_FZ, + GEOZONE_ACTION_STATE_FLYOUT_NFZ, + GEOZONE_ACTION_STATE_POSHOLD, + GEOZONE_ACTION_STATE_RTH +} geozoneActionState_e; + +typedef struct geoZoneRuntimeConfig_s +{ + geoZoneConfig_t config; + bool enable; + bool isInfZone; + uint32_t radius; + fpVector2_t *verticesLocal; +} geoZoneRuntimeConfig_t; + +typedef struct pathPoint_s pathPoint_t; +struct pathPoint_s { + fpVector3_t point; + pathPoint_t* prev; + float distance; + bool visited; +}; + +static bool isInitalised = false; +static geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; +static fpVector2_t verticesLocal[MAX_VERTICES]; +static uint8_t currentZoneCount = 0; + +static bool isAtLeastOneInclusiveZoneActive = false; +static geoZoneRuntimeConfig_t activeGeoZones[MAX_GEOZONES]; +static uint8_t activeGeoZonesCount = 0; +static geoZoneConfig_t safeHomeGeozoneConfig; +static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; +static timeMs_t actionStartTime = 0; +static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; +static fpVector3_t avoidingPoint; +static bool geozoneIsEnabled = false; +static fpVector3_t rthWaypoints[MAX_RTH_WAYPOINTS]; +static uint8_t rthWaypointIndex = 0; +static int8_t rthWaypointCount = 0; +static bool aboveOrUnderZone = false; +static timeMs_t rthOverrideStartTime; +static bool noZoneRTH = false; +static bool rthHomeSwitchLastState = false; +static bool lockRTZ = false; + +geozone_t geozone; + +PG_REGISTER_WITH_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, PG_GEOZONE_CONFIG, 0); + +PG_RESET_TEMPLATE(geozone_config_t, geoZoneConfig, + .fenceDetectionDistance = SETTING_GEOZONE_DETECTION_DISTANCE_DEFAULT, + .avoidAltitudeRange = SETTING_GEOZONE_AVOID_ALTITUDE_RANGE_DEFAULT, + .safeAltitudeDistance = SETTING_GEOZONE_SAFE_ALTITUDE_DISTANCE_DEFAULT, + .nearestSafeHomeAsInclusivZone = SETTING_GEOZONE_SAFEHOME_AS_INCLUSIVE_DEFAULT, + .copterFenceStopDistance = SETTING_GEOZONE_MR_STOP_DISTANCE_DEFAULT, + .noWayHomeAction = SETTING_GEOZONE_NO_WAY_HOME_ACTION_DEFAULT +); + +PG_REGISTER_ARRAY_WITH_RESET_FN(geoZoneConfig_t, MAX_GEOZONES_IN_CONFIG, geoZonesConfig, PG_GEOZONES, 1); + +void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) +{ + for (int i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + RESET_CONFIG(geoZoneConfig_t, &instance[i], + .shape = GEOZONE_TYPE_EXCLUSIVE, + .type = GEOZONE_SHAPE_CIRCULAR, + .minAltitude = 0, + .maxAltitude = 0, + .fenceAction = GEOFENCE_ACTION_NONE, + .vertexCount = 0 + ); + } +} + +PG_REGISTER_ARRAY_WITH_RESET_FN(vertexConfig_t, MAX_VERTICES_IN_CONFIG, geoZoneVertices, PG_GEOZONE_VERTICES, 0); + +void pgResetFn_geoZoneVertices(vertexConfig_t *instance) +{ + for (int i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + RESET_CONFIG(vertexConfig_t, &instance[i], + .zoneId = -1, + .idx = 0, + .lat = 0, + .lon = 0 + ); + } +} + +uint8_t geozoneGetUsedVerticesCount(void) +{ + uint8_t count = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + count += geoZonesConfig(i)->vertexCount; + } + return count; +} + +void geozoneResetVertices(const int8_t zoneId, const int16_t idx) +{ + if (zoneId < 0 && idx < 0) { + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + } + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx < 0) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount = 0; + } + } else if (zoneId >= 0 && zoneId < MAX_GEOZONES_IN_CONFIG && idx >= 0 && idx < MAX_VERTICES_IN_CONFIG) { + bool found = false; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == idx) { + geoZoneVerticesMutable(i)->lat = 0; + geoZoneVerticesMutable(i)->lon = 0; + geoZoneVerticesMutable(i)->idx = 0; + geoZoneVerticesMutable(i)->zoneId = -1; + found = true; + break; + } + } + if (found) { + geoZonesConfigMutable(zoneId)->vertexCount--; + } + } +} + +bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon) +{ + if (vertexId < MAX_VERTICES_IN_CONFIG) + { + int16_t vertexConfigIdx = -1; + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == -1) { + vertexConfigIdx = i; + break; + } + } + if (vertexConfigIdx >= 0) { + geoZoneVerticesMutable(vertexConfigIdx)->zoneId = zoneId; + geoZoneVerticesMutable(vertexConfigIdx)->idx = vertexId; + geoZoneVerticesMutable(vertexConfigIdx)->lat = lat; + geoZoneVerticesMutable(vertexConfigIdx)->lon = lon; + return true; + } + } + return false; +} + +int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) +{ + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + if (geoZoneVertices(i)->zoneId == zoneId && geoZoneVertices(i)->idx == vertexId) { + return i; + } + } + return -1; +} + +static bool areSticksDeflectdFromChannelValue(void) +{ + return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; +} + +static bool isNonGeozoneModeFromBoxInput(void) +{ + return !(IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)); +} + +static bool isPointOnBorder(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos) +{ + fpVector2_t *prev = &zone->verticesLocal[zone->config.vertexCount -1]; + fpVector2_t *current; + bool isOnBorder = false; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + current = &zone->verticesLocal[i]; + if (isPointOnLine2(prev, current, (fpVector2_t*)pos)) { + isOnBorder = true; + break; + } + prev = current; + } + + if (isOnBorder) { + return (pos->z >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos->z <= zone->config.maxAltitude; + } + + return isOnBorder; +} + +static bool isInZoneAltitudeRange(geoZoneRuntimeConfig_t *zone, const float pos) +{ + return (pos >= zone->config.minAltitude || zone->config.minAltitude == 0) && pos <= zone->config.maxAltitude; +} + +static bool isInGeozone(geoZoneRuntimeConfig_t *zone, const fpVector3_t *pos, bool ignoreAltitude) +{ + if (activeGeoZonesCount == 0) { + return false; + } + + bool isIn2D = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + isIn2D = isPointInPloygon((fpVector2_t*)pos, zone->verticesLocal, zone->config.vertexCount) || isPointOnBorder(zone, pos); + } else { // cylindric + isIn2D = isPointInCircle((fpVector2_t*)pos, &zone->verticesLocal[0], zone->radius); + } + + if (isIn2D && !ignoreAltitude) { + return isInZoneAltitudeRange(zone, pos->z); + } + + return isIn2D; +} + +static bool isPointInAnyOtherZone(const geoZoneRuntimeConfig_t *zone, uint8_t type, const fpVector3_t *pos) +{ + bool isInZone = false; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (zone != &activeGeoZones[i] && activeGeoZones[i].config.type == type && isInGeozone(&activeGeoZones[i], pos, false)) { + isInZone = true; + break; + } + } + return isInZone; +} + +static uint8_t getZonesForPos(geoZoneRuntimeConfig_t *zones[], const fpVector3_t *pos, const bool ignoreAltitude) +{ + uint8_t count = 0; + for (int i = 0; i < activeGeoZonesCount; i++) { + if (isInGeozone(&activeGeoZones[i], pos, ignoreAltitude)) { + zones[count++] = &activeGeoZones[i]; + } + } + return count; +} + +static uint8_t getCurrentZones(geoZoneRuntimeConfig_t *zones[], const bool ignoreAltitude) +{ + return getZonesForPos(zones, &navGetCurrentActualPositionAndVelocity()->pos, ignoreAltitude); +} + +static int geoZoneRTComp(const void *a, const void *b) +{ + geoZoneRuntimeConfig_t *zoneA = (geoZoneRuntimeConfig_t*)a; + geoZoneRuntimeConfig_t *zoneB = (geoZoneRuntimeConfig_t*)b; + + if (zoneA->enable == zoneB->enable) { + return 0; + } else if (zoneA->enable) { + return -1; + } else { + return 1; + } +} + +// in cm and cms/s +static uint32_t calcTime(const int32_t distance, const int32_t speed) +{ + if (speed <= 0) { + return 0; + } + + return distance / speed; +} + +static void calcPreviewPoint(fpVector3_t *target, const int32_t distance) +{ + calculateFarAwayTarget(target, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), distance); + target->z = getEstimatedActualPosition(Z) + calcTime(geoZoneConfig()->fenceDetectionDistance, gpsSol.groundSpeed) * getEstimatedActualVelocity(Z); +} + +static bool calcIntersectionForZone(fpVector3_t *intersection, float *distance, geoZoneRuntimeConfig_t *zone, const fpVector3_t *start, const fpVector3_t *end) +{ + bool hasIntersection = false; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + if (calcLine3dPolygonIntersection( + intersection, + distance, + start, + end, + zone->verticesLocal, + zone->config.vertexCount, + zone->config.minAltitude, + zone->config.maxAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + fpVector3_t circleCenter = { .x = zone->verticesLocal[0].x, .y = zone->verticesLocal[0].y, .z = zone->config.minAltitude }; + if (calcLineCylinderIntersection( + intersection, + distance, + start, + end, + &circleCenter, + zone->radius, + zone->config.maxAltitude - zone->config.minAltitude, + zone->config.type == GEOZONE_TYPE_INCLUSIVE)) { + hasIntersection = true; + } + } + + if (hasIntersection && isPointOnLine3(start, end, intersection)){ + return true; + } + *distance = -1; + return false; +} + +static int32_t calcBouceCoursePolygon(const int32_t course, const fpVector2_t* pos, const fpVector2_t *intersect, const fpVector2_t* p1, const fpVector2_t* p2) +{ + int32_t newCourse = 0; + int32_t angelp1 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p1, pos, intersect)); + int32_t angelp2 = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(p2, pos, intersect)); + if (angelp1 < angelp2) { + if (isPointRightFromLine(pos, intersect, p1)) { + newCourse = course - 2 * angelp1; + } + else { + newCourse = course + 2 * angelp1; + } + } + else { + if (isPointRightFromLine(pos, intersect, p2)) { + newCourse = course - 2 * angelp2; + } + else { + newCourse = course + 2 * angelp2; + } + } + return wrap_36000(newCourse); +} + +static int32_t calcBouceCourseCircle(const int32_t course, const fpVector2_t* pos, const fpVector2_t* intersect, const fpVector2_t* mid) +{ + int32_t newCourse = 0; + int32_t angel = DEGREES_TO_CENTIDEGREES(calcAngelFrom3Points(mid, pos, intersect)); + if (isPointRightFromLine(pos, mid, intersect)) { + newCourse = course + 2 * (angel - 9000); + } + else { + newCourse = course - 2 * (angel - 9000); + } + return wrap_36000(newCourse); +} + +static bool findNearestIntersectionZone(geoZoneRuntimeConfig_t **intersectZone, fpVector3_t *intersection, float *distance, const float detectionDistance, const fpVector3_t *start, const fpVector3_t *end) +{ + geoZoneRuntimeConfig_t *zone = NULL; + fpVector3_t intersect; + float distanceToZone = FLT_MAX; + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + float currentDistance = FLT_MAX; + if (!calcIntersectionForZone( + ¤tIntersect, + ¤tDistance, + &activeGeoZones[i], + start, + end)) { + continue; + } + + if (currentDistance < distanceToZone) { + distanceToZone = currentDistance; + zone = &activeGeoZones[i]; + intersect = currentIntersect; + } + } + + if (zone && distanceToZone < detectionDistance) { + *intersectZone = zone; + *distance = distanceToZone; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + + return false; +} + +static bool isPointDirectReachable(const fpVector3_t* start, const fpVector3_t *point) +{ + float currentDistance = 0; + bool pointIsInOverlappingZone = false, pointIsInExclusiveZone = false, hasIntersection = false; + + /* + if (start->x == point->x && start->y == point->y) { + return false; + } + */ + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + fpVector3_t currentIntersect; + + if (!calcIntersectionForZone(¤tIntersect, ¤tDistance, &activeGeoZones[i], start, point)) { + continue; + } + hasIntersection = true; + + // Intersct a exclusive Zone? + geoZoneRuntimeConfig_t *intersectZones[MAX_GEOZONES]; + uint8_t intersectZonesCount = getZonesForPos(intersectZones, ¤tIntersect, false); + for (uint8_t j = 0; j < intersectZonesCount; j++) { + if (intersectZones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE ) { + pointIsInExclusiveZone = true; + break; + } + } + + if (pointIsInExclusiveZone) { + break; + } + + // We targeting a exit point (in min two zones) + if (intersectZonesCount < 2) { + break; + } + + geoZoneRuntimeConfig_t *startZones[MAX_GEOZONES]; + uint8_t startZonesCount = getZonesForPos(startZones, start, false); + geoZoneRuntimeConfig_t *endZones[MAX_GEOZONES]; + uint8_t endZonesCount = getZonesForPos(endZones, point, false); + + for (uint8_t j = 0; j < intersectZonesCount; j++) { + for (uint8_t k = 0; k < startZonesCount; k++) { + for (uint8_t l = 0; l < endZonesCount; l++) { + if (intersectZones[j] == startZones[k] && intersectZones[j] == endZones[l]) { + pointIsInOverlappingZone = true; + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + if (pointIsInOverlappingZone) { + break; + } + } + } + + return !pointIsInExclusiveZone && (!hasIntersection || pointIsInOverlappingZone); +} + +uint32_t geozoneGetDetectionDistance(void) +{ + uint32_t detctionDistance = 0; + if (STATE(AIRPLANE)) { + detctionDistance = navConfig()->fw.loiter_radius * 1.5; + } else { + detctionDistance = geoZoneConfig()->copterFenceStopDistance; + } + return detctionDistance; +} + +static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) +{ + int32_t course; + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t intersect; + bool found = false; + fpVector2_t *p1 = &zone->verticesLocal[zone->config.vertexCount - 1]; + fpVector2_t *p2; + for (uint8_t i = 0; i < zone->config.vertexCount; i++) { + p2 = &zone->verticesLocal[i]; + if (calcLineLineIntersection(&intersect, p1, p2, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)prevPoint, true)) { + found = true; + break; + } + p1 = p2; + } + + if (!found) { + return -1; + } + course = calcBouceCoursePolygon(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &intersect, p1, p2); + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + course = calcBouceCourseCircle(DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)intersection, &zone->verticesLocal[0]); + } + return course; +} + +static bool initPathPoint(pathPoint_t *pathPoints, const fpVector3_t pos, uint8_t *idx) +{ + if (*idx + 1 >= MAX_PATH_PONITS) { + return false; + } + + pathPoints[*idx].distance = FLT_MAX; + pathPoints[*idx].visited = false; + pathPoints[(*idx)++].point = pos; + + return true; +} + +static bool isPosInGreenAlt(geoZoneRuntimeConfig_t *zones[], const uint8_t zoneCount, const float alt) +{ + bool isInNfz = false, isInFz = false; + for (uint8_t j = 0; j < zoneCount; j++) { + if(isInZoneAltitudeRange(zones[j], alt)){ + isInFz = zones[j]->config.type == GEOZONE_TYPE_INCLUSIVE; + isInNfz = zones[j]->config.type == GEOZONE_TYPE_EXCLUSIVE; + } + } + return !isInNfz && (!isAtLeastOneInclusiveZoneActive || isInFz); +} + +static bool checkPathPointOrSetAlt(fpVector3_t *pos) +{ + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, pos, true); + + if (zoneCount == 0) { + return !isAtLeastOneInclusiveZoneActive; + } + + if (zoneCount == 1 && zones[0]->config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + + bool isInExclusice = false; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(zones[i], pos, false)) { + isInExclusice = true; + if (zones[i]->config.minAltitude != 0) { + float min = zones[i]->config.minAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + if (isPosInGreenAlt(zones, zoneCount, min)) { + pos->z = min; + return true; + } + } + + if (!zones[i]->isInfZone || zones[i]->config.maxAltitude < INT32_MAX) { + float max = zones[i]->config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + if(isPosInGreenAlt(zones, zoneCount, max)) { + pos->z = max; + return true; + } + } + } + } + + return !isInExclusice; +} + +// Return value: 0 - Target direct reachable; -1 No way; >= 1 Waypoints to target +#define CIRCLE_POLY_SIDES 6 +static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fpVector3_t* target) +{ + pathPoint_t pathPoints[MAX_PATH_PONITS]; + uint8_t pathPointCount = 0, waypointCount = 0; + fpVector3_t start = *point; + + if (isPointDirectReachable(&start, target)) { + return 0; + } + + float offset = geozoneGetDetectionDistance(); + if (geozone.distanceVertToNearestZone <= offset) { + int bearing = wrap_36000(geozone.directionToNearestZone + 18000); + start.x += offset * cos_approx(CENTIDEGREES_TO_RADIANS(bearing)); + start.y += offset * sin_approx(CENTIDEGREES_TO_RADIANS(bearing)); + } + + pathPoints[pathPointCount].visited = true; + pathPoints[pathPointCount].distance = 0; + pathPoints[pathPointCount++].point = start; + + for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { + fpVector2_t *verticesZone; + fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; + uint8_t verticesZoneCount; + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + verticesZone = activeGeoZones[i].verticesLocal; + verticesZoneCount = activeGeoZones[i].config.vertexCount; + } else { + generatePolygonFromCircle(verticesCirclePoly, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius, CIRCLE_POLY_SIDES); + verticesZone = verticesCirclePoly; + verticesZoneCount = CIRCLE_POLY_SIDES; + } + + fpVector2_t safeZone[MAX_VERTICES]; + offset = geozoneGetDetectionDistance() * 2 / 3; + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + offset *= -1; + } + + float zMin = start.z, zMax = 0; + if (!isInZoneAltitudeRange(&activeGeoZones[i], start.z) && activeGeoZones[i].config.minAltitude > 0) { + zMin = activeGeoZones[i].config.minAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude - 2 * geoZoneConfig()->safeAltitudeDistance; + } else if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && (!activeGeoZones[i].isInfZone || activeGeoZones[i].config.maxAltitude < INT32_MAX)) { + zMax = activeGeoZones[i].config.maxAltitude + 2 * geoZoneConfig()->safeAltitudeDistance; + } + + generateOffsetPolygon(safeZone, verticesZone, verticesZoneCount, offset); + fpVector2_t *prev = &safeZone[verticesZoneCount - 1]; + fpVector2_t *current; + for (uint8_t j = 0; j < verticesZoneCount; j++) { + current = &safeZone[j]; + + if (zMax > 0 ) { + fpVector3_t max = { .x = current->x, .y = current->y, .z = zMax }; + if (checkPathPointOrSetAlt(&max)) { + if (!initPathPoint(pathPoints, max, &pathPointCount)) { + return -1; + } + } + + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + // Set some "fly over points" + float dist = calculateDistance2(prev, current); + if (dist > MAX_DISTANCE_FLY_OVER_POINTS) { + uint8_t sectionCount = (uint8_t)(dist / MAX_DISTANCE_FLY_OVER_POINTS); + float dist = MAX_DISTANCE_FLY_OVER_POINTS; + for (uint8_t k = 0; k < sectionCount; k++) { + fpVector3_t flyOverPoint; + calcPointOnLine((fpVector2_t*)&flyOverPoint, prev, current, dist); + fpVector3_t maxFo = { .x = flyOverPoint.x, .y = flyOverPoint.y, .z = zMax }; + if (checkPathPointOrSetAlt(&maxFo)) { + if (!initPathPoint(pathPoints, maxFo, &pathPointCount)) { + return -1; + } + } + dist += MAX_DISTANCE_FLY_OVER_POINTS; + } + } + } + } + + if (zMin > 0) { + fpVector3_t min = { .x = current->x, .y = current->y, .z = zMin }; + if (checkPathPointOrSetAlt(&min)) { + if (!initPathPoint(pathPoints, min, &pathPointCount)) { + return -1; + } + } + + } + prev = current; + } + } + + if (!initPathPoint(pathPoints, *target, &pathPointCount)) { + return -1; + } + + // Dijkstra + pathPoint_t *current = pathPoints; + while (!pathPoints[pathPointCount - 1].visited) { + pathPoint_t *next = current; + float min = FLT_MAX; + for (uint16_t i = 1; i < pathPointCount; i++) { + + float currentDist = FLT_MAX; + if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { + float dist2D = calculateDistance2((fpVector2_t*)¤t->point, (fpVector2_t*)&pathPoints[i].point); + float distAlt = ABS(current->point.z - pathPoints[i].point.z); + currentDist = current->distance + dist2D + 2 * distAlt; + } + + if (currentDist < pathPoints[i].distance && !pathPoints[i].visited) { + pathPoints[i].distance = currentDist; + pathPoints[i].prev = current; + } + if (min > pathPoints[i].distance && !pathPoints[i].visited) { + min = pathPoints[i].distance; + next = &pathPoints[i]; + } + } + + if (min == FLT_MAX) { + return -1; + } + + current = next; + current->visited = true; + } + + waypointCount = 0; + current = &pathPoints[pathPointCount - 1]; + while (current != pathPoints) { + waypointCount++; + current = current->prev; + } + // Don't set home to the WP list + current = pathPoints[pathPointCount - 1].prev; + uint8_t i = waypointCount - 2; + while (current != pathPoints) { + waypoints[i] = current->point; + current = current->prev; + i--; + } + return waypointCount - 1; +} + +static void updateCurrentZones(void) +{ + currentZoneCount = getCurrentZones(currentZones, false); + geozone.insideNfz = false; + geozone.insideFz = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE) { + geozone.insideNfz = true; + } + if (currentZones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + geozone.insideFz = true; + } + } +} + +static void updateZoneInfos(void) +{ + float nearestDistanceToBorder = FLT_MAX; + fpVector3_t nearestBorderPoint; + aboveOrUnderZone = false; + + geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; + uint8_t currentZoneCount = getCurrentZones(currentZones, true); + int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; + + if (currentZoneCount == 1) { + currentMaxAltitude = currentZones[0]->config.maxAltitude; + currentMinAltitude = currentZones[0]->config.minAltitude; + + if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { + nearestHorInclZone = currentZones[0]; + } + + } else if (currentZoneCount >= 2) { + + geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; + float distAbove = FLT_MAX, distBelow = FLT_MAX; + geoZoneRuntimeConfig_t *current = NULL; + for (uint8_t i = 0; i < currentZoneCount; i++) { + current = currentZones[i]; + + if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { + currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); + currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + } + + if (current->config.minAltitude > getEstimatedActualPosition(Z)) { + float dist = current->config.maxAltitude - getEstimatedActualPosition(Z); + if (dist < distAbove) { + aboveZone = current; + distAbove = dist; + } + } + + if (current->config.maxAltitude < getEstimatedActualPosition(Z)) { + float dist = getEstimatedActualPosition(Z) - current->config.maxAltitude; + if (dist < distBelow) { + belowZone = current; + distBelow = dist; + } + } + } + + if (aboveZone) { + if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); + nearestHorInclZone = aboveZone; + } else { + currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); + } + } + + if (belowZone) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); + nearestHorInclZone = belowZone; + } else { + currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); + } + } + } + + if (currentMinAltitude == INT32_MAX) { + currentMinAltitude = 0; + } + + if (currentMaxAltitude == INT32_MIN) { + currentMaxAltitude = 0; + } + + if (currentMaxAltitude == INT32_MAX && currentMinAltitude != 0) { + geozone.distanceVertToNearestZone = ABS(currentMinAltitude - getEstimatedActualPosition(Z)); + } else if (currentMinAltitude == 0 && currentMaxAltitude != 0) { + geozone.distanceVertToNearestZone = currentMaxAltitude - getEstimatedActualPosition(Z); + } else if (currentMinAltitude != 0 && currentMaxAltitude > 0) { + int32_t distToMin = currentMinAltitude - getEstimatedActualPosition(Z); + int32_t distToMax = currentMaxAltitude - getEstimatedActualPosition(Z); + if (getEstimatedActualPosition(Z) > currentMinAltitude && getEstimatedActualPosition(Z) < currentMaxAltitude) { + if (ABS(distToMin) < ABS(currentMaxAltitude - currentMinAltitude) / 2 ) { + geozone.distanceVertToNearestZone = distToMin; + } else { + geozone.distanceVertToNearestZone = distToMax; + } + } else { + geozone.distanceVertToNearestZone = MIN(ABS(distToMin), distToMax); + } + } else { + geozone.distanceVertToNearestZone = 0; + } + + if (currentZoneCount == 0) { + geozone.currentzoneMaxAltitude = 0; + geozone.currentzoneMinAltitude = 0; + } else { + + if (getEstimatedActualPosition(Z) < currentMinAltitude || getEstimatedActualPosition(Z) > currentMaxAltitude) { + aboveOrUnderZone = true; + } + + if (currentMaxAltitude > 0) { + geozone.currentzoneMaxAltitude = currentMaxAltitude - geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMaxAltitude = 0; + } + + if (currentMinAltitude > 0) { + geozone.currentzoneMinAltitude = currentMinAltitude + geoZoneConfig()->safeAltitudeDistance; + } else { + geozone.currentzoneMinAltitude = 0; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) + { + // Ignore NFZ for now, we want back fo the FZ, we will check for NFZ later at RTH + if (currentZoneCount == 0 && isAtLeastOneInclusiveZoneActive && activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE) { + continue; + } + + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector2_t* prev = &activeGeoZones[i].verticesLocal[activeGeoZones[i].config.vertexCount - 1]; + fpVector2_t* current = NULL; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + current = &activeGeoZones[i].verticesLocal[j]; + fpVector2_t pos = calcNearestPointOnLine(prev, current, (fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos); + float dist = calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &pos); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + nearestBorderPoint.x = pos.x; + nearestBorderPoint.y = pos.y; + nearestBorderPoint.z = getEstimatedActualPosition(Z); + geozone.directionToNearestZone = calculateBearingToDestination(&nearestBorderPoint); + geozone.distanceHorToNearestZone = roundf(nearestDistanceToBorder); + nearestInclusiveZone = &activeGeoZones[i]; + } + prev = current; + } + } else if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + float dist = fabsf(calculateDistance2((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0]) - activeGeoZones[i].radius); + if (dist < nearestDistanceToBorder) { + nearestDistanceToBorder = dist; + int32_t directionToBorder = calculateBearingToDestination((fpVector3_t*)&activeGeoZones[i].verticesLocal[0]); + + if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { + directionToBorder = wrap_36000(directionToBorder + 18000); + } + + geozone.directionToNearestZone = directionToBorder; + geozone.distanceHorToNearestZone = roundf(dist); + nearestInclusiveZone = &activeGeoZones[i]; + } + } + + if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorInclZone; + geozone.distanceHorToNearestZone = 0; + } + } +} + +void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) +{ + // Actions only for assisted modes now + if (isNonGeozoneModeFromBoxInput() || geozone.avoidInRTHInProgress || (calculateDistanceToDestination(intersection) > geozoneGetDetectionDistance())) { + return; + } + + int32_t alt = roundf(intersection->z); + if (alt == zone->config.maxAltitude || alt == zone->config.minAltitude) { + return; + } + + fpVector3_t prevPoint; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + + if (zone->config.fenceAction == GEOFENCE_ACTION_AVOID) { + bool avoidFzStep = false; + float fzStepAlt = 0; + if (zone->config.type == GEOZONE_TYPE_INCLUSIVE) { + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES_IN_CONFIG]; + uint8_t zoneCount = getZonesForPos(zones, intersection, true); + + float maxAlt = FLT_MAX; + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE && zones[i]->config.minAltitude > intersection->z) { + float alt = zones[i]->config.minAltitude; + if (alt < maxAlt) { + maxAlt = alt; + } + } + } + + if (maxAlt < FLT_MAX) { + fzStepAlt = maxAlt + geoZoneConfig()->safeAltitudeDistance * 2; + avoidFzStep = true; + } + } + // We can move upwards + if ((zone->config.type == GEOZONE_TYPE_EXCLUSIVE && geozone.zoneInfo > 0 && (geozone.zoneInfo < geoZoneConfig()->avoidAltitudeRange)) || avoidFzStep) { + + calculateFarAwayTarget(&posControl.sendTo.targetPos, posControl.actualState.cog, geoZoneConfig()->fenceDetectionDistance * 2); + if (avoidFzStep) { + posControl.sendTo.targetPos.z = fzStepAlt; + } else { + posControl.sendTo.targetPos.z = zone->config.maxAltitude + geoZoneConfig()->safeAltitudeDistance * 2; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING_UPWARD; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } else { + // Calc new course + avoidCourse = calcBounceCourseForZone(zone, &prevPoint, intersection); + + if (avoidCourse == -1) { + return; + } + + calculateFarAwayTarget(&posControl.sendTo.targetPos, avoidCourse, geoZoneConfig()->fenceDetectionDistance * 2); // Its too far, mode will be abort if we are on the right course + + // Check for min/max altitude + if (geozone.currentzoneMaxAltitude > 0 && getEstimatedActualPosition(Z) > geozone.currentzoneMaxAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 0.25; + } else if (geozone.currentzoneMinAltitude != 0 && getEstimatedActualPosition(Z) < geozone.currentzoneMinAltitude) { + posControl.sendTo.targetPos.z = geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 0.25; + } + + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = AVOID_TIMEOUT; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.altitudeTargetRange = geoZoneConfig()->safeAltitudeDistance / 2; + avoidingPoint = *intersection; + actionState = GEOZONE_ACTION_STATE_AVOIDING; + actionStartTime = millis(); + avoidingPoint = *intersection; + activateSendTo(); + } + } else if (zone->config.fenceAction == GEOFENCE_ACTION_POS_HOLD) { + actionState = GEOZONE_ACTION_STATE_POSHOLD; + + if (STATE(AIRPLANE)) { + if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { + fpVector3_t refPoint; + int32_t course = calcBounceCourseForZone(zone, &prevPoint, intersection); + calculateFarAwayTarget(&refPoint, course, geoZoneConfig()->fenceDetectionDistance * 2); + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, (fpVector2_t*)&refPoint)) { + geozone.loiterDir = 1; // Right + } else { + geozone.loiterDir = -1; // Left + } + } else if (zone->config.shape == GEOZONE_SHAPE_CIRCULAR) { + if (isPointRightFromLine((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, (fpVector2_t*)&prevPoint, &zone->verticesLocal[0])) { + geozone.loiterDir = -1; // Left + } else { + geozone.loiterDir = 1; // Right + } + } + } + + geozone.sticksLocked = true; + activateForcedPosHold(); + actionStartTime = millis(); + } else if (zone->config.fenceAction == GEOFENCE_ACTION_RTH) { + actionState = GEOZONE_ACTION_STATE_RTH; + geozone.sticksLocked = true; + activateForcedRTH(); + actionStartTime = millis(); + } +} + +static void endFenceAction(void) +{ + posControl.sendTo.lockSticks = false; + geozone.sticksLocked = false; + geozone.sticksLocked = 0; + + switch (actionState) { + case GEOZONE_ACTION_STATE_AVOIDING: + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + abortSendTo(); + break; + case GEOZONE_ACTION_STATE_POSHOLD: + abortForcedPosHold(); + break; + case GEOZONE_ACTION_STATE_RTH: + abortForcedRTH(); + break; + default: + break; + } + + actionState = GEOZONE_ACTION_STATE_NONE; + + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)){ + setDesiredPosition(&posControl.sendTo.targetPos, 0, NAV_POS_UPDATE_Z); + } + + abortSendTo(); +} + +static void geoZoneInit(void) +{ + activeGeoZonesCount = 0; + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) + { + if (geoZonesConfig(i)->vertexCount > 0) { + memcpy(&activeGeoZones[activeGeoZonesCount].config, geoZonesConfig(i), sizeof(geoZoneRuntimeConfig_t)); + if (activeGeoZones[i].config.maxAltitude == 0) { + activeGeoZones[i].config.maxAltitude = INT32_MAX; + } + + activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0; + + if (!STATE(AIRPLANE) && activeGeoZones[i].config.fenceAction == GEOFENCE_ACTION_AVOID) { + activeGeoZones[i].config.fenceAction = GEOFENCE_ACTION_POS_HOLD; + } + + activeGeoZones[activeGeoZonesCount].enable = true; + activeGeoZonesCount++; + } + } + + if (activeGeoZonesCount > 0) { + // Covert geozone vertices to local + for (uint8_t i = 0; i < MAX_VERTICES_IN_CONFIG; i++) { + gpsLocation_t vertexLoc; + fpVector3_t posLocal3; + + if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) { + if (geoZonesConfig(geoZoneVertices(i)->zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) { + activeGeoZones[geoZoneVertices(i)->zoneId].radius = geoZoneVertices(i)->lat; + activeGeoZones[geoZoneVertices(i)->zoneId].config.vertexCount = 1; + continue; + } + + vertexLoc.lat = geoZoneVertices(i)->lat; + vertexLoc.lon = geoZoneVertices(i)->lon; + geoConvertGeodeticToLocal(&posLocal3, &posControl.gpsOrigin, &vertexLoc, GEO_ALT_ABSOLUTE); + + uint8_t vertexIdx = 0; + for (uint8_t j = 0; j < geoZoneVertices(i)->zoneId; j++) { + vertexIdx += activeGeoZones[j].config.vertexCount; + } + vertexIdx += geoZoneVertices(i)->idx; + + verticesLocal[vertexIdx].x = posLocal3.x; + verticesLocal[vertexIdx].y = posLocal3.y; + + if (geoZoneVertices(i)->idx == 0) { + activeGeoZones[geoZoneVertices(i)->zoneId].verticesLocal = &verticesLocal[vertexIdx]; + } + } + } + } + + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) + { + safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; + safeHomeGeozoneConfig.type = GEOZONE_TYPE_INCLUSIVE; + safeHomeGeozoneConfig.fenceAction = geoZoneConfig()->safeHomeFenceAction; + safeHomeGeozoneConfig.maxAltitude = 0; + safeHomeGeozoneConfig.minAltitude = 0; + safeHomeGeozoneConfig.vertexCount = 1; + + activeGeoZones[activeGeoZonesCount].config = safeHomeGeozoneConfig; + activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; + activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; + activeGeoZonesCount++; + } + + updateCurrentZones(); + uint8_t newActiveZoneCount = activeGeoZonesCount; + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (!geozone.insideFz) { + // Deactivate all inclusive geozones with distance > GEOZONE_INCLUSE_IGNORE_DISTANCE + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && !isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + fpVector2_t pos2 = { .x = getEstimatedActualPosition(X), .y = getEstimatedActualPosition(Y) }; + + uint32_t minDistanceToZone = INT32_MAX; + for (uint8_t j = 0; j < activeGeoZones[i].config.vertexCount; j++) { + float dist = calculateDistance2(&pos2, &activeGeoZones[i].verticesLocal[j]); + if (activeGeoZones[i].config.shape == GEOZONE_SHAPE_CIRCULAR) { + minDistanceToZone = dist - activeGeoZones[i].radius; + break; + } + + if (dist < minDistanceToZone) { + minDistanceToZone = dist; + } + } + if (minDistanceToZone > GEOZONE_INCLUSE_IGNORE_DISTANCE) { + activeGeoZones[i].enable = false; + newActiveZoneCount--; + } + } + } + } + + activeGeoZonesCount = newActiveZoneCount; + if (activeGeoZonesCount == 0) { + setTaskEnabled(TASK_GEOZONE, false); + geozoneIsEnabled = false; + return; + } + geozoneIsEnabled = true; + + qsort(activeGeoZones, MAX_GEOZONES, sizeof(geoZoneRuntimeConfig_t), geoZoneRTComp); + + for (int i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + isAtLeastOneInclusiveZoneActive = true; + break; + } + } +} + +// Called by Scheduler +void geozoneUpdate(timeUs_t curentTimeUs) +{ + UNUSED(curentTimeUs); + + geozone.messageState = GEOZONE_MESSAGE_STATE_NONE; + if (!isInitalised && navigationPositionEstimateIsHealthy()) { + geoZoneInit(); + isInitalised = true; + } + + if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + noZoneRTH = false; + return; + } + + // Zone RTH Override: Toggle RTH switch short + if (geozone.avoidInRTHInProgress) { + if (rthHomeSwitchLastState != IS_RC_MODE_ACTIVE(BOXNAVRTH)) { + if (millis() - rthOverrideStartTime < RTH_OVERRIDE_TIMEOUT) { + geozoneResetRTH(); + noZoneRTH = true; + } + rthOverrideStartTime = millis(); + } + rthHomeSwitchLastState = IS_RC_MODE_ACTIVE(BOXNAVRTH); + } + + updateCurrentZones(); + updateZoneInfos(); + + // User has switched to non geofence mode, end all actions and switch to mode from box input + if (isNonGeozoneModeFromBoxInput()) { + if (actionState != GEOZONE_ACTION_STATE_NONE) { + endFenceAction(); + } + lockRTZ = false; + return; + } + + switch (actionState) + { + case GEOZONE_ACTION_STATE_AVOIDING: + if (calculateDistanceToDestination(&avoidingPoint) > geozoneGetDetectionDistance()) { + posControl.sendTo.lockSticks = false; + } + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(avoidCourse, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse), 500) || millis() - actionStartTime > AVOID_TIMEOUT || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RETURN_TO_FZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE; + if ((geozone.insideFz && ABS(geozone.distanceVertToNearestZone ) > geoZoneConfig()->safeAltitudeDistance) || !posControl.flags.sendToActive) { + lockRTZ = true; + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_FLYOUT_NFZ: + geozone.messageState = GEOZONE_MESSAGE_STATE_FLYOUT_NFZ; + if (!geozone.insideNfz || !posControl.flags.sendToActive) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_AVOIDING_UPWARD: + geozone.messageState = GEOZONE_MESSAGE_STATE_AVOIDING_FB; + if (IS_IN_TOLERANCE_RANGE(getEstimatedActualPosition(Z), posControl.sendTo.targetPos.z, posControl.sendTo.altitudeTargetRange) || !posControl.flags.sendToActive || millis() - actionStartTime > AVOID_TIMEOUT) { + endFenceAction(); + } + return; + case GEOZONE_ACTION_STATE_RTH: + case GEOZONE_ACTION_STATE_POSHOLD: + geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { + geozone.sticksLocked = false; + } + if (!geozone.sticksLocked && areSticksDeflectdFromChannelValue()) { + endFenceAction(); + } + return; + default: + break; + } + + bool currentZoneHasAction = false; + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + currentZoneHasAction = true; + break; + } + } + + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + + float targetAltitide = 0; + if (!geozone.insideFz) { + if (geozone.distanceVertToNearestZone < 0) { + targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + } else { + targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + } else { + targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + } + + fpVector3_t targetPos; + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitide; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + actionState = GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE; + actionStartTime = millis(); + activateSendTo(); + return; + } + + // RTH active: Further checks are done in RTH logic + if ((navGetCurrentStateFlags() & NAV_AUTO_RTH) || IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated || FLIGHT_MODE(NAV_FW_AUTOLAND)) { + return; + } else if (geozone.avoidInRTHInProgress) { + geozoneResetRTH(); + } + + if (lockRTZ && (geozone.insideFz || geozone.insideNfz)) { + lockRTZ = false; + } + + if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { + + if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_OUTSIDE_FZ; + } + + if (geozone.insideNfz) { + geozone.messageState = GEOZONE_MESSAGE_STATE_NFZ; + } + + if (!isNonGeozoneModeFromBoxInput()) { + bool flyOutNfz = false; + geoZoneRuntimeConfig_t *targetZone = nearestInclusiveZone; + + for (uint8_t i = 0; i < currentZoneCount; i++) { + if (currentZones[i]->config.type == GEOZONE_TYPE_EXCLUSIVE && currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { + flyOutNfz = true; + targetZone = currentZones[i]; + break; + } + } + + if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { + int32_t targetAltitude = 0; + if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z); + } + + fpVector3_t targetPos; + if (aboveOrUnderZone) { + if (ABS(geozone.distanceVertToNearestZone) < 2000) { + calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); + if(geozone.distanceVertToNearestZone > 0) { + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + } else { + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + } + + } else { + targetPos = navGetCurrentActualPositionAndVelocity()->pos; + } + } else { + calculateFarAwayTarget(&targetPos, geozone.directionToNearestZone, geozone.distanceHorToNearestZone + geoZoneConfig()->fenceDetectionDistance / 2); + } + + posControl.sendTo.targetPos.x = targetPos.x; + posControl.sendTo.targetPos.y = targetPos.y; + posControl.sendTo.targetPos.z = targetAltitude; + posControl.sendTo.altitudeTargetRange = 200; + posControl.sendTo.targetRange = POS_DETECTION_DISTANCE; + posControl.sendTo.lockSticks = true; + posControl.sendTo.lockStickTime = STICK_LOCK_MIN_TIME; + + if (flyOutNfz) { + actionState = GEOZONE_ACTION_STATE_FLYOUT_NFZ; + } else { + actionState = GEOZONE_ACTION_STATE_RETURN_TO_FZ; + } + + activateSendTo(); + } + } + } + + + geoZoneRuntimeConfig_t *intersectZone = NULL; + fpVector3_t intersection, prevPoint; + float distanceToZone = 0; + calcPreviewPoint(&prevPoint, geoZoneConfig()->fenceDetectionDistance); + if (findNearestIntersectionZone(&intersectZone, &intersection, &distanceToZone, geoZoneConfig()->fenceDetectionDistance, &navGetCurrentActualPositionAndVelocity()->pos, &prevPoint)) { + if (geozone.insideFz) { + if (!isPointInAnyOtherZone(intersectZone, GEOZONE_TYPE_INCLUSIVE, &intersection)) { + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + geozone.messageState = GEOZONE_MESSAGE_STATE_LEAVING_FZ; + performeFenceAction(intersectZone, &intersection); + } + } + + if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { + + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); + + int32_t minAltitude = intersectZone->config.minAltitude; + int32_t maxAltitude = intersectZone->config.maxAltitude; + if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { + geozone.zoneInfo = INT32_MAX; + } else if (maxAltitude == INT32_MAX) { + geozone.zoneInfo = minAltitude - getEstimatedActualPosition(Z); + } else if (minAltitude == 0) { + geozone.zoneInfo = maxAltitude - getEstimatedActualPosition(Z); + } else { + int32_t distToMax = maxAltitude - getEstimatedActualPosition(Z); + int32_t distToMin = minAltitude - getEstimatedActualPosition(Z); + if (ABS(distToMin) < ABS(distToMax)) { + geozone.zoneInfo = distToMin; + } else { + geozone.zoneInfo = (distToMax); + } + } + + geozone.messageState = GEOZONE_MESSAGE_STATE_ENTERING_NFZ; + performeFenceAction(intersectZone, &intersection); + } + } +} + +fpVector3_t *geozoneGetCurrentRthAvoidWaypoint(void) +{ + return &rthWaypoints[rthWaypointIndex]; +} + +void geozoneAdvanceRthAvoidWaypoint(void) +{ + if (rthWaypointIndex < rthWaypointCount) { + rthWaypointIndex++; + } +} + +bool geoZoneIsLastRthWaypoint(void) +{ + return rthWaypointIndex == rthWaypointCount - 1; +} + +void geozoneResetRTH(void) +{ + geozone.avoidInRTHInProgress = false; + rthWaypointIndex = 0; + rthWaypointCount = 0; +} + +void geozoneSetupRTH() { + if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { + noZoneRTH = true; + } else { + noZoneRTH = false; + } +} + +int8_t geozoneCheckForNFZAtCourse(bool isRTH) +{ + UNUSED(isRTH); + + if (geozone.avoidInRTHInProgress || noZoneRTH || !geozoneIsEnabled || !isInitalised) { + return 0; + } + + updateCurrentZones(); + + // Never mind, lets fly out of the zone on current course + if (geozone.insideNfz || (isAtLeastOneInclusiveZoneActive && !geozone.insideFz)) { + return 0; + } + + int8_t waypointCount = calcRthCourse(rthWaypoints, &navGetCurrentActualPositionAndVelocity()->pos, &posControl.rthState.homePosition.pos); + if (waypointCount > 0) { + rthWaypointCount = waypointCount; + rthWaypointIndex = 0; + geozone.avoidInRTHInProgress = true; + return 1; + } + + return waypointCount; +} + +bool isGeozoneActive(void) +{ + return activeGeoZonesCount > 0; +} + +void geozoneUpdateMaxHomeAltitude(void) { + int32_t altitude = INT32_MIN; + geoZoneRuntimeConfig_t *zones[MAX_GEOZONES]; + uint8_t zoneCount = getZonesForPos(zones, &posControl.rthState.homePosition.pos, false); + for (uint8_t i = 0; i < zoneCount; i++) { + if (zones[i]->config.type == GEOZONE_TYPE_INCLUSIVE) { + altitude = MAX(zones[i]->config.maxAltitude, altitude); + } + } + + if (altitude > INT32_MIN) { + geozone.maxHomeAltitude = altitude - geoZoneConfig()->safeAltitudeDistance; + geozone.homeHasMaxAltitue = true; + } else { + geozone.homeHasMaxAltitue = false; + } +} + +// Avoid arming in NFZ +bool geozoneIsInsideNFZ(void) +{ + // Do not generate arming flags unless we are sure about them + if (!isInitalised || activeGeoZonesCount == 0) { + return false; + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_EXCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return true; + } + } + + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE && isInGeozone(&activeGeoZones[i], &navGetCurrentActualPositionAndVelocity()->pos, false)) { + return false; + } + } + + // We aren't in any zone, is an inclusive one still active? + for (uint8_t i = 0; i < activeGeoZonesCount; i++) { + if (activeGeoZones[i].config.type == GEOZONE_TYPE_INCLUSIVE) { + return true; + } + } + + return false; +} + +#endif diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h new file mode 100755 index 00000000000..38849719af0 --- /dev/null +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -0,0 +1,480 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#include +#include +#include + +#include "common/vector.h" +#include "navigation/navigation_private.h" + +#define K_EPSILON 1e-8 + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = line1End->x - line1Start->x; + double t1 = -(line2End->x - line2Start->x); + double r1 = line2Start->x - line1Start->x; + + double s2 = line1End->y - line1Start->y; + double t2 = -(line2End->y - line2Start->y); + double r2 = line2Start->y - line1Start->y; + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return ABS(a + b - c) <= 1; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } + else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6d7b386cbde..fe277a00617 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -113,6 +113,11 @@ typedef struct navigationFlags_s { bool rthTrackbackActive; // Activation status of RTH trackback bool wpTurnSmoothingActive; // Activation status WP turn smoothing bool manualEmergLandActive; // Activation status of manual emergency landing + +#ifdef USE_GEOZONE + bool sendToActive; + bool forcedPosholdActive; +#endif } navigationFlags_t; typedef struct { @@ -160,6 +165,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING, + NAV_FSM_EVENT_SWITCH_TO_SEND_TO, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -245,6 +251,10 @@ typedef enum { NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46, NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47, NAV_PERSISTENT_ID_FW_LANDING_FINISHED = 48, + + NAV_PERSISTENT_ID_SEND_TO_INITALIZE = 49, + NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES = 50, + NAV_PERSISTENT_ID_SEND_TO_FINISHED = 51 } navigationPersistentId_e; typedef enum { @@ -304,6 +314,10 @@ typedef enum { NAV_STATE_MIXERAT_IN_PROGRESS, NAV_STATE_MIXERAT_ABORT, + NAV_STATE_SEND_TO_INITALIZE, + NAV_STATE_SEND_TO_IN_PROGESS, + NAV_STATE_SEND_TO_FINISHED, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -406,6 +420,17 @@ typedef enum { RTH_HOME_FINAL_LAND, // Home position and altitude } rthTargetMode_e; +#ifdef USE_GEOZONE +typedef struct navSendTo_s { + fpVector3_t targetPos; + uint16_t altitudeTargetRange; // 0 for only "2D" + uint32_t targetRange; + bool lockSticks; + uint32_t lockStickTime; + timeMs_t startTime; +} navSendTo_t; +#endif + typedef struct { fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming uint32_t distance; // distance to the nearest safehome @@ -478,6 +503,10 @@ typedef struct { fwLandState_t fwLandState; #endif +#ifdef USE_GEOZONE + navSendTo_t sendTo; // Used for Geozones +#endif + /* Internals & statistics */ int16_t rcAdjustment[4]; float totalTripDistance; @@ -502,7 +531,9 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void); bool isThrustFacingDownwards(void); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); +void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance); int32_t calculateBearingToDestination(const fpVector3_t * destinationPos); +float calculateDistance2(const fpVector2_t* startPos, const fpVector2_t* destinationPos); bool isLandingDetected(void); void resetLandingDetector(void); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 176812d8f3f..5d38a6dba59 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -137,6 +137,10 @@ typedef enum { TASK_TELEMETRY_SBUS2, #endif +#if defined (USE_GEOZONE) && defined(USE_GPS) + TASK_GEOZONE, +#endif + /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/common.h b/src/main/target/common.h index f7cc3f747ce..3e035710fca 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -221,3 +221,4 @@ #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER +#define USE_GEOZONE From 314ab9009dd501d0c3313fd399a200fb965dc15d Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:27:08 -0300 Subject: [PATCH 136/175] Bugfix --- src/main/io/osd.c | 116 +++++++++--------- src/main/navigation/navigation.c | 36 ++++-- src/main/navigation/navigation.h | 3 +- src/main/navigation/navigation_geozone.c | 81 ++++++------ .../navigation_geozone_calculations.h | 3 +- 5 files changed, 129 insertions(+), 110 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 0307f63c1ab..dedbf641c10 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -5973,6 +5973,64 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } else if (STATE(LANDING_DETECTED)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED); } else { +#ifdef USE_GEOZONE + char buf[12], buf1[12]; + switch (geozone.messageState) { + case GEOZONE_MESSAGE_STATE_NFZ: + messages[messageCount++] = OSD_MSG_NFZ; + break; + case GEOZONE_MESSAGE_STATE_LEAVING_FZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: + messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; + break; + case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: + osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); + if (geozone.zoneInfo == INT32_MAX) { + tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); + } else { + osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); + } + tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); + messages[messageCount++] = messageBuf; + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_FB: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: + messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: + messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: + messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; + if (!posControl.sendTo.lockSticks) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_POS_HOLD: + messages[messageCount++] = OSD_MSG_AVOIDING_FB; + if (!geozone.sticksLocked) { + messages[messageCount++] = OSD_MSG_MOVE_STICKS; + } + break; + case GEOZONE_MESSAGE_STATE_NONE: + break; + } +#endif /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */ /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ if (STATE(AIRPLANE)) { /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */ @@ -6011,64 +6069,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH); } } -#ifdef USE_GEOZONE - char buf[12], buf1[12]; - switch (geozone.messageState) { - case GEOZONE_MESSAGE_STATE_NFZ: - messages[messageCount++] = OSD_MSG_NFZ; - break; - case GEOZONE_MESSAGE_STATE_LEAVING_FZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - tfp_sprintf(messageBuf, OSD_MSG_LEAVING_FZ, buf); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_OUTSIDE_FZ: - messages[messageCount++] = OSD_MSG_OUTSIDE_FZ; - break; - case GEOZONE_MESSAGE_STATE_ENTERING_NFZ: - osdFormatDistanceSymbol(buf, geozone.distanceToZoneBorder3d, 0, 3); - if (geozone.zoneInfo == INT32_MAX) { - tfp_sprintf(buf1, "%s%c", "INF", SYM_ALT_M); - } else { - osdFormatAltitudeSymbol(buf1, geozone.zoneInfo); - } - tfp_sprintf(messageBuf, OSD_MSG_ENTERING_NFZ, buf, buf1); - messages[messageCount++] = messageBuf; - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_FB: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE: - messages[messageCount++] = OSD_MSG_RETURN_TO_ZONE; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH: - messages[messageCount++] = OSD_MSG_AVOIDING_ALT_BREACH; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_FLYOUT_NFZ: - messages[messageCount++] = OSD_MSG_FLYOUT_NFZ; - if (!posControl.sendTo.lockSticks) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_LOITER: - messages[messageCount++] = OSD_MSG_AVOIDING_FB; - if (!geozone.sticksLocked) { - messages[messageCount++] = OSD_MSG_MOVE_STICKS; - } - break; - case GEOZONE_MESSAGE_STATE_NONE: - break; - } -#endif } else if (STATE(MULTIROTOR)) { /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */ if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bbf8e7910f0..fcde4980f44 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -648,15 +648,16 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE] = NAV_STATE_RTH_INITIALIZE, } }, @@ -3606,14 +3607,15 @@ bool isProbablyStillFlying(void) *-----------------------------------------------------------*/ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { + + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + #ifdef USE_GEOZONE - if ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || - (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 )) { + if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { return 0.0f; } #endif - - const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { @@ -4253,6 +4255,14 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); + if (geozone.sticksLocked) { + posControl.flags.isAdjustingAltitude = false; + posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; + } + if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ad6d60ee06f..2a1dd8484d3 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,7 +131,7 @@ typedef enum { GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE, GEOZONE_MESSAGE_STATE_FLYOUT_NFZ, GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH, - GEOZONE_MESSAGE_STATE_LOITER + GEOZONE_MESSAGE_STATE_POS_HOLD } geozoneMessageState_e; enum fenceAction_e { @@ -197,6 +197,7 @@ typedef struct geozone_s { int32_t zoneInfo; int32_t currentzoneMaxAltitude; int32_t currentzoneMinAltitude; + bool nearestHorZoneHasAction; bool sticksLocked; int8_t loiterDir; bool avoidInRTHInProgress; diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 8a2505a0ff9..80b9cf4def0 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -58,7 +58,7 @@ #define MAX_DISTANCE_FLY_OVER_POINTS 50000 #define MAX_PATH_PONITS (2 + 2 * MAX_VERTICES) #define POS_DETECTION_DISTANCE 7500 -#define STICK_LOCK_MIN_TIME 1500 +#define STICK_LOCK_MIN_TIME 2500 #define AVOID_TIMEOUT 30000 #define MAX_LOCAL_VERTICES 128 #define GEOZONE_INCLUSE_IGNORE_DISTANCE 2000 * 100 // m @@ -109,6 +109,7 @@ static geoZoneConfig_t safeHomeGeozoneConfig; static geozoneActionState_e actionState = GEOZONE_ACTION_STATE_NONE; static timeMs_t actionStartTime = 0; static int32_t avoidCourse; +static geoZoneRuntimeConfig_t *nearestHorZone = NULL; static geoZoneRuntimeConfig_t *nearestInclusiveZone = NULL; static fpVector3_t avoidingPoint; static bool geozoneIsEnabled = false; @@ -650,6 +651,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp return 0; } + // Set starting point slightly away from our current position float offset = geozoneGetDetectionDistance(); if (geozone.distanceVertToNearestZone <= offset) { int bearing = wrap_36000(geozone.directionToNearestZone + 18000); @@ -660,7 +662,11 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp pathPoints[pathPointCount].visited = true; pathPoints[pathPointCount].distance = 0; pathPoints[pathPointCount++].point = start; - + + // Calculate possible waypoints + // Vertices of the zones are possible waypoints, + // inclusive zones are “reduced”, exclusive zones are “enlarged” to keep distance, + // round zones are converted into hexagons and long sides get additional points to be able to fly over zones. for (uint8_t i = 0 ; i < activeGeoZonesCount; i++) { fpVector2_t *verticesZone; fpVector2_t verticesCirclePoly[CIRCLE_POLY_SIDES]; @@ -748,7 +754,7 @@ static int8_t calcRthCourse(fpVector3_t* waypoints, const fpVector3_t* point, fp while (!pathPoints[pathPointCount - 1].visited) { pathPoint_t *next = current; float min = FLT_MAX; - for (uint16_t i = 1; i < pathPointCount; i++) { + for (uint8_t i = 1; i < pathPointCount; i++) { float currentDist = FLT_MAX; if (isPointDirectReachable(¤t->point, &pathPoints[i].point)) { @@ -813,7 +819,7 @@ static void updateZoneInfos(void) fpVector3_t nearestBorderPoint; aboveOrUnderZone = false; - geoZoneRuntimeConfig_t *nearestHorInclZone = NULL; + nearestHorZone = NULL; geoZoneRuntimeConfig_t *currentZones[MAX_GEOZONES]; uint8_t currentZoneCount = getCurrentZones(currentZones, true); int32_t currentMaxAltitude = INT32_MIN, currentMinAltitude = INT32_MAX; @@ -821,11 +827,8 @@ static void updateZoneInfos(void) if (currentZoneCount == 1) { currentMaxAltitude = currentZones[0]->config.maxAltitude; currentMinAltitude = currentZones[0]->config.minAltitude; - - if (!isInZoneAltitudeRange(currentZones[0], getEstimatedActualPosition(Z))) { - nearestHorInclZone = currentZones[0]; - } - + nearestHorZone = currentZones[0]; + } else if (currentZoneCount >= 2) { geoZoneRuntimeConfig_t *aboveZone = NULL, *belowZone = NULL; @@ -837,6 +840,7 @@ static void updateZoneInfos(void) if (isInZoneAltitudeRange(current, getEstimatedActualPosition(Z))) { currentMaxAltitude = MAX(current->config.maxAltitude, currentMaxAltitude); currentMinAltitude = MIN(current->config.minAltitude, currentMinAltitude); + nearestHorZone = current; } if (current->config.minAltitude > getEstimatedActualPosition(Z)) { @@ -859,16 +863,16 @@ static void updateZoneInfos(void) if (aboveZone) { if (aboveZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMaxAltitude = MAX(aboveZone->config.maxAltitude, currentMaxAltitude); - nearestHorInclZone = aboveZone; + nearestHorZone = aboveZone; } else { currentMaxAltitude = MIN(aboveZone->config.minAltitude, currentMaxAltitude); } } if (belowZone) { - if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { + if (belowZone->config.type == GEOZONE_TYPE_INCLUSIVE) { currentMinAltitude = MIN(belowZone->config.minAltitude, currentMinAltitude); - nearestHorInclZone = belowZone; + nearestHorZone = belowZone; } else { currentMinAltitude = MAX(belowZone->config.maxAltitude, currentMinAltitude); } @@ -959,18 +963,19 @@ static void updateZoneInfos(void) if (isPointInCircle((fpVector2_t*)&navGetCurrentActualPositionAndVelocity()->pos, &activeGeoZones[i].verticesLocal[0], activeGeoZones[i].radius)) { directionToBorder = wrap_36000(directionToBorder + 18000); } - geozone.directionToNearestZone = directionToBorder; geozone.distanceHorToNearestZone = roundf(dist); nearestInclusiveZone = &activeGeoZones[i]; } } - if (aboveOrUnderZone && nearestHorInclZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { - nearestInclusiveZone = nearestHorInclZone; + if (aboveOrUnderZone && nearestHorZone != NULL && ABS(geozone.distanceVertToNearestZone) < geozone.distanceHorToNearestZone) { + nearestInclusiveZone = nearestHorZone; geozone.distanceHorToNearestZone = 0; } } + + geozone.nearestHorZoneHasAction = nearestHorZone && nearestHorZone->config.fenceAction != GEOFENCE_ACTION_NONE; } void performeFenceAction(geoZoneRuntimeConfig_t *zone, fpVector3_t *intersection) @@ -1248,7 +1253,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) isInitalised = true; } - if (!ARMING_FLAG(ARMED) || !isGPSHeadingValid() || !isInitalised || activeGeoZonesCount == 0 || ((STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH))) ) { + if (!ARMING_FLAG(ARMED) || !isInitalised || activeGeoZonesCount == 0) { noZoneRTH = false; return; } @@ -1267,6 +1272,10 @@ void geozoneUpdate(timeUs_t curentTimeUs) updateCurrentZones(); updateZoneInfos(); + + if (STATE(AIRPLANE) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { + return; + } // User has switched to non geofence mode, end all actions and switch to mode from box input if (isNonGeozoneModeFromBoxInput()) { @@ -1315,7 +1324,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) return; case GEOZONE_ACTION_STATE_RTH: case GEOZONE_ACTION_STATE_POSHOLD: - geozone.messageState = GEOZONE_MESSAGE_STATE_LOITER; + geozone.messageState = GEOZONE_MESSAGE_STATE_POS_HOLD; if (geozone.sticksLocked && millis() - actionStartTime > STICK_LOCK_MIN_TIME) { geozone.sticksLocked = false; } @@ -1327,25 +1336,26 @@ void geozoneUpdate(timeUs_t curentTimeUs) break; } - bool currentZoneHasAction = false; - for (uint8_t i = 0; i < currentZoneCount; i++) { - if (currentZones[i]->config.fenceAction != GEOFENCE_ACTION_NONE) { - currentZoneHasAction = true; - break; - } - } - - if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && currentZoneHasAction && (geozone.currentzoneMaxAltitude > 0 || geozone.currentzoneMinAltitude != 0) && ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { + if ((IS_RC_MODE_ACTIVE(BOXHORIZON) || IS_RC_MODE_ACTIVE(BOXANGLE)) && + actionState == GEOZONE_ACTION_STATE_NONE && + geozone.nearestHorZoneHasAction && + ABS(geozone.distanceVertToNearestZone) > 0 && + ABS(geozone.distanceVertToNearestZone) < geoZoneConfig()->safeAltitudeDistance) { float targetAltitide = 0; - if (!geozone.insideFz) { - if (geozone.distanceVertToNearestZone < 0) { - targetAltitide = nearestInclusiveZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25; + uint32_t extraSafteyAlt = geoZoneConfig()->safeAltitudeDistance * 0.25; + if (nearestHorZone->config.type == GEOZONE_TYPE_INCLUSIVE && geozone.insideFz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMaxAltitude - extraSafteyAlt; } else { - targetAltitide = nearestInclusiveZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; + targetAltitide = geozone.currentzoneMinAltitude + extraSafteyAlt; + } + } else if (nearestHorZone->config.type == GEOZONE_TYPE_EXCLUSIVE && !geozone.insideNfz) { + if (geozone.distanceVertToNearestZone > 0) { + targetAltitide = geozone.currentzoneMinAltitude - extraSafteyAlt; + } else { + targetAltitide = geozone.currentzoneMaxAltitude + extraSafteyAlt; } - } else { - targetAltitide = geozone.distanceVertToNearestZone > 0 ? geozone.currentzoneMaxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.25 : geozone.currentzoneMinAltitude + geoZoneConfig()->safeAltitudeDistance * 1.25; } fpVector3_t targetPos; @@ -1374,6 +1384,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) lockRTZ = false; } + // RTZ: Return to zone: if (geozone.insideNfz || (!geozone.insideFz && isAtLeastOneInclusiveZoneActive)) { if (isAtLeastOneInclusiveZoneActive && !geozone.insideFz) { @@ -1457,9 +1468,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) } if (!geozone.insideNfz && intersectZone->config.type == GEOZONE_TYPE_EXCLUSIVE && (intersectZone->config.minAltitude != 0 || intersection.z > 0)) { - - geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); - + geozone.distanceToZoneBorder3d = (uint32_t)roundf(distanceToZone); int32_t minAltitude = intersectZone->config.minAltitude; int32_t maxAltitude = intersectZone->config.maxAltitude; if (intersectZone->isInfZone || (minAltitude == 0 && maxAltitude == INT32_MAX)) { @@ -1474,7 +1483,7 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(distToMin) < ABS(distToMax)) { geozone.zoneInfo = distToMin; } else { - geozone.zoneInfo = (distToMax); + geozone.zoneInfo = distToMax; } } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 38849719af0..d29d7deb35f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -348,8 +348,7 @@ static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* dista if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { intersect.x = i1.x; intersect.y = i1.y; - } - else { + } else { intersect.x = i2.x; intersect.y = i2.y; } From 30a0ca9c642c9db089fe842a2b0d32e6ba716bd0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:34:49 -0300 Subject: [PATCH 137/175] Revert VScode files --- .vscode/settings.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index d1eb2357c02..5de759a0db4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -4,7 +4,7 @@ "cmath": "c", "ranges": "c", "navigation.h": "c", - "rth_trackback.h": "c" + "rth_trackback.h": "c", "platform.h": "c", "timer.h": "c", "bus.h": "c" @@ -15,4 +15,4 @@ "editor.expandTabs": true, "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" -} +} \ No newline at end of file From fc7365fd1280a9089c22f9f64d05bf608f54a803 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 00:34:49 -0300 Subject: [PATCH 138/175] Revert VScode files --- .vscode/c_cpp_properties.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3a8d8d1c8ca..3c84282e518 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -41,4 +41,4 @@ } ], "version": 4 -} \ No newline at end of file +} From 7d540a89cf7d70af04f9aa4794f6234530f02402 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 08:29:29 -0300 Subject: [PATCH 139/175] Update Settings.md --- docs/Settings.md | 70 ++++++++++++++++++++++++++++++++ src/main/navigation/navigation.c | 6 ++- 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ce4ff9c1ce4..e35e8168cc1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1472,6 +1472,76 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv --- +### geozone_avoid_altitude_range + +Altitude range in which an attempt is made to avoid a geozone upwards + +| Default | Min | Max | +| --- | --- | --- | +| 5000 | 0 | 1000000 | + +--- + +### geozone_detection_distance + +Distance from which a geozone is detected + +| Default | Min | Max | +| --- | --- | --- | +| 50000 | 0 | 1000000 | + +--- + +### geozone_mr_stop_distance + +Distance in which multirotors stops before the border + +| Default | Min | Max | +| --- | --- | --- | +| 15000 | 0 | 100000 | + +--- + +### geozone_no_way_home_action + +Action if RTH with active geozones is unable to calculate a course to home + +| Default | Min | Max | +| --- | --- | --- | +| RTH | | | + +--- + +### geozone_safe_altitude_distance + +Vertical distance that must be maintained to the upper and lower limits of the zone. + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | 0 | 10000 | + +--- + +### geozone_safehome_as_inclusive + +Treat nearest safehome as inclusive geozone + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### geozone_safehome_zone_action + +Fence action for safehome zone + +| Default | Min | Max | +| --- | --- | --- | +| NONE | | | + +--- + ### gimbal_pan_channel Gimbal pan rc channel index. 0 is no channel. diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fcde4980f44..900cc8f3ba6 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3611,8 +3611,8 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); #ifdef USE_GEOZONE - if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || - (geozone.currentzoneMinAltitude > 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { + if (!emergLandingIsActive && geozone.nearestHorZoneHasAction && ((geozone.currentzoneMaxAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z >= geozone.currentzoneMaxAltitude && posControl.desiredState.climbRateDemand > 0) || + (geozone.currentzoneMinAltitude != 0 && navGetCurrentActualPositionAndVelocity()->pos.z <= geozone.currentzoneMinAltitude && posControl.desiredState.climbRateDemand < 0 ))) { return 0.0f; } #endif @@ -4255,6 +4255,7 @@ static void processNavigationRCAdjustments(void) /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); +#ifdef USE_GEOZONE if (geozone.sticksLocked) { posControl.flags.isAdjustingAltitude = false; posControl.flags.isAdjustingPosition = false; @@ -4262,6 +4263,7 @@ static void processNavigationRCAdjustments(void) return; } +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { From 58d2efddc8993b1395b46160f17935255971d368 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:03:44 -0300 Subject: [PATCH 140/175] Fix MacOS SITL and F722 --- cmake/sitl.cmake | 1 + src/main/navigation/navigation.h | 2 -- src/main/navigation/navigation_geozone.c | 4 ++-- src/main/target/common.h | 4 ++++ 4 files changed, 7 insertions(+), 4 deletions(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 39e6456830a..66e9844e449 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -62,6 +62,7 @@ if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr -Wno-error=maybe-uninitialized + -Wno-double-promotion -fsingle-precision-constant ) if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 2a1dd8484d3..cc944eb9fc9 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -118,8 +118,6 @@ void resetFwAutolandApproach(int8_t idx); #if defined(USE_GEOZONE) -#define MAX_GEOZONES_IN_CONFIG 63 -#define MAX_VERTICES_IN_CONFIG 126 typedef enum { GEOZONE_MESSAGE_STATE_NONE, diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 80b9cf4def0..64714594c44 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -551,7 +551,7 @@ uint32_t geozoneGetDetectionDistance(void) static int32_t calcBounceCourseForZone(geoZoneRuntimeConfig_t *zone, fpVector3_t *prevPoint, fpVector3_t *intersection) { - int32_t course; + int32_t course = 0; if (zone->config.shape == GEOZONE_SHAPE_POLYGON) { fpVector2_t intersect; bool found = false; @@ -1517,7 +1517,7 @@ void geozoneResetRTH(void) rthWaypointCount = 0; } -void geozoneSetupRTH() { +void geozoneSetupRTH(void) { if (!geozone.insideFz && isAtLeastOneInclusiveZoneActive) { noZoneRTH = true; } else { diff --git a/src/main/target/common.h b/src/main/target/common.h index 3e035710fca..bcfb3bbbf60 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,8 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 #elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif @@ -217,6 +219,8 @@ #define SKIP_CLI_COMMAND_HELP #undef USE_SERIALRX_SPEKTRUM #undef USE_TELEMETRY_SRXL + #define MAX_GEOZONES_IN_CONFIG 32 + #define MAX_VERTICES_IN_CONFIG 64 #endif #define USE_EZ_TUNE From 27535ebf167b83b2d4bc07e9fed4b4bd1a49af6b Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:06:23 -0300 Subject: [PATCH 141/175] Oops --- cmake/sitl.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 66e9844e449..9bbf9a5c2c9 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -62,7 +62,6 @@ if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr -Wno-error=maybe-uninitialized - -Wno-double-promotion -fsingle-precision-constant ) if (CMAKE_COMPILER_IS_GNUCC AND NOT CMAKE_C_COMPILER_VERSION VERSION_LESS 12.0) @@ -70,6 +69,7 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} + -Wno-double-promotion ) endif() From a523ad4758746098e77f6c2e7c02794409413d81 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:10:58 -0300 Subject: [PATCH 142/175] MacOs? --- cmake/sitl.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 9bbf9a5c2c9..1f377103246 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -69,7 +69,7 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} - -Wno-double-promotion + -Wno-error=double-promotion ) endif() From 39b10121b858f17791e00574238779987de24f76 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 10:38:34 -0300 Subject: [PATCH 143/175] Exclude F722 --- src/main/fc/runtime_config.h | 1 - src/main/fc/settings.yaml | 1 - src/main/io/osd.c | 6 ++++++ src/main/navigation/navigation.c | 3 +++ src/main/target/common.h | 5 ++--- 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 637ea867e64..452256a6b64 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,6 @@ typedef enum { WAS_EVER_ARMED = (1 << 3), SIMULATOR_MODE_HITL = (1 << 4), SIMULATOR_MODE_SITL = (1 << 5), - ARMING_DISABLED_GEOZONE = (1 << 6), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_NOT_LEVEL = (1 << 8), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 490fab4b914..c64f56a9602 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3829,7 +3829,6 @@ groups: condition: USE_OSD || USE_DJI_HD_OSD members: - name: osd_speed_source - condition: USE_GEOZONE description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" default_value: "GROUND" field: speedSource diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a00f8f97b40..fc3278d8110 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -866,8 +866,14 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); case ARMING_DISABLED_DSHOT_BEEPER: return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); + case ARMING_DISABLED_GEOZONE: +#ifdef USE_GEOZONE return OSD_MESSAGE_STR(OSD_MSG_NFZ); +#else + FALLTHROUGH; +#endif + // Cases without message case ARMING_DISABLED_LANDING_DETECTED: FALLTHROUGH; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 900cc8f3ba6..a72cd027966 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4515,13 +4515,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_RTH; } +#ifdef USE_GEOZONE if (posControl.flags.sendToActive) { return NAV_FSM_EVENT_SWITCH_TO_SEND_TO; } + if (posControl.flags.forcedPosholdActive) { return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; } +#endif /* WP mission activation control: * canActivateWaypoint & waypointWasActivated are used to prevent WP mission diff --git a/src/main/target/common.h b/src/main/target/common.h index bcfb3bbbf60..f4963636715 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,6 +209,7 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#define USE_GEOZONE #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 #elif !defined(STM32F7) @@ -219,10 +220,8 @@ #define SKIP_CLI_COMMAND_HELP #undef USE_SERIALRX_SPEKTRUM #undef USE_TELEMETRY_SRXL - #define MAX_GEOZONES_IN_CONFIG 32 - #define MAX_VERTICES_IN_CONFIG 64 #endif #define USE_EZ_TUNE #define USE_ADAPTIVE_FILTER -#define USE_GEOZONE + From 06abdc5e77a9f9b28916c4d7b9f1684d453f1047 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 8 Nov 2024 11:50:30 -0300 Subject: [PATCH 144/175] Enaable for SITL, Fix feature in cli --- src/main/fc/cli.c | 4 ++-- src/main/target/SITL/target.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d5c115c66b1..cb2be8edb38 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -151,8 +151,8 @@ static uint8_t commandBatchErrorCount = 0; // sync this with features_e static const char * const featureNames[] = { - "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP", - "GEOZONE", "SOFTSERIAL", "GPS", "RPM_FILTERS", + "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "GEOZONE", + "", "SOFTSERIAL", "GPS", "RPM_FILTERS", "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "", "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index f6d80f74971..f7963dfdd95 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -80,6 +80,9 @@ #define USE_HEADTRACKER_MSP #undef USE_DASHBOARD +#define USE_GEOZONE +#define MAX_GEOZONES_IN_CONFIG 63 +#define MAX_VERTICES_IN_CONFIG 126 #undef USE_GYRO_KALMAN // Strange behaviour under x86/x64 ?!? #undef USE_VCP From 363bf09be0b4a9fe600ba287ba4594c41c905de4 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 9 Nov 2024 08:36:01 +0000 Subject: [PATCH 145/175] Updates - Force band and mode to uppercase - Fix bug with band and mode not finalising the string output to the OSD - Changed RSSI dBm and SNR display outputs slightly. So that the OSD is more consistent - Show mW symbol for power elements when no data is present. Again, more consistency and doesn't make people wonder why the elements are not showing up. --- src/main/fc/fc_msp.c | 5 +++++ src/main/io/osd.c | 10 ++++++---- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3182ede9700..989fdee5521 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -34,6 +34,7 @@ #include "common/color.h" #include "common/maths.h" #include "common/streambuf.h" +#include "common/string_light.h" #include "common/bitarray.h" #include "common/time.h" #include "common/utils.h" @@ -2947,9 +2948,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxLinkStatistics.band[i] = sbufReadU8(src); } + sl_toupperptr(rxLinkStatistics.band); + for (int i = 0; i < 6; i++) { rxLinkStatistics.mode[i] = sbufReadU8(src); } + + sl_toupperptr(rxLinkStatistics.mode); } return MSP_RESULT_NO_REPLY; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a7f9f024868..3370ced1075 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2471,7 +2471,7 @@ static bool osdDrawSingleElement(uint8_t item) if (rssi <= -100) { tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); } else { - tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' '); + tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM); } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -2560,7 +2560,7 @@ static bool osdDrawSingleElement(uint8_t item) if (snrFiltered <= -10 || snrFiltered >= 10) { tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); } else { - tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); + tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB); } } break; @@ -2569,7 +2569,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_TX_POWER_UPLINK: { if (!failsafeIsReceivingRxData()) - tfp_sprintf(buff, "%s%c", " ", SYM_BLANK); + tfp_sprintf(buff, "%s%c", " ", SYM_MW); else tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); break; @@ -2578,7 +2578,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_RX_POWER_DOWNLINK: { if (!failsafeIsReceivingRxData()) - tfp_sprintf(buff, "%s%c%c", " ", SYM_BLANK, SYM_BLANK); + tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN); else tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN); break; @@ -2586,11 +2586,13 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_RX_BAND: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND); strcat(buff, rxLinkStatistics.band); + buff[4] = '\0'; break; case OSD_RX_MODE: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE); strcat(buff, rxLinkStatistics.mode); + buff[6] = '\0'; break; #endif From 026ca5897636675af34ff821ddbbd56f2dd7f263 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 9 Nov 2024 10:06:18 +0000 Subject: [PATCH 146/175] Set lengths for strings in band and mode --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3370ced1075..729c98474bd 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2585,13 +2585,13 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_RX_BAND: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND); - strcat(buff, rxLinkStatistics.band); + tfp_sprintf(buff, "%4s", rxLinkStatistics.band); buff[4] = '\0'; break; case OSD_RX_MODE: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE); - strcat(buff, rxLinkStatistics.mode); + tfp_sprintf(buff, "%6s", rxLinkStatistics.mode); buff[6] = '\0'; break; #endif From 619948e7b69b65f094593098dff881879b6a8e77 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Sat, 9 Nov 2024 11:17:52 +0000 Subject: [PATCH 147/175] Make band and mode left aligned --- src/main/io/osd.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 729c98474bd..ac88e83c476 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2585,13 +2585,19 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_RX_BAND: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND); - tfp_sprintf(buff, "%4s", rxLinkStatistics.band); + strcat(buff, rxLinkStatistics.band); + if (strlen(rxLinkStatistics.band) < 4) + for (uint8_t i = strlen(rxLinkStatistics.band); i < 4; i++) + buff[i] = ' '; buff[4] = '\0'; break; case OSD_RX_MODE: displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE); - tfp_sprintf(buff, "%6s", rxLinkStatistics.mode); + strcat(buff, rxLinkStatistics.mode); + if (strlen(rxLinkStatistics.mode) < 6) + for (uint8_t i = strlen(rxLinkStatistics.mode); i < 6; i++) + buff[i] = ' '; buff[6] = '\0'; break; #endif From d2a2a03105e37808cb5447ae63b9ad8793159c06 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:33:58 -0300 Subject: [PATCH 148/175] Improvements from PR --- src/main/fc/cli.c | 30 ++++++++--- src/main/fc/fc_core.c | 12 ++--- src/main/fc/fc_msp.c | 12 +++-- src/main/fc/fc_tasks.c | 6 +-- src/main/io/osd.c | 2 +- src/main/io/osd.h | 2 +- src/main/navigation/navigation.c | 26 ++++----- src/main/navigation/navigation.h | 7 ++- src/main/navigation/navigation_geozone.c | 54 +++++++++++++++++-- .../navigation_geozone_calculations.h | 12 ++--- src/main/target/common.h | 2 + 11 files changed, 118 insertions(+), 47 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index cb2be8edb38..f6506ef110a 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1565,7 +1565,7 @@ static void cliSafeHomes(char *cmdline) #if defined(USE_GEOZONE) static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) { - const char *format = "geozone %u %u %u %d %d %u"; + const char *format = "geozone %u %u %u %d %d %d %u"; for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { bool equalsDefault = false; if (defaultGeoZone) { @@ -1573,11 +1573,12 @@ static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, cons && geoZone[i].shape == defaultGeoZone->shape && geoZone[i].type == defaultGeoZone->type && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude - && geoZone[i].minAltitude == defaultGeoZone->minAltitude; + && geoZone[i].minAltitude == defaultGeoZone->minAltitude + && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].fenceAction); + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction); } - cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].fenceAction); + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction); } } @@ -1722,8 +1723,16 @@ static void cliGeozone(char* cmdLine) uint8_t totalVertices = geozoneGetUsedVerticesCount(); cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); + } else if (sl_strncasecmp(cmdLine, "reset", 5) == 0) { + const char* ptr = &cmdLine[5]; + if ((ptr = nextArg(ptr))) { + int idx = fastA2I(ptr); + geozoneReset(idx); + } else { + geozoneReset(-1); + } } else { - int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0; + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0; int32_t minAltitude = 0, maxAltitude = 0; const char* ptr = cmdLine; uint8_t argumentCount = 1; @@ -1766,6 +1775,14 @@ static void cliGeozone(char* cmdLine) return; } + if ((ptr = nextArg(ptr))){ + argumentCount++; + seaLevelRef = fastA2I(ptr); + } else { + cliShowParseError(); + return; + } + if ((ptr = nextArg(ptr))){ argumentCount++; fenceAction = fastA2I(ptr); @@ -1782,7 +1799,7 @@ static void cliGeozone(char* cmdLine) argumentCount++; } - if (argumentCount != 6) { + if (argumentCount != 7) { cliShowParseError(); return; } @@ -1801,6 +1818,7 @@ static void cliGeozone(char* cmdLine) geoZonesConfigMutable(idx)->maxAltitude = maxAltitude; geoZonesConfigMutable(idx)->minAltitude = minAltitude; + geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef; geoZonesConfigMutable(idx)->fenceAction = fenceAction; } } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index ba29a03d5e0..b2a6c1c8026 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -278,12 +278,12 @@ static void updateArmingStatus(void) } #endif -#if defined(USE_GEOZONE) && defined (USE_GPS) - if (geozoneIsInsideNFZ()) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); - } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); - } +#ifdef USE_GEOZONE + if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } #endif /* CHECK: */ diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c875587ae5e..cad74fa306e 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1771,7 +1771,7 @@ static mspResult_e mspFwApproachOutCommand(sbuf_t *dst, sbuf_t *src) } #endif -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); @@ -1780,6 +1780,7 @@ static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, geoZonesConfig(idx)->shape); sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); sbufWriteU32(dst, geoZonesConfig(idx)->maxAltitude); + sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef); sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); sbufWriteU8(dst, idx); @@ -3365,9 +3366,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) gpsUbloxSendCommand(src->ptr, dataSize, 0); break; -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE case MSP2_INAV_SET_GEOZONE: - if (dataSize == 13) { + if (dataSize == 14) { uint8_t geozoneId; if (!sbufReadU8Safe(&geozoneId, src) || geozoneId >= MAX_GEOZONES_IN_CONFIG) { return MSP_RESULT_ERROR; @@ -3377,7 +3378,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) geoZonesConfigMutable(geozoneId)->type = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->shape = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->minAltitude = sbufReadU32(src); - geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->maxAltitude = sbufReadU32(src); + geoZonesConfigMutable(geozoneId)->isSealevelRef = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->fenceAction = sbufReadU8(src); geoZonesConfigMutable(geozoneId)->vertexCount = sbufReadU8(src); } else { @@ -3986,7 +3988,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = mspFwApproachOutCommand(dst, src); break; #endif -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE case MSP2_INAV_GEOZONE: *ret = mspFcGeozoneOutCommand(dst, src); break; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index df335ccb428..475a69fdc4a 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -339,7 +339,7 @@ void taskUpdateAux(timeUs_t currentTimeUs) #endif } -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE void geozoneUpdateTask(timeUs_t currentTimeUs) { if (feature(FEATURE_GEOZONE)) { @@ -463,7 +463,7 @@ void fcTasksInit(void) serialProxyStart(); #endif -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE setTaskEnabled(TASK_GEOZONE, feature(FEATURE_GEOZONE)); #endif @@ -754,7 +754,7 @@ cfTask_t cfTasks[TASK_COUNT] = { }, #endif -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE [TASK_GEOZONE] = { .taskName = "GEOZONE", .taskFunc = geozoneUpdateTask, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fc3278d8110..74e26018ba2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2); void osdStartedSaveProcess(void) { diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 69d5433e0a5..0952da1fe7f 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -136,7 +136,7 @@ #define OSD_MSG_NFZ "NO FLY ZONE" #define OSD_MSG_LEAVING_FZ "LEAVING FZ IN %s" #define OSD_MSG_OUTSIDE_FZ "OUTSIDE FZ" -#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s" +#define OSD_MSG_ENTERING_NFZ "ENTERING NFZ IN %s %s" #define OSD_MSG_AVOIDING_FB "AVOIDING FENCE BREACH" #define OSD_MSG_RETURN_TO_ZONE "RETURN TO FZ" #define OSD_MSG_FLYOUT_NFZ "FLY OUT NFZ" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a72cd027966..a3de782b2bc 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1713,22 +1713,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { #endif - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL); - if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { - // Successfully reached position target - update XYZ-position - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) { + // Successfully reached position target - update XYZ-position + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - posControl.landingDelay = 0; + posControl.landingDelay = 0; - if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) - posControl.rthState.rthLinearDescentActive = false; + if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive) + posControl.rthState.rthLinearDescentActive = false; - return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING - } else { - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); - return NAV_FSM_EVENT_NONE; - } + return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING + } else { + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY); + return NAV_FSM_EVENT_NONE; + } #ifdef USE_GEOZONE } #endif @@ -3348,7 +3348,7 @@ void updateHomePosition(void) setHome = true; break; } -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE geozoneUpdateMaxHomeAltitude(); #endif } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cc944eb9fc9..9132974e480 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -118,6 +118,9 @@ void resetFwAutolandApproach(int8_t idx); #if defined(USE_GEOZONE) +#ifndef USE_GPS + #error "Geozone needs GPS support" +#endif typedef enum { GEOZONE_MESSAGE_STATE_NONE, @@ -156,6 +159,7 @@ typedef struct geoZoneConfig_s uint8_t type; int32_t minAltitude; int32_t maxAltitude; + bool isSealevelRef; uint8_t fenceAction; uint8_t vertexCount; } geoZoneConfig_t; @@ -209,9 +213,10 @@ bool geozoneSetVertex(uint8_t zoneId, uint8_t vertexId, int32_t lat, int32_t lon int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId); bool isGeozoneActive(void); uint8_t geozoneGetUsedVerticesCount(void); +void geozoneReset(int8_t idx); void geozoneResetVertices(int8_t zoneId, int16_t idx); void geozoneUpdate(timeUs_t curentTimeUs); -bool geozoneIsInsideNFZ(void); +bool geozoneIsBlockingArming(void); void geozoneAdvanceRthAvoidWaypoint(void); int8_t geozoneCheckForNFZAtCourse(bool isRTH); bool geoZoneIsLastRthWaypoint(void); diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 64714594c44..70d9dac0115 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -48,7 +48,7 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" -#if defined(USE_GEOZONE) && defined (USE_GPS) +#ifdef USE_GEOZONE #include "navigation_geozone_calculations.h" @@ -145,6 +145,7 @@ void pgResetFn_geoZonesConfig(geoZoneConfig_t *instance) .type = GEOZONE_SHAPE_CIRCULAR, .minAltitude = 0, .maxAltitude = 0, + .isSealevelRef = false, .fenceAction = GEOFENCE_ACTION_NONE, .vertexCount = 0 ); @@ -174,6 +175,29 @@ uint8_t geozoneGetUsedVerticesCount(void) return count; } +void geozoneReset(const int8_t idx) +{ + if (idx < 0) { + for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { + geoZonesConfigMutable(i)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(i)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(i)->maxAltitude = 0; + geoZonesConfigMutable(i)->minAltitude = 0; + geoZonesConfigMutable(i)->isSealevelRef = false; + geoZonesConfigMutable(i)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(i)->vertexCount = 0; + } + } else if (idx < MAX_GEOZONES_IN_CONFIG) { + geoZonesConfigMutable(idx)->shape = GEOZONE_SHAPE_CIRCULAR; + geoZonesConfigMutable(idx)->type = GEOZONE_TYPE_EXCLUSIVE; + geoZonesConfigMutable(idx)->maxAltitude = 0; + geoZonesConfigMutable(idx)->minAltitude = 0; + geoZonesConfigMutable(idx)->isSealevelRef = false; + geoZonesConfigMutable(idx)->fenceAction = GEOFENCE_ACTION_NONE; + geoZonesConfigMutable(idx)->vertexCount = 0; + } +} + void geozoneResetVertices(const int8_t zoneId, const int16_t idx) { if (zoneId < 0 && idx < 0) { @@ -1129,6 +1153,7 @@ static void endFenceAction(void) static void geoZoneInit(void) { activeGeoZonesCount = 0; + uint8_t expectedVertices = 0, configuredVertices = 0; for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { if (geoZonesConfig(i)->vertexCount > 0) { @@ -1136,6 +1161,17 @@ static void geoZoneInit(void) if (activeGeoZones[i].config.maxAltitude == 0) { activeGeoZones[i].config.maxAltitude = INT32_MAX; } + + if (activeGeoZones[i].config.isSealevelRef) { + + if (activeGeoZones[i].config.maxAltitude != 0) { + activeGeoZones[i].config.maxAltitude -= GPS_home.alt; + } + + if (activeGeoZones[i].config.minAltitude != 0) { + activeGeoZones[i].config.minAltitude -= GPS_home.alt; + } + } activeGeoZones[i].isInfZone = activeGeoZones[i].config.maxAltitude == INT32_MAX && activeGeoZones[i].config.minAltitude == 0; @@ -1146,6 +1182,7 @@ static void geoZoneInit(void) activeGeoZones[activeGeoZonesCount].enable = true; activeGeoZonesCount++; } + expectedVertices += geoZonesConfig(i)->vertexCount; } if (activeGeoZonesCount > 0) { @@ -1155,6 +1192,7 @@ static void geoZoneInit(void) fpVector3_t posLocal3; if (geoZoneVertices(i)->zoneId >= 0 && geoZoneVertices(i)->zoneId < MAX_GEOZONES_IN_CONFIG && geoZoneVertices(i)->idx <= MAX_VERTICES_IN_CONFIG) { + configuredVertices++; if (geoZonesConfig(geoZoneVertices(i)->zoneId)->shape == GEOZONE_SHAPE_CIRCULAR && geoZoneVertices(i)->idx == 1) { activeGeoZones[geoZoneVertices(i)->zoneId].radius = geoZoneVertices(i)->lat; activeGeoZones[geoZoneVertices(i)->zoneId].config.vertexCount = 1; @@ -1180,7 +1218,7 @@ static void geoZoneInit(void) } } } - + if (geoZoneConfig()->nearestSafeHomeAsInclusivZone && posControl.safehomeState.index >= 0) { safeHomeGeozoneConfig.shape = GEOZONE_SHAPE_CIRCULAR; @@ -1194,6 +1232,8 @@ static void geoZoneInit(void) activeGeoZones[activeGeoZonesCount].verticesLocal = (fpVector2_t*)&posControl.safehomeState.nearestSafeHome; activeGeoZones[activeGeoZonesCount].radius = navConfig()->general.safehome_max_distance; activeGeoZonesCount++; + expectedVertices++; + configuredVertices++; } updateCurrentZones(); @@ -1225,7 +1265,7 @@ static void geoZoneInit(void) } activeGeoZonesCount = newActiveZoneCount; - if (activeGeoZonesCount == 0) { + if (activeGeoZonesCount == 0 || expectedVertices != configuredVertices) { setTaskEnabled(TASK_GEOZONE, false); geozoneIsEnabled = false; return; @@ -1525,6 +1565,10 @@ void geozoneSetupRTH(void) { } } +// Return value +// -1: Unable to calculate a course home +// 0: No NFZ in the way +// >0: Number of waypoints int8_t geozoneCheckForNFZAtCourse(bool isRTH) { UNUSED(isRTH); @@ -1575,10 +1619,10 @@ void geozoneUpdateMaxHomeAltitude(void) { } // Avoid arming in NFZ -bool geozoneIsInsideNFZ(void) +bool geozoneIsBlockingArming(void) { // Do not generate arming flags unless we are sure about them - if (!isInitalised || activeGeoZonesCount == 0) { + if (!isInitalised || !geozoneIsEnabled || activeGeoZonesCount == 0) { return false; } diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index d29d7deb35f..789da2b36a4 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -93,13 +93,13 @@ bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line intersection->x = intersection->y = 0; // Double precision is needed here - double s1 = line1End->x - line1Start->x; - double t1 = -(line2End->x - line2Start->x); - double r1 = line2Start->x - line1Start->x; + double s1 = (double)(line1End->x - line1Start->x); + double t1 = (double)(-(line2End->x - line2Start->x)); + double r1 = (double)(line2Start->x - line1Start->x); - double s2 = line1End->y - line1Start->y; - double t2 = -(line2End->y - line2Start->y); - double r2 = line2Start->y - line1Start->y; + double s2 = (double)(line1End->y - line1Start->y); + double t2 = (double)(-(line2End->y - line2Start->y)); + double r2 = (double)(line2Start->y - line1Start->y); // Use Cramer's rule for the solution of the system of linear equations double determ = s1 * t2 - t1 * s2; diff --git a/src/main/target/common.h b/src/main/target/common.h index f4963636715..af0de4bf30f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -209,9 +209,11 @@ #define USE_34CHANNELS #define MAX_MIXER_PROFILE_COUNT 2 #define USE_SMARTPORT_MASTER +#ifdef USE_GPS #define USE_GEOZONE #define MAX_GEOZONES_IN_CONFIG 63 #define MAX_VERTICES_IN_CONFIG 126 +#endif #elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif From ec604486b66a22c51785edb36d9e5333686bcaa0 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:38:57 -0300 Subject: [PATCH 149/175] MacOs (again) --- cmake/sitl.cmake | 1 - src/main/navigation/navigation_geozone.c | 10 +++++----- src/main/navigation/navigation_geozone_calculations.h | 2 +- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 1f377103246..39e6456830a 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -69,7 +69,6 @@ if(NOT MACOSX) endif() else() set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} - -Wno-error=double-promotion ) endif() diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 70d9dac0115..31a60b4084d 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -566,7 +566,7 @@ uint32_t geozoneGetDetectionDistance(void) { uint32_t detctionDistance = 0; if (STATE(AIRPLANE)) { - detctionDistance = navConfig()->fw.loiter_radius * 1.5; + detctionDistance = navConfig()->fw.loiter_radius * 1.5f; } else { detctionDistance = geoZoneConfig()->copterFenceStopDistance; } @@ -1450,9 +1450,9 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (targetZone != NULL && !lockRTZ && (flyOutNfz || (!geozone.insideFz && targetZone->config.fenceAction != GEOFENCE_ACTION_NONE))) { int32_t targetAltitude = 0; if (getEstimatedActualPosition(Z) >= targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance) { - targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = targetZone->config.maxAltitude - geoZoneConfig()->safeAltitudeDistance * 1.5f; } else if (getEstimatedActualPosition(Z) <= targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance) { - targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = targetZone->config.minAltitude + geoZoneConfig()->safeAltitudeDistance * 1.5f; } else { targetAltitude = getEstimatedActualPosition(Z); } @@ -1462,9 +1462,9 @@ void geozoneUpdate(timeUs_t curentTimeUs) if (ABS(geozone.distanceVertToNearestZone) < 2000) { calculateFarAwayTarget(&targetPos, posControl.actualState.cog, 100000); if(geozone.distanceVertToNearestZone > 0) { - targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = getEstimatedActualPosition(Z) + ABS(geozone.distanceVertToNearestZone) + geoZoneConfig()->safeAltitudeDistance * 1.5f; } else { - targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5; + targetAltitude = getEstimatedActualPosition(Z) - ABS(geozone.distanceVertToNearestZone) - geoZoneConfig()->safeAltitudeDistance * 1.5f; } } else { diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h index 789da2b36a4..6d3cd7ad98f 100755 --- a/src/main/navigation/navigation_geozone_calculations.h +++ b/src/main/navigation/navigation_geozone_calculations.h @@ -24,7 +24,7 @@ #include "common/vector.h" #include "navigation/navigation_private.h" -#define K_EPSILON 1e-8 +#define K_EPSILON 1e-8f static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) { From 535dbc5514217394dd266f0a9a0fd7cc81100435 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sun, 10 Nov 2024 17:57:53 -0300 Subject: [PATCH 150/175] Rebase --- src/main/fc/fc_msp.c | 2 +- src/main/io/osd.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b957a561707..0d379a39a09 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1777,6 +1777,7 @@ static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t idx = sbufReadU8(src); if (idx < MAX_GEOZONES_IN_CONFIG) { + sbufWriteU8(dst, idx); sbufWriteU8(dst, geoZonesConfig(idx)->type); sbufWriteU8(dst, geoZonesConfig(idx)->shape); sbufWriteU32(dst, geoZonesConfig(idx)->minAltitude); @@ -1784,7 +1785,6 @@ static mspResult_e mspFcGeozoneOutCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, geoZonesConfig(idx)->isSealevelRef); sbufWriteU8(dst, geoZonesConfig(idx)->fenceAction); sbufWriteU8(dst, geoZonesConfig(idx)->vertexCount); - sbufWriteU8(dst, idx); return MSP_RESULT_ACK; } else { return MSP_RESULT_ERROR; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2f6e9adb847..ef718c9cf01 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -305,13 +305,13 @@ typedef enum { OSD_CUSTOM_ELEMENT_6, OSD_CUSTOM_ELEMENT_7, OSD_CUSTOM_ELEMENT_8, - OSD_COURSE_TO_FENCE, - OSD_H_DIST_TO_FENCE, - OSD_V_DIST_TO_FENCE, OSD_LQ_DOWNLINK, OSD_RX_POWER_DOWNLINK, // 160 OSD_RX_BAND, OSD_RX_MODE, + OSD_COURSE_TO_FENCE, + OSD_H_DIST_TO_FENCE, + OSD_V_DIST_TO_FENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From d32e5d2af279828bcc74075a05100523bdc37407 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Tue, 5 Nov 2024 10:16:54 +0100 Subject: [PATCH 151/175] add lucid h7 --- src/main/target/TBS_LUCID_H7/CMakeLists.txt | 1 + src/main/target/TBS_LUCID_H7/config.c | 39 +++++ src/main/target/TBS_LUCID_H7/target.c | 61 +++++++ src/main/target/TBS_LUCID_H7/target.h | 183 ++++++++++++++++++++ 4 files changed, 284 insertions(+) create mode 100644 src/main/target/TBS_LUCID_H7/CMakeLists.txt create mode 100644 src/main/target/TBS_LUCID_H7/config.c create mode 100644 src/main/target/TBS_LUCID_H7/target.c create mode 100644 src/main/target/TBS_LUCID_H7/target.h diff --git a/src/main/target/TBS_LUCID_H7/CMakeLists.txt b/src/main/target/TBS_LUCID_H7/CMakeLists.txt new file mode 100644 index 00000000000..0586bde8b6d --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(TBS_LUCID_H7) diff --git a/src/main/target/TBS_LUCID_H7/config.c b/src/main/target/TBS_LUCID_H7/config.c new file mode 100644 index 00000000000..9912439df7a --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/config.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/TBS_LUCID_H7/target.c b/src/main/target/TBS_LUCID_H7/target.c new file mode 100644 index 00000000000..30e16b84828 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/target.c @@ -0,0 +1,61 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_gyro1_mpu6000, DEVHW_MPU6000, GYRO1_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro1_icm42688, DEVHW_ICM42605, GYRO1_SPI_BUS, GYRO1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro2_mpu6000, DEVHW_MPU6000, GYRO2_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_gyro2_icm42688, DEVHW_ICM42605, GYRO2_SPI_BUS, GYRO2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // RGB_LED + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h new file mode 100644 index 00000000000..d499464fa53 --- /dev/null +++ b/src/main/target/TBS_LUCID_H7/target.h @@ -0,0 +1,183 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 file 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 http://www.gnu.org/licenses/. + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "LUH7" + +#define USBD_PRODUCT_STRING "TBS_LUCID_H7" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2500 + +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define GYRO1_SPI_BUS BUS_SPI1 +#define GYRO1_CS_PIN PC15 +#define GYRO2_SPI_BUS BUS_SPI4 +#define GYRO2_CS_PIN PC13 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C2 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PITOT_I2C_BUS BUS_I2C2 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 +#define ADC_CHANNEL_4_PIN PC4 +#define ADC_CHANNEL_5_PIN PA4 +#define ADC_CHANNEL_6_PIN PA7 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 +#define PINIO2_PIN PD11 + +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 \ No newline at end of file From dc3477fa46a536ffa2b1c591e90be774c00591e8 Mon Sep 17 00:00:00 2001 From: Mr D - RC Date: Mon, 11 Nov 2024 16:59:25 +0000 Subject: [PATCH 152/175] Update fc_msp.c --- src/main/fc/fc_msp.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 7a0ed98bdf9..57b68f02402 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1862,8 +1862,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } rxMspFrameReceive(frame, channelCount); } - - return MSP_RESULT_NO_REPLY; } break; #endif From 0eaf99b3b48a8d0fc6733699150ffc5340d12a6b Mon Sep 17 00:00:00 2001 From: Scavanger Date: Tue, 12 Nov 2024 11:39:27 -0300 Subject: [PATCH 153/175] CLI --- src/main/fc/cli.c | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index f6506ef110a..eaca5eb2608 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1565,7 +1565,7 @@ static void cliSafeHomes(char *cmdline) #if defined(USE_GEOZONE) static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, const geoZoneConfig_t *defaultGeoZone) { - const char *format = "geozone %u %u %u %d %d %d %u"; + const char *format = "geozone %u %u %u %d %d %u %u %u"; for (uint8_t i = 0; i < MAX_GEOZONES_IN_CONFIG; i++) { bool equalsDefault = false; if (defaultGeoZone) { @@ -1574,11 +1574,13 @@ static void printGeozones(uint8_t dumpMask, const geoZoneConfig_t *geoZone, cons && geoZone[i].type == defaultGeoZone->type && geoZone[i].maxAltitude == defaultGeoZone->maxAltitude && geoZone[i].minAltitude == defaultGeoZone->minAltitude - && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef; + && geoZone[i].isSealevelRef == defaultGeoZone->isSealevelRef + && geoZone[i].fenceAction == defaultGeoZone->fenceAction + && geoZone[i].vertexCount == defaultGeoZone->vertexCount; - cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction); + cliDefaultPrintLinef(dumpMask, equalsDefault, format, defaultGeoZone[i].shape, defaultGeoZone[i].type, defaultGeoZone[i].minAltitude, defaultGeoZone[i].maxAltitude, defaultGeoZone[i].isSealevelRef, defaultGeoZone[i].fenceAction, defaultGeoZone[i].vertexCount); } - cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction); + cliDumpPrintLinef(dumpMask, equalsDefault, format, i, geoZone[i].shape, geoZone[i].type, geoZone[i].minAltitude, geoZone[i].maxAltitude, geoZone[i].isSealevelRef, geoZone[i].fenceAction, geoZone[i].vertexCount); } } @@ -1718,7 +1720,6 @@ static void cliGeozone(char* cmdLine) geoZoneVerticesMutable(vertexIdx)->lon = lon; geoZoneVerticesMutable(vertexIdx)->zoneId = zoneId; geoZoneVerticesMutable(vertexIdx)->idx = vertexZoneIdx; - geoZonesConfigMutable(zoneId)->vertexCount++; uint8_t totalVertices = geozoneGetUsedVerticesCount(); cliPrintLinef("# %u vertices free (Used %u of %u)", MAX_VERTICES_IN_CONFIG - totalVertices, totalVertices, MAX_VERTICES_IN_CONFIG); @@ -1728,11 +1729,13 @@ static void cliGeozone(char* cmdLine) if ((ptr = nextArg(ptr))) { int idx = fastA2I(ptr); geozoneReset(idx); + geozoneResetVertices(idx, -1); } else { geozoneReset(-1); + geozoneResetVertices(-1, -1); } } else { - int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0; + int8_t idx = 0, isPolygon = 0, isInclusive = 0, fenceAction = 0, seaLevelRef = 0, vertexCount = 0; int32_t minAltitude = 0, maxAltitude = 0; const char* ptr = cmdLine; uint8_t argumentCount = 1; @@ -1795,11 +1798,23 @@ static void cliGeozone(char* cmdLine) return; } + if ((ptr = nextArg(ptr))){ + argumentCount++; + vertexCount = fastA2I(ptr); + if (vertexCount < 1 || vertexCount > MAX_VERTICES_IN_CONFIG) { + cliShowArgumentRangeError("vertex count", 1, MAX_VERTICES_IN_CONFIG); + return; + } + } else { + cliShowParseError(); + return; + } + if ((ptr = nextArg(ptr))){ argumentCount++; } - if (argumentCount != 7) { + if (argumentCount != 8) { cliShowParseError(); return; } @@ -1820,6 +1835,7 @@ static void cliGeozone(char* cmdLine) geoZonesConfigMutable(idx)->minAltitude = minAltitude; geoZonesConfigMutable(idx)->isSealevelRef = (bool)seaLevelRef; geoZonesConfigMutable(idx)->fenceAction = fenceAction; + geoZonesConfigMutable(idx)->vertexCount = vertexCount; } } #endif From 574032be41c7b3420ac61b6370d2cdc753c9ce73 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Wed, 13 Nov 2024 11:39:20 +0100 Subject: [PATCH 154/175] Create Geozones.md Round 1 --- docs/Geozones.md | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 docs/Geozones.md diff --git a/docs/Geozones.md b/docs/Geozones.md new file mode 100644 index 00000000000..33197fd258e --- /dev/null +++ b/docs/Geozones.md @@ -0,0 +1,35 @@ +# Geozones + +## Introduction +The Geozone feature allows pilots to define one or multiple areas on the map in Mission Control, to prevent accidental flying outside of allowed zones (Flight-Zones, FZ) or to avoid certain areas they are not allowed or not willing to fly in (No-Flight-Zone, NFZ). +This type of feature might be known to many pilots as a "Geofence" and despite providing the same core functionality, INAV Geozones are significantly more versatile and provide more safety and features. + +Geozones can not only inform the Pilot on the OSD, if the aircraft is approaching a geozone border, it also gives the distance and direction to the closest border and the remaining flight distance to the breach point. Additionally it provides autonomous avoidance features, if the Aircraft is in any kind of self-leveling flight mode. +The most important feature for safety, is the automatic path planning for RTH (Return To Home), that automatically avoids NFZ areas if possible. + +![image](https://github.com/user-attachments/assets/48e3e5cc-8517-4096-8497-399cf00ee541) + + +## Compatibility +- INAV Version: 8.0 or later +- INAV Configurator: 8.0 or Later +- [MWPTools:](https://github.com/stronnag/mwptools) Snapshot 2024-11-xx or later +- Only flight controller with more than 512k of Flash (STM32F405, STM32F765, STM32H743, etc.) +- Plane, Multirotor (Rover and Boat are untested at time of writing) + +## Setup Procedure +- In the INAV Configurator, switch to the Configuration Panel and enable "Geozone" in the features. +- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as the you enter Mission Control. +- Click on the "+" Symbol to add a new zone and define its parameters to your desire. +- The following Options are available: + - Shape: Configures a Zone as a Circle or a Polygon + - Type: Inclusive (FZ, green) or Exclusive (NFZ, red) + - Min. Alt (cm): lower ceiling of the Zone + - Max. Alt (cm): upper ceiling of the Zone + - Action: Ection to execute if an aircraft approaches the border of that Zone + - Radius: Circular Zone only, Radius of the Circle +- Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) +- Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details + +## Functions and Behaviors +- From 5d91e4ed7dd646e00e8574179e82b3ee50def1bf Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:03:27 +0100 Subject: [PATCH 155/175] Update Geozones.md round 2 --- docs/Geozones.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/docs/Geozones.md b/docs/Geozones.md index 33197fd258e..6f316f4adf4 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -32,4 +32,14 @@ The most important feature for safety, is the automatic path planning for RTH (R - Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details ## Functions and Behaviors +- Zone Type: Inclusive + - If craft is armed inside the Inclusive FZ, everything outside that zone is considered a NFZ. + - Inclusive FZ can be combined if they overlap and will be handled as one zone. + - Overlapping multiple FZ allows different upper and lower altitude limits for each zone, as long as they still overlap in 3D Space (Both overlapping zones have to have a overlapping altitude range as well). + - Arming the aircraft outside of an Inclusive Zone is prohibited within a 2km distance to the next vertex (Distance to a border between two vertex is not checked). Arming override can be used. Arming at a distance bigger than 2km is possible. + - Arming a craft outside of an Inclusive FZ will disable all Inclusive zones. +- Zone Type: Exclusive + - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside a NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. + - Exclusive Zones can be combined and overlapped as needed + - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. - From 88c193c2690b3eb718d8f96c2d50e32818cc4f50 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:29:53 +0100 Subject: [PATCH 156/175] Update Geozones.md update 3 --- docs/Geozones.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/docs/Geozones.md b/docs/Geozones.md index 6f316f4adf4..6225a444874 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -29,8 +29,12 @@ The most important feature for safety, is the automatic path planning for RTH (R - Action: Ection to execute if an aircraft approaches the border of that Zone - Radius: Circular Zone only, Radius of the Circle - Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) +- To add additional vertices, click on the border-line of the zone you are editing. This will add a new vertex on that line to move around. - Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details +## Additional Settings +- + ## Functions and Behaviors - Zone Type: Inclusive - If craft is armed inside the Inclusive FZ, everything outside that zone is considered a NFZ. @@ -42,4 +46,17 @@ The most important feature for safety, is the automatic path planning for RTH (R - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside a NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. - Exclusive Zones can be combined and overlapped as needed - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. +- Actions: + - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary “billiard ball effect” + - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling. + - RTH: Triggers return to home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. + - None: No action (only info in OSD) +- RTH: + - If RTH is enabled by Failsafe, User Command or Zone Action, INAV will calculate a path to the Home Location that automatically avoids NFZ and tries to stay inside the current FZ. + - If no Path can be calculated (Not able to climb over a blocking NFZ, No Intersection between FZ, too tight gaps between blocking NFZ) a configurable alternative action will be executed + - Direct RTH: Ignores Flight zones and comes back in a direct path + - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes) + - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed + +## Limitations - From 058eb1cd9a926afb3222e8347ac91b19d5aaa934 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:43:55 +0100 Subject: [PATCH 157/175] Update Geozones.md update 4 --- docs/Geozones.md | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index 6225a444874..356f6525971 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -30,7 +30,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - Radius: Circular Zone only, Radius of the Circle - Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) - To add additional vertices, click on the border-line of the zone you are editing. This will add a new vertex on that line to move around. -- Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details +- Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details) ## Additional Settings - @@ -59,4 +59,11 @@ The most important feature for safety, is the automatic path planning for RTH (R - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed ## Limitations -- +- The maximum amount of dedicated zones of any type is 63. +- The maximum amount of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. +- INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that can not hover. + - Complicated Zone setups with overlaps and tight areas can cause a Loiter or "bounce" into another NFZ that was not considered before. + - This can lead to a "Return to FZ" action that tries to find the shortest path into a legal area. +- All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible to plan his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such area and the pilot disables WP mode, a "Return to FZ" action will be executed. +- All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown. +- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clock wise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before upload. From 9fb51e0655b067aa9ca4e7bcbb1bcfda304fbadc Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Wed, 13 Nov 2024 18:22:57 +0000 Subject: [PATCH 158/175] [mspv2] pass "flags" field back to sender, define ILMI flag (#10464) --- src/main/fc/fc_msp.c | 12 ++++++------ src/main/msp/msp.h | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 57b68f02402..f490d28128a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -2717,7 +2717,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src); } - // API version 1.42 - extension for pitmode frequency + // API version 1.42 - extension for pitmode frequency if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); //skip pitModeFreq } @@ -2738,15 +2738,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (sbufBytesRemaining(src) >= 2) { sbufReadU16(src); // freq } - + if (sbufBytesRemaining(src) >= 3) { sbufReadU8(src); // band count sbufReadU8(src); // channel count - + uint8_t newPowerCount = sbufReadU8(src); if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) { vtxDevice->capability.powerCount = newPowerCount; - } + } } } } @@ -2944,7 +2944,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (sublinkID == 0) { rxLinkStatistics.uplinkTXPower = sbufReadU16(src); rxLinkStatistics.downlinkTXPower = sbufReadU16(src); - + for (int i = 0; i < 4; i++) { rxLinkStatistics.band[i] = sbufReadU8(src); } @@ -4249,7 +4249,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro if (cmd->flags & MSP_FLAG_DONT_REPLY) { ret = MSP_RESULT_NO_REPLY; } - + reply->flags = cmd->flags; reply->result = ret; return ret; } diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index 0c00b81ece7..e988ea2eddf 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -46,6 +46,7 @@ typedef struct mspPacket_s { typedef enum { MSP_FLAG_DONT_REPLY = (1 << 0), + MSP_FLAG_ILMI = (1 << 1), // "In-Line Message identifier" } mspFlags_e; struct serialPort_s; From 9b02a29f85be87738a7001c4dd14d2c35c5cad0b Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 19:32:29 +0100 Subject: [PATCH 159/175] Update Geozones.md --- docs/Geozones.md | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index 356f6525971..b2cf1eef150 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -19,7 +19,8 @@ The most important feature for safety, is the automatic path planning for RTH (R ## Setup Procedure - In the INAV Configurator, switch to the Configuration Panel and enable "Geozone" in the features. -- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as the you enter Mission Control. +- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as the you enter Mission Control. + ![image](https://github.com/user-attachments/assets/23cd2149-b6a2-4945-b349-ee99863e74f0) - Click on the "+" Symbol to add a new zone and define its parameters to your desire. - The following Options are available: - Shape: Configures a Zone as a Circle or a Polygon @@ -30,10 +31,21 @@ The most important feature for safety, is the automatic path planning for RTH (R - Radius: Circular Zone only, Radius of the Circle - Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) - To add additional vertices, click on the border-line of the zone you are editing. This will add a new vertex on that line to move around. + ![image](https://github.com/user-attachments/assets/eacb6d3c-62d3-4bab-8874-1543c0a6b06d) - Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details) +- After finishing the zone setup, click the "Store in EEPROM" Button to save the zones on the Flight Controller. It is important that the FC reboots after storing, as the Zones can only be used after a fresh boot process. + ![image](https://github.com/user-attachments/assets/4b278dd0-aa65-45f6-b914-22bdd753feaf) ## Additional Settings -- +- In the Advanced Tuning Panel you will find additional global settings for Geozones + ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9) + - Detection Distance: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent + - Avoid Altitude Range: When the Aircraft approaches a NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. + - Safe Altitude Distance: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. + - Safehome as Inclusive: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude + - Safehome Zone Action: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. + - Multirotor Stop Distance: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane) + - No Way Home Action: If RTH can not find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. ## Functions and Behaviors - Zone Type: Inclusive @@ -62,8 +74,15 @@ The most important feature for safety, is the automatic path planning for RTH (R - The maximum amount of dedicated zones of any type is 63. - The maximum amount of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. - INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that can not hover. - - Complicated Zone setups with overlaps and tight areas can cause a Loiter or "bounce" into another NFZ that was not considered before. - - This can lead to a "Return to FZ" action that tries to find the shortest path into a legal area. + - Complicated Zone setups with overlaps and tight areas can cause a loiter or "bounce" into another NFZ that was not considered before. + - This can lead to a "Return to FZ" action that tries to find the shortest path into an allowed area. - All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible to plan his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such area and the pilot disables WP mode, a "Return to FZ" action will be executed. - All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown. -- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clock wise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before upload. +- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clock wise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before upload. + - Examples of Zones that are not allowed: + ![image](https://github.com/user-attachments/assets/63d287fe-e2eb-44d4-a8ba-2b862d69eb71) +- To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, have to be at least 2.5x Loiter-Radius apart for Airplanes at at least 2.5x Multirotor Stop Distance apart for Multirotors. + - Example: + ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) +- If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 3x Avoid Altitude Range +- It is not recommended, to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can leat to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. From 57e3f1c1601ea68dbf1566791c04b35a670f05d9 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 19:49:18 +0100 Subject: [PATCH 160/175] Update Geozones.md --- docs/Geozones.md | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index b2cf1eef150..e95d97b9bbc 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -25,8 +25,8 @@ The most important feature for safety, is the automatic path planning for RTH (R - The following Options are available: - Shape: Configures a Zone as a Circle or a Polygon - Type: Inclusive (FZ, green) or Exclusive (NFZ, red) - - Min. Alt (cm): lower ceiling of the Zone - - Max. Alt (cm): upper ceiling of the Zone + - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken for a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower) + - Max. Alt (cm): upper ceiling of the Zone (A value if 0 means no upper altitude limit) - Action: Ection to execute if an aircraft approaches the border of that Zone - Radius: Circular Zone only, Radius of the Circle - Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) @@ -59,7 +59,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - Exclusive Zones can be combined and overlapped as needed - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. - Actions: - - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary “billiard ball effect” + - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, similar to a pool ball bouncing off the table border. Multirotor will switch into "Position Hold". - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling. - RTH: Triggers return to home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. - None: No action (only info in OSD) @@ -69,6 +69,11 @@ The most important feature for safety, is the automatic path planning for RTH (R - Direct RTH: Ignores Flight zones and comes back in a direct path - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes) - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed +- Return to Zone: + - If the Aircraft breaches into a NFZ or out of a FZ (by avoiding in tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. + +## OSD Elements +- ## Limitations - The maximum amount of dedicated zones of any type is 63. From 8820a3ffea35f9ace1c56467cebd66d7e02ce6e3 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 19:52:21 +0100 Subject: [PATCH 161/175] Update Geozones.md --- docs/Geozones.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index e95d97b9bbc..aaab3786bae 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -69,6 +69,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - Direct RTH: Ignores Flight zones and comes back in a direct path - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes) - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed + - To abort the Smart-RTH feature and come back on a direct way, disable and Re-Enable RTH within 1 Second. This temporarily ignores all FZ and NFZ borders. - Return to Zone: - If the Aircraft breaches into a NFZ or out of a FZ (by avoiding in tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. @@ -89,5 +90,5 @@ The most important feature for safety, is the automatic path planning for RTH (R - To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, have to be at least 2.5x Loiter-Radius apart for Airplanes at at least 2.5x Multirotor Stop Distance apart for Multirotors. - Example: ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) -- If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 3x Avoid Altitude Range +- If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m. - It is not recommended, to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can leat to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. From 10502a486ed38f0b3998d6e715e29ccb539d6455 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 20:09:54 +0100 Subject: [PATCH 162/175] Update Geozones.md --- docs/Geozones.md | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index aaab3786bae..703bf3175fa 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -39,12 +39,12 @@ The most important feature for safety, is the automatic path planning for RTH (R ## Additional Settings - In the Advanced Tuning Panel you will find additional global settings for Geozones ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9) - - Detection Distance: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent + - Detection Distance: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent. - Avoid Altitude Range: When the Aircraft approaches a NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. - Safe Altitude Distance: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. - - Safehome as Inclusive: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude + - Safehome as Inclusive: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude. - Safehome Zone Action: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. - - Multirotor Stop Distance: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane) + - Multirotor Stop Distance: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane). - No Way Home Action: If RTH can not find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. ## Functions and Behaviors @@ -56,26 +56,34 @@ The most important feature for safety, is the automatic path planning for RTH (R - Arming a craft outside of an Inclusive FZ will disable all Inclusive zones. - Zone Type: Exclusive - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside a NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. - - Exclusive Zones can be combined and overlapped as needed + - Exclusive Zones can be combined and overlapped as needed. - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. - Actions: - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, similar to a pool ball bouncing off the table border. Multirotor will switch into "Position Hold". - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling. - RTH: Triggers return to home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. - - None: No action (only info in OSD) + - None: No action (only info in OSD). - RTH: - If RTH is enabled by Failsafe, User Command or Zone Action, INAV will calculate a path to the Home Location that automatically avoids NFZ and tries to stay inside the current FZ. - - If no Path can be calculated (Not able to climb over a blocking NFZ, No Intersection between FZ, too tight gaps between blocking NFZ) a configurable alternative action will be executed - - Direct RTH: Ignores Flight zones and comes back in a direct path - - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes) - - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed + - If no Path can be calculated (Not able to climb over a blocking NFZ, No Intersection between FZ, too tight gaps between blocking NFZ) a configurable alternative action will be executed. + - Direct RTH: Ignores Flight zones and comes back in a direct path. + - Emergency Land: Executes a GPS enabled Emergency Landing (Circle down with horizontal position hold active on Planes). + - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed. - To abort the Smart-RTH feature and come back on a direct way, disable and Re-Enable RTH within 1 Second. This temporarily ignores all FZ and NFZ borders. - Return to Zone: - If the Aircraft breaches into a NFZ or out of a FZ (by avoiding in tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. ## OSD Elements -- +- Three dedicated OSD Elements have been added: + - Fence-Distance Horizontal shows the distance to the nearest Fence Border and the heading to that border. + - Fence-Distance Vertical shows the distance to the nearest ceiling or floor of a zone. + - Fence-Direction Vertical is an optional element to show if the nearest vertical border is above or below the aircraft. + ![image](https://github.com/user-attachments/assets/87dd3c5a-1046-4bd4-93af-5f8c9078b868) +- The Flight-Mode will show AUTO if the Aircraft executes any kind of Fence-Action. +- The System-Message shows the distance to a potential fence breach point, based on the current aircraft Attitude and Heading. +- Additionally the System Message shows the current Fence Action that is Executed. + ## Limitations - The maximum amount of dedicated zones of any type is 63. - The maximum amount of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. From 2e587aa33bf23e892ff5e2d89cfee76c803e5a63 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 20:27:33 +0100 Subject: [PATCH 163/175] Update Geozones.md --- docs/Geozones.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index 703bf3175fa..efca30580da 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -27,7 +27,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - Type: Inclusive (FZ, green) or Exclusive (NFZ, red) - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken for a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower) - Max. Alt (cm): upper ceiling of the Zone (A value if 0 means no upper altitude limit) - - Action: Ection to execute if an aircraft approaches the border of that Zone + - Action: Action to execute if an aircraft approaches the border of that Zone - Radius: Circular Zone only, Radius of the Circle - Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) - To add additional vertices, click on the border-line of the zone you are editing. This will add a new vertex on that line to move around. @@ -94,7 +94,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown. - Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clock wise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before upload. - Examples of Zones that are not allowed: - ![image](https://github.com/user-attachments/assets/63d287fe-e2eb-44d4-a8ba-2b862d69eb71) + ![image](https://github.com/user-attachments/assets/50f1a441-39da-4f1c-9128-7375bc593fa5) - To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, have to be at least 2.5x Loiter-Radius apart for Airplanes at at least 2.5x Multirotor Stop Distance apart for Multirotors. - Example: ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) From 5891a39064ccee40bf24b2833e1cba94eab0e8cd Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 21:47:14 +0100 Subject: [PATCH 164/175] Update Geozones.md Added CLI stuff --- docs/Geozones.md | 53 +++++++++++++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 12 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index efca30580da..f95fa31b788 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -36,16 +36,16 @@ The most important feature for safety, is the automatic path planning for RTH (R - After finishing the zone setup, click the "Store in EEPROM" Button to save the zones on the Flight Controller. It is important that the FC reboots after storing, as the Zones can only be used after a fresh boot process. ![image](https://github.com/user-attachments/assets/4b278dd0-aa65-45f6-b914-22bdd753feaf) -## Additional Settings +## Global Settings - In the Advanced Tuning Panel you will find additional global settings for Geozones ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9) - - Detection Distance: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent. - - Avoid Altitude Range: When the Aircraft approaches a NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. - - Safe Altitude Distance: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. - - Safehome as Inclusive: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude. - - Safehome Zone Action: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. - - Multirotor Stop Distance: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane). - - No Way Home Action: If RTH can not find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. + - Detection Distance `geozone_detection_distance`: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent. + - Avoid Altitude Range `geozone_avoid_altitude_range`: When the Aircraft approaches a NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. + - Safe Altitude Distance `geozone_safe_altitude_distance`: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. + - Safehome as Inclusive `geozone_safehome_as_inclusive`: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude. + - Safehome Zone Action `geozone_safehome_zone_action`: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. + - Multirotor Stop Distance `geozone_mr_stop_distance:`: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane). + - No Way Home Action `geozone_no_way_home_action`: If RTH can not find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. ## Functions and Behaviors - Zone Type: Inclusive @@ -75,9 +75,9 @@ The most important feature for safety, is the automatic path planning for RTH (R ## OSD Elements - Three dedicated OSD Elements have been added: - - Fence-Distance Horizontal shows the distance to the nearest Fence Border and the heading to that border. - - Fence-Distance Vertical shows the distance to the nearest ceiling or floor of a zone. - - Fence-Direction Vertical is an optional element to show if the nearest vertical border is above or below the aircraft. + - Fence-Distance Horizontal shows the distance to the nearest Fence Border and the heading to that border. (ID 145) + - Fence-Distance Vertical shows the distance to the nearest ceiling or floor of a zone. (ID 146) + - Fence-Direction Vertical is an optional element to show if the nearest vertical border is above or below the aircraft. (ID 144) ![image](https://github.com/user-attachments/assets/87dd3c5a-1046-4bd4-93af-5f8c9078b868) - The Flight-Mode will show AUTO if the Aircraft executes any kind of Fence-Action. - The System-Message shows the distance to a potential fence breach point, based on the current aircraft Attitude and Heading. @@ -99,4 +99,33 @@ The most important feature for safety, is the automatic path planning for RTH (R - Example: ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) - If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m. -- It is not recommended, to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can leat to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. +- It is not recommended, to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can leat to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. + +## CLI +The Geozone Information are stored in two separate data arrays. The first array holds the main Geozone Information and settings. The second array holds the Geozone vertices. +The following commands are available for users: + +- `geozone` without argument lists the current settings +- `geozone vertex` - lists all vertices. +- `geozone vertex reset` - deletes all vertices. +- `geozone` vertex reset ` - Deletes all vertices of the zone. +- `geozone` vertex reset ` - Deletes the vertex with the corresponding id from a zone. + +The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_! + +`geozone ` + +- id: 0 - 63 +- shape: 0 = Circular, 1 = Polygonal +- type: 0 = Exclusive, 1 = Inclusive +- minimum altitude: In centimetres, 0 = ground +- maximum altitude: In centimetres, 0 = infinity +- fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return to home + +`geozone vertex ` + +- zone id: (0-63) The zone id to which this vertex belongs +- vertex idx: Index of the vertex (0-126) +- latitude/ logitude: Longitude and latitude of the vertex. Values in decimal degrees * 1e7. Example:the value 47.562004o becomes 475620040 + + From 07842239578787f6da5d2cbb3ed6b518ea52f8cc Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 21:58:27 +0100 Subject: [PATCH 165/175] hopefully Final Grammar touches --- docs/Geozones.md | 50 ++++++++++++++++++++++++------------------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index f95fa31b788..b6648182790 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -4,8 +4,8 @@ The Geozone feature allows pilots to define one or multiple areas on the map in Mission Control, to prevent accidental flying outside of allowed zones (Flight-Zones, FZ) or to avoid certain areas they are not allowed or not willing to fly in (No-Flight-Zone, NFZ). This type of feature might be known to many pilots as a "Geofence" and despite providing the same core functionality, INAV Geozones are significantly more versatile and provide more safety and features. -Geozones can not only inform the Pilot on the OSD, if the aircraft is approaching a geozone border, it also gives the distance and direction to the closest border and the remaining flight distance to the breach point. Additionally it provides autonomous avoidance features, if the Aircraft is in any kind of self-leveling flight mode. -The most important feature for safety, is the automatic path planning for RTH (Return To Home), that automatically avoids NFZ areas if possible. +Geozones can not only inform the Pilot on the OSD, if the aircraft is approaching a geozone border, it also gives the distance and direction to the closest border and the remaining flight distance to the breach point. Additionally, it provides autonomous avoidance features, if the Aircraft is in any kind of self-leveling flight mode. +The most important feature for safety is the automatic path planning for RTH (Start Return To Home), that automatically avoids NFZ areas if possible. ![image](https://github.com/user-attachments/assets/48e3e5cc-8517-4096-8497-399cf00ee541) @@ -19,49 +19,49 @@ The most important feature for safety, is the automatic path planning for RTH (R ## Setup Procedure - In the INAV Configurator, switch to the Configuration Panel and enable "Geozone" in the features. -- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as the you enter Mission Control. +- Switch to the Mission Control Panel and you will see a new Icon to call up the Geozone Editor. If Zones are already set up, they will be loaded automatically, as soon as you enter Mission Control. ![image](https://github.com/user-attachments/assets/23cd2149-b6a2-4945-b349-ee99863e74f0) - Click on the "+" Symbol to add a new zone and define its parameters to your desire. - The following Options are available: - Shape: Configures a Zone as a Circle or a Polygon - Type: Inclusive (FZ, green) or Exclusive (NFZ, red) - - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken for a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower) + - Min. Alt (cm): lower ceiling of the Zone (0 represents the ground relative from the launch location or AMSL. No action will be taken at a minimum altitude of 0, so the aircraft can "dive out" of an Inclusive FZ on a hill. To have a Minimum Altitude action, set a negative altitude of -1 or lower) - Max. Alt (cm): upper ceiling of the Zone (A value if 0 means no upper altitude limit) - Action: Action to execute if an aircraft approaches the border of that Zone - Radius: Circular Zone only, Radius of the Circle -- Move the Zone-Markers to the desired locations, to create a bordered area with the needed shape and size (Or change the radius in case of a Circular Zone) -- To add additional vertices, click on the border-line of the zone you are editing. This will add a new vertex on that line to move around. +- Move the Zone-Markers to the desired locations, to create a bordered area with the shape and size needed (Or change the radius in case of a Circular Zone) +- To add additional vertices, click on the borderline of the zone you are editing. This will add a new vertex to that line to move around. ![image](https://github.com/user-attachments/assets/eacb6d3c-62d3-4bab-8874-1543c0a6b06d) - Add additional Zones as you like, Zones can be separated but also overlapping (See [Limitations]( ) for details) - After finishing the zone setup, click the "Store in EEPROM" Button to save the zones on the Flight Controller. It is important that the FC reboots after storing, as the Zones can only be used after a fresh boot process. ![image](https://github.com/user-attachments/assets/4b278dd0-aa65-45f6-b914-22bdd753feaf) ## Global Settings -- In the Advanced Tuning Panel you will find additional global settings for Geozones +- In the Advanced Tuning Panel, you will find additional global settings for Geozones ![image](https://github.com/user-attachments/assets/db567521-e256-4fb6-8ca6-6e6b8b57d7a9) - Detection Distance `geozone_detection_distance`: Defines at what distance a Geozone will be shown as a System Message if a breach is imminent. - - Avoid Altitude Range `geozone_avoid_altitude_range`: When the Aircraft approaches a NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. + - Avoid Altitude Range `geozone_avoid_altitude_range`: When the Aircraft approaches an NFZ that has a upper limit (can be overflown at higher altitude), INAV will climb above the Zone automatically if the altitude difference between Zone ceiling and current Aircraft altitude is less, than this value. For fixed wing, you need to consider how steep the possible climb angle is. - Safe Altitude Distance `geozone_safe_altitude_distance`: Vertical safety margin to avoid a ceiling or floor altitude breach at high vertical speed. If your FZ Ceiling is at 100m and this value set to 10m, the aircraft will not allow you to fly above 90m and descents if the Aircraft overshoots. - Safehome as Inclusive `geozone_safehome_as_inclusive`: Defines every Safehome location as a circular Inclusive zone with the radius of `safehome_max_distance` to allow a FZ at ground altitude (For Landings) if the general FZ around it might have a minimum altitude. - Safehome Zone Action `geozone_safehome_zone_action`: Defines the action on zone breach if Safehome is enabled as inclusive. This is helpful for flying fields with distance or altitude restrictions for LOS Pilots. - Multirotor Stop Distance `geozone_mr_stop_distance:`: Distance from the Border a Multirotor will stop, if the Fence Action is Avoid or Loiter (For fixed wings, this will be calculated from the Loiter-Radius of the Plane). - - No Way Home Action `geozone_no_way_home_action`: If RTH can not find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. + - No Way Home Action `geozone_no_way_home_action`: If RTH cannot find a possible route in FS or RTH modes, the Aircraft will either emergency land or fly straight back home and ignores NFZ. ## Functions and Behaviors - Zone Type: Inclusive - - If craft is armed inside the Inclusive FZ, everything outside that zone is considered a NFZ. + - If craft is armed inside the Inclusive FZ, everything outside that zone is considered an NFZ. - Inclusive FZ can be combined if they overlap and will be handled as one zone. - Overlapping multiple FZ allows different upper and lower altitude limits for each zone, as long as they still overlap in 3D Space (Both overlapping zones have to have a overlapping altitude range as well). - Arming the aircraft outside of an Inclusive Zone is prohibited within a 2km distance to the next vertex (Distance to a border between two vertex is not checked). Arming override can be used. Arming at a distance bigger than 2km is possible. - Arming a craft outside of an Inclusive FZ will disable all Inclusive zones. - Zone Type: Exclusive - - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside a NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. + - Arming inside an Exclusive NFZ is prohibited. Arming override can be used but not recommended. If armed inside an NFZ the Pilot keeps control until the NFZ is left, then any configured Action will be executed on re-enter. - Exclusive Zones can be combined and overlapped as needed. - Exclusive NFZ with an upper or lower limit other than 0 can be overflown and underflown. The Automatic avoidance will only fly above NFZ if possible and never below. - Actions: - - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, similar to a pool ball bouncing off the table border. Multirotor will switch into "Position Hold". + - Avoid: Also called “Bounce” (only airplanes): The aircraft flies away from the boundary at the same angle it approached it, like a pool ball bouncing off the table border. Multirotor will switch into "Position Hold". - Hold: Position in front of the boundary is held. Airplances will adjust their loiter center according to the loider radius, to stay away from the border while circling. - - RTH: Triggers return to home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. + - RTH: Triggers Return To Home. The Failsafe RTH Procedure is executed, so RTH Trackback is also used if enabled for Failsafe situations. - None: No action (only info in OSD). - RTH: - If RTH is enabled by Failsafe, User Command or Zone Action, INAV will calculate a path to the Home Location that automatically avoids NFZ and tries to stay inside the current FZ. @@ -71,7 +71,7 @@ The most important feature for safety, is the automatic path planning for RTH (R - When direct "Line of sight" with the Home location is reached (No zones blocking path), regular RTH settings are executed. - To abort the Smart-RTH feature and come back on a direct way, disable and Re-Enable RTH within 1 Second. This temporarily ignores all FZ and NFZ borders. - Return to Zone: - - If the Aircraft breaches into a NFZ or out of a FZ (by avoiding in tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. + - If the Aircraft breaches into an NFZ or out of a FZ (by avoiding tight angled Zones or flown in Acro mode and then the mode switches to any Self-Level mode), RTZ is initiated and the aircraft flies back into the last permitted zone on the shortest possible course. ## OSD Elements - Three dedicated OSD Elements have been added: @@ -81,25 +81,25 @@ The most important feature for safety, is the automatic path planning for RTH (R ![image](https://github.com/user-attachments/assets/87dd3c5a-1046-4bd4-93af-5f8c9078b868) - The Flight-Mode will show AUTO if the Aircraft executes any kind of Fence-Action. - The System-Message shows the distance to a potential fence breach point, based on the current aircraft Attitude and Heading. -- Additionally the System Message shows the current Fence Action that is Executed. +- Additionally, the System Message shows the current Fence Action that is Executed. ## Limitations -- The maximum amount of dedicated zones of any type is 63. -- The maximum amount of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. -- INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that can not hover. +- The maximum number of dedicated zones of any type is 63. +- The maximum number of vertices of all zones combined is 127. Every circular zone uses 2 vertices while every polygon has to consist of at least 3 vertices. +- INAV can only execute one border-breach action at a time. This is especially important to consider for Airplanes that cannot hover. - Complicated Zone setups with overlaps and tight areas can cause a loiter or "bounce" into another NFZ that was not considered before. - This can lead to a "Return to FZ" action that tries to find the shortest path into an allowed area. -- All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible to plan his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such area and the pilot disables WP mode, a "Return to FZ" action will be executed. +- All Geozone Actions are disabled when in Waypoint Mode. The Pilot is responsible for planning his mission accordingly, to not create a path that crosses NFZ areas. If a mission leads to such an area and the pilot disables WP mode, a "Return to FZ" action will be executed. - All Geozone Actions are disabled in ACRO and MANUAL Mode. INAV will not take over control in these modes and only OSD Warnings are shown. -- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clock wise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before upload. +- Planning the Geozone as a polygon, needs to have the vertex numbering to be counter clockwise in ascending order. One vertex must not be dragged over another border to create crossing borders within one zone. INAV Configurator and MWP will check for that before uploading. - Examples of Zones that are not allowed: ![image](https://github.com/user-attachments/assets/50f1a441-39da-4f1c-9128-7375bc593fa5) -- To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, have to be at least 2.5x Loiter-Radius apart for Airplanes at at least 2.5x Multirotor Stop Distance apart for Multirotors. +- To properly combine multiple Inclusion FZ into one area, the Zones need to overlap at 2 borders and the points where the borders touch, must be at least 2.5x Loiter-Radius apart from Airplanes at least 2.5x Multirotor Stop Distance apart for Multirotor. - Example: ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) - If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m. -- It is not recommended, to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can leat to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. +- It is not recommended to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can lead to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. ## CLI The Geozone Information are stored in two separate data arrays. The first array holds the main Geozone Information and settings. The second array holds the Geozone vertices. @@ -108,8 +108,8 @@ The following commands are available for users: - `geozone` without argument lists the current settings - `geozone vertex` - lists all vertices. - `geozone vertex reset` - deletes all vertices. -- `geozone` vertex reset ` - Deletes all vertices of the zone. -- `geozone` vertex reset ` - Deletes the vertex with the corresponding id from a zone. +- `geozone vertex reset ` - Deletes all vertices of the zone. +- `geozone vertex reset ` - Deletes the vertex with the corresponding id from a zone. The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_! @@ -120,7 +120,7 @@ The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY - type: 0 = Exclusive, 1 = Inclusive - minimum altitude: In centimetres, 0 = ground - maximum altitude: In centimetres, 0 = infinity -- fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return to home +- fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return To Home `geozone vertex ` From b9700f889584f9fe6e35938ff53aaeb3e528c1e0 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 22:21:14 +0100 Subject: [PATCH 166/175] Update Geozones.md added missing CLI commands and array field --- docs/Geozones.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index b6648182790..c94167d2040 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -106,6 +106,7 @@ The Geozone Information are stored in two separate data arrays. The first array The following commands are available for users: - `geozone` without argument lists the current settings +- `geozone reset ` resets a specific geozone and all related vertices. If no ID proveded, all geozones and vertices will be deleted. - `geozone vertex` - lists all vertices. - `geozone vertex reset` - deletes all vertices. - `geozone vertex reset ` - Deletes all vertices of the zone. @@ -113,7 +114,7 @@ The following commands are available for users: The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_! -`geozone ` +`geozone ` - id: 0 - 63 - shape: 0 = Circular, 1 = Polygonal @@ -121,6 +122,7 @@ The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY - minimum altitude: In centimetres, 0 = ground - maximum altitude: In centimetres, 0 = infinity - fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return To Home +- vertices count: 0-126 - Sanity check if number of vertices matches with configured zones `geozone vertex ` From 74b0a9b86fb25a9d522b5b40ffc92edc3b739bcc Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 22:44:30 +0100 Subject: [PATCH 167/175] Update Geozones.md --- docs/Geozones.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Geozones.md b/docs/Geozones.md index c94167d2040..4f9e8494c13 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -99,6 +99,7 @@ The most important feature for safety is the automatic path planning for RTH (St - Example: ![image](https://github.com/user-attachments/assets/cc50e24b-dc83-4408-bcba-90d6da33eb63) - If multiple zones with different minimum and maximum altitudes are combined, they need to vertically overlap at least 50m. +- There is a chance that Smart RTH cannot find a path around NFZ areas, if there are multiple very big zones blocking the path. Due to hardware limitations, the amount of waypoints that Smart RTH can create are limited. Many Zones with very long border lines (>500m) cause additional waypoints. - It is not recommended to edit geozones in CLI by hand as this bypasses a lot of sanity checks. Potential errors in zones will disable them or can lead to unexpected behaviors. Transferring Geozones with a DIFF between aircraft is fine. ## CLI From 4c3049c8b3210f0781fccb3a23048175afb7f8b0 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Thu, 14 Nov 2024 22:56:07 +0100 Subject: [PATCH 168/175] Update Geozones.md added AMSL to geozone fields --- docs/Geozones.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index 4f9e8494c13..e4d898ac780 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -115,13 +115,14 @@ The following commands are available for users: The following information are for app-developers. _DO NOT EDIT GEOZONES MANUALLY CLI_! -`geozone ` +`geozone ` - id: 0 - 63 - shape: 0 = Circular, 1 = Polygonal - type: 0 = Exclusive, 1 = Inclusive - minimum altitude: In centimetres, 0 = ground - maximum altitude: In centimetres, 0 = infinity +- is_amsl: 0 = relative, 1 = AMSL - fence action: 0 = None, 1 = Avoid, 2 = Position hold, 3 = Return To Home - vertices count: 0-126 - Sanity check if number of vertices matches with configured zones From 3d6b23518e56190f54290f6d4a5d7e37880fc4f1 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 15 Nov 2024 09:21:13 -0300 Subject: [PATCH 169/175] Move calculations to c file --- src/main/navigation/navigation_geozone.c | 457 ++++++++++++++++- .../navigation_geozone_calculations.h | 479 ------------------ 2 files changed, 455 insertions(+), 481 deletions(-) delete mode 100755 src/main/navigation/navigation_geozone_calculations.h diff --git a/src/main/navigation/navigation_geozone.c b/src/main/navigation/navigation_geozone.c index 31a60b4084d..dfc7539859b 100755 --- a/src/main/navigation/navigation_geozone.c +++ b/src/main/navigation/navigation_geozone.c @@ -50,8 +50,6 @@ #ifdef USE_GEOZONE -#include "navigation_geozone_calculations.h" - #define MAX_VERTICES (MAX_VERTICES_IN_CONFIG + 1) #define MAX_GEOZONES (MAX_GEOZONES_IN_CONFIG + 1) // +1 for safe home @@ -67,6 +65,8 @@ #define GEOZONE_INACTIVE INT8_MAX #define RTH_OVERRIDE_TIMEOUT 1000 +#define K_EPSILON 1e-8f + #define IS_IN_TOLERANCE_RANGE(a, b, t) (((a) > (b) - (t)) && ((a) < (b) + (t))) typedef enum { @@ -274,6 +274,459 @@ int8_t geozoneGetVertexIdx(uint8_t zoneId, uint8_t vertexId) return -1; } + +static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) +{ + return calculateDistance2(point, center) < radius; +} + +static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) +{ + bool result = false; + fpVector2_t *p1, *p2; + fpVector2_t* prev = &vertices[verticesNum - 1]; + fpVector2_t *current; + for (uint8_t i = 0; i < verticesNum; i++) + { + current = &vertices[i]; + + if (current->x > prev->x) { + p1 = prev; + p2 = current; + } else { + p1 = current; + p2 = prev; + } + + if ((current->x < point->x) == (point->x <= prev->x) + && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) + { + result = !result; + } + prev = current; + } + return result; +} + +static float angelFromSideLength(const float a, const float b, const float c) +{ + return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); +} + +static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { + return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; +} + +static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) +{ + float ab = calculateDistance2(a, b); + float ac = calculateDistance2(a, c); + float bc = calculateDistance2(b, c); + + return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); +} + +static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) +{ + fpVector2_t dir, a; + float fac; + vector2Sub(&dir, end, start); + fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); + vector2Scale(&a, &dir, fac); + vector2Add(result, start, &a); +} + +// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection +bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) +{ + intersection->x = intersection->y = 0; + + // Double precision is needed here + double s1 = (double)(line1End->x - line1Start->x); + double t1 = (double)(-(line2End->x - line2Start->x)); + double r1 = (double)(line2Start->x - line1Start->x); + + double s2 = (double)(line1End->y - line1Start->y); + double t2 = (double)(-(line2End->y - line2Start->y)); + double r2 = (double)(line2Start->y - line1Start->y); + + // Use Cramer's rule for the solution of the system of linear equations + double determ = s1 * t2 - t1 * s2; + if (determ == 0) { // No solution + return false; + } + + double s0 = (r1 * t2 - t1 * r2) / determ; + double t0 = (s1 * r2 - r1 * s2) / determ; + + if (s0 == 0 && t0 == 0) { + return false; + } + + // No intersection + if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { + return false; + } + + intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); + intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); + + return true; +} + +float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) +{ + return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); +} + +static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) +{ + fpVector3_t result, a, b; + vectorSub(&a, p2, p1); + vectorSub(&b, p3, p1); + vectorCrossProduct(&result, &a, &b); + return result; +} + +static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) +{ + fpVector3_t result; + vectorSub(&result, p1, p2); + return result; +} + +static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) +{ + for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { + polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; + polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; + } +} + +// TRUE if point is in the same direction from pos as ref +static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) +{ + fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); + fpVector3_t dir = calcDirVectorFromPoints(point, pos); + return vectorDotProduct(&refDir, &dir) < 0.0f; +} + +static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) +{ + fpVector2_t ap, ab, prod2, result; + float distance, magAb, prod; + vector2Sub(&ap, point, lineStart); + vector2Sub(&ab, lineEnd, lineStart); + magAb = vector2NormSquared(&ab); + prod = vector2DotProduct(&ap, &ab); + distance = prod / magAb; + if (distance < 0) { + return *lineStart; + } else if (distance > 1) { + return *lineEnd; + } + vector2Scale(&prod2, &ab, distance); + vector2Add(&result, lineStart, &prod2); + return result; +} + +static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) +{ + float a = roundf(calculateDistance2(linepoint, lineStart)); + float b = roundf(calculateDistance2(linepoint, lineEnd)); + float c = roundf(calculateDistance2(lineStart, lineEnd)); + return a + b == c; +} + +static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) +{ + float a = roundf(calculateDistance3(linepoint, lineStart)); + float b = roundf(calculateDistance3(linepoint, lineEnd)); + float c = roundf(calculateDistance3(lineStart, lineEnd)); + return a + b == c; +} + +static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) +{ + if (vectorDotProduct(linePoint, planeNormal) == 0) { + return false; + } + fpVector3_t diff, p1, p4; + float p2 = 0, p3 = 0; + vectorSub(&diff, linePoint, planePoint); + vectorAdd(&p1, &diff, planePoint); + p2 = vectorDotProduct(&diff, planeNormal); + p3 = vectorDotProduct(lineVector, planeNormal); + vectorScale(&p4, lineVector, -p2 / p3); + vectorAdd(result, &p1, &p4); + return true; +} + + +// Möller–Trumbore intersection algorithm +static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) +{ + fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; + float det, invDet, t, u, v; + + vectorSub(&v0v1, v1, v0); + vectorSub(&v0v2, v2, v0); + vectorCrossProduct(&pvec, dir, &v0v2); + det = vectorDotProduct(&v0v1, &pvec); + if (fabsf(det) < K_EPSILON) { + return false; + } + invDet = 1.f / det; + vectorSub(&tvec, org, v0); + u = vectorDotProduct(&tvec, &pvec) * invDet; + if (u < 0 || u > 1) { + return false; + } + vectorCrossProduct(&quvec, &tvec, &v0v1); + v = vectorDotProduct(dir, &quvec) * invDet; + if (v < 0 || u + v > 1) { + return false; + } + t = vectorDotProduct(&v0v2, &quvec) * invDet; + vectorScale(&prod, dir, t); + vectorAdd(intersection, &prod, org); + return true; +} + + +static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) +{ + if (verticesNum < 3) { + return false; + } + + fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; + fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; + fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; + fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); + fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); + + fpVector3_t tmp; + if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { + if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { + memcpy(intersect, &tmp, sizeof(fpVector3_t)); + return true; + } + } + return false; +} + +static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) +{ + // Unfortunately, we need double precision here + double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); + double yIntercept = (double)startPos->y - slope * (double)startPos->x; + + double a = (double)1.0 + sq(slope); + double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); + double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); + + double discr = sq(b) - (double)4.0 * a * c; + if (discr > 0) + { + double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y1 = slope * x1 + yIntercept; + double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); + double y2 = slope * x2 + yIntercept; + + intersection1->x = (float)x1; + intersection1->y = (float)y1; + intersection2->x = (float)x2; + intersection2->y = (float)y2; + return true; + } + return false; +} + +static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) +{ + fpVector2_t* prev = &verticesOld[numVertices - 1]; + fpVector2_t* current; + fpVector2_t* next; + for (uint8_t i = 0; i < numVertices; i++) { + current = &verticesOld[i]; + if (i + 1 < numVertices) { + next = &verticesOld[i + 1]; + } + else { + next = &verticesOld[0]; + } + + fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; + vector2Sub(&v, current, prev); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcp1.x = prev->x - vs.y; + pcp1.y = prev->y + vs.x; + pcp2.x = current->x - vs.y; + pcp2.y = current->y + vs.x; + + vector2Sub(&v, next, current); + vector2Normalize(&vn, &v); + vector2Scale(&vs, &vn, offset); + pcn1.x = current->x - vs.y; + pcn1.y = current->y + vs.x; + pcn2.x = next->x - vs.y; + pcn2.y = next->y + vs.x; + + if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { + verticesNew[i].x = intersect.x; + verticesNew[i].y = intersect.y; + } + prev = current; + } +} + +// Calculates the nearest intersection point +// Inspired by raytracing algortyhms +static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t i1, i2; + if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { + return false; + } + + if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { + intersect.x = i1.x; + intersect.y = i1.y; + } else { + intersect.x = i2.x; + intersect.y = i2.y; + } + + float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); + float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); + fpVector2_t p4, p5, p6, p7; + p4.x = 0; + p4.y = endPos->z; + p5.x = dist2; + p5.y = startPos->z; + p6.x = dist1; + p6.y = circleCenter->z; + p7.x = dist1; + p7.y = circleCenter->z + height; + + fpVector2_t heightIntersection; + if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { + intersect.z = heightIntersection.y; + if (isInFront(startPos, &intersect, endPos)) { + distToIntersection = calculateDistance3(startPos, &intersect); + } + } + + fpVector3_t intersectCap; + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { + fpVector3_t p1 = *circleCenter; + p1.x = circleCenter->x + radius; + fpVector3_t p2 = *circleCenter; + p2.y = circleCenter->y + radius; + fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > circleCenter->z + height || inside) { + fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; + fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; + fpVector3_t p3 = *circleCenter; + p3.z = circleCenter->z + height; + fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); + + if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) + && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) + && isInFront(startPos, &intersectCap, endPos)) { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + +static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) +{ + float distToIntersection = FLT_MAX; + fpVector3_t intersect; + + fpVector2_t* prev = &vertices[numVertices - 1]; + fpVector2_t* current; + for (uint8_t i = 0; i < numVertices; i++) { + current = &vertices[i]; + + fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; + fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; + fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; + fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; + + fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); + fpVector3_t intersectCurrent; + if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) + || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { + float distWall = calculateDistance3(startPos, &intersectCurrent); + if (distWall < distToIntersection) { + distToIntersection = distWall; + intersect = intersectCurrent; + } + } + prev = current; + } + + fpVector3_t intersectCap; + if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (startPos->z > maxHeight || isInclusiveZone) { + if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) + { + float distanceCap = calculateDistance3(startPos, &intersectCap); + if (distanceCap < distToIntersection) { + distToIntersection = distanceCap; + intersect = intersectCap; + } + } + } + + if (distToIntersection < FLT_MAX) { + *distance = distToIntersection; + memcpy(intersection, &intersect, sizeof(fpVector3_t)); + return true; + } + return false; +} + static bool areSticksDeflectdFromChannelValue(void) { return ABS(rxGetChannelValue(ROLL) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(PITCH) - PWM_RANGE_MIDDLE) + ABS(rxGetChannelValue(YAW) - PWM_RANGE_MIDDLE) >= STICK_MOVE_THRESHOULD; diff --git a/src/main/navigation/navigation_geozone_calculations.h b/src/main/navigation/navigation_geozone_calculations.h deleted file mode 100755 index 6d3cd7ad98f..00000000000 --- a/src/main/navigation/navigation_geozone_calculations.h +++ /dev/null @@ -1,479 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV 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. - * - * INAV 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 INAV. If not, see . - */ - -#pragma once - -#include -#include -#include - -#include "common/vector.h" -#include "navigation/navigation_private.h" - -#define K_EPSILON 1e-8f - -static bool isPointInCircle(const fpVector2_t *point, const fpVector2_t *center, const float radius) -{ - return calculateDistance2(point, center) < radius; -} - -static bool isPointInPloygon(const fpVector2_t *point, fpVector2_t* vertices, const uint8_t verticesNum) -{ - bool result = false; - fpVector2_t *p1, *p2; - fpVector2_t* prev = &vertices[verticesNum - 1]; - fpVector2_t *current; - for (uint8_t i = 0; i < verticesNum; i++) - { - current = &vertices[i]; - - if (current->x > prev->x) { - p1 = prev; - p2 = current; - } else { - p1 = current; - p2 = prev; - } - - if ((current->x < point->x) == (point->x <= prev->x) - && (point->y - p1->y) * (p2->x - p1->x) < (p2->y - p1->y) * (point->x - p1->x)) - { - result = !result; - } - prev = current; - } - return result; -} - -static float angelFromSideLength(const float a, const float b, const float c) -{ - return acos_approx((sq(b) + sq(c) - sq(a)) / (2 * b * c)); -} - -static bool isPointRightFromLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) { - return (lineEnd->x - lineStart->x) * (point->y - lineStart->y) - (lineEnd->y - lineStart->y) * (point->x - lineStart->x) > 0; -} - -static float calcAngelFrom3Points(const fpVector2_t* a, const fpVector2_t* b, const fpVector2_t* c) -{ - float ab = calculateDistance2(a, b); - float ac = calculateDistance2(a, c); - float bc = calculateDistance2(b, c); - - return RADIANS_TO_DEGREES(angelFromSideLength(ab, ac, bc)); -} - -static void calcPointOnLine(fpVector2_t *result, const fpVector2_t *start, fpVector2_t *end, float distance) -{ - fpVector2_t dir, a; - float fac; - vector2Sub(&dir, end, start); - fac = distance / fast_fsqrtf(vector2NormSquared(&dir)); - vector2Scale(&a, &dir, fac); - vector2Add(result, start, &a); -} - -// Intersection of two lines https://en.wikipedia.org/wiki/Line-line_intersection -bool calcLineLineIntersection(fpVector2_t* intersection, const fpVector2_t* line1Start, const fpVector2_t* line1End, const fpVector2_t* line2Start, const fpVector2_t* line2End, bool isSegment) -{ - intersection->x = intersection->y = 0; - - // Double precision is needed here - double s1 = (double)(line1End->x - line1Start->x); - double t1 = (double)(-(line2End->x - line2Start->x)); - double r1 = (double)(line2Start->x - line1Start->x); - - double s2 = (double)(line1End->y - line1Start->y); - double t2 = (double)(-(line2End->y - line2Start->y)); - double r2 = (double)(line2Start->y - line1Start->y); - - // Use Cramer's rule for the solution of the system of linear equations - double determ = s1 * t2 - t1 * s2; - if (determ == 0) { // No solution - return false; - } - - double s0 = (r1 * t2 - t1 * r2) / determ; - double t0 = (s1 * r2 - r1 * s2) / determ; - - if (s0 == 0 && t0 == 0) { - return false; - } - - // No intersection - if (isSegment && (s0 <= 0 || s0 >= 1 || t0 <= 0 || t0 >= 1)) { - return false; - } - - intersection->x = (line1Start->x + (float)s0 * (line1End->x - line1Start->x)); - intersection->y = (line1Start->y + (float)s0 * (line1End->y - line1Start->y)); - - return true; -} - -float calculateDistance3(const fpVector3_t* startPos, const fpVector3_t* destinationPos) -{ - return fast_fsqrtf(sq(destinationPos->x - startPos->x) + sq(destinationPos->y - startPos->y) + sq(destinationPos->z - startPos->z)); -} - -static fpVector3_t calcPlaneNormalFromPoints(const fpVector3_t* p1, const fpVector3_t* p2, const fpVector3_t* p3) -{ - fpVector3_t result, a, b; - vectorSub(&a, p2, p1); - vectorSub(&b, p3, p1); - vectorCrossProduct(&result, &a, &b); - return result; -} - -static fpVector3_t calcDirVectorFromPoints(const fpVector3_t* p1, const fpVector3_t* p2) -{ - fpVector3_t result; - vectorSub(&result, p1, p2); - return result; -} - -static void generatePolygonFromCircle(fpVector2_t* polygon, const fpVector2_t* center, const float radius, const uint8_t sides) -{ - for (uint8_t i = 0, j = sides -1; i < sides; i++, j--) { - polygon[i].x = radius * cos_approx(2 * M_PIf * j / sides) + center->x; - polygon[i].y = radius * sin_approx(2 * M_PIf * j / sides) + center->y; - } -} - -// TRUE if point is in the same direction from pos as ref -static bool isInFront(const fpVector3_t *pos, const fpVector3_t *point, const fpVector3_t *ref) -{ - fpVector3_t refDir = calcDirVectorFromPoints(pos, ref); - fpVector3_t dir = calcDirVectorFromPoints(point, pos); - return vectorDotProduct(&refDir, &dir) < 0.0f; -} - -static fpVector2_t calcNearestPointOnLine(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *point) -{ - fpVector2_t ap, ab, prod2, result; - float distance, magAb, prod; - vector2Sub(&ap, point, lineStart); - vector2Sub(&ab, lineEnd, lineStart); - magAb = vector2NormSquared(&ab); - prod = vector2DotProduct(&ap, &ab); - distance = prod / magAb; - if (distance < 0) { - return *lineStart; - } else if (distance > 1) { - return *lineEnd; - } - vector2Scale(&prod2, &ab, distance); - vector2Add(&result, lineStart, &prod2); - return result; -} - -static bool isPointOnLine2(const fpVector2_t *lineStart, const fpVector2_t *lineEnd, const fpVector2_t *linepoint) -{ - float a = roundf(calculateDistance2(linepoint, lineStart)); - float b = roundf(calculateDistance2(linepoint, lineEnd)); - float c = roundf(calculateDistance2(lineStart, lineEnd)); - return ABS(a + b - c) <= 1; -} - -static bool isPointOnLine3(const fpVector3_t *lineStart, const fpVector3_t *lineEnd, const fpVector3_t *linepoint) -{ - float a = roundf(calculateDistance3(linepoint, lineStart)); - float b = roundf(calculateDistance3(linepoint, lineEnd)); - float c = roundf(calculateDistance3(lineStart, lineEnd)); - return ABS(a + b - c) <= 1; -} - -static bool calcIntersectionLinePlane(fpVector3_t* result, const fpVector3_t* lineVector, const fpVector3_t* linePoint, const fpVector3_t* planeNormal, const fpVector3_t* planePoint) -{ - if (vectorDotProduct(linePoint, planeNormal) == 0) { - return false; - } - fpVector3_t diff, p1, p4; - float p2 = 0, p3 = 0; - vectorSub(&diff, linePoint, planePoint); - vectorAdd(&p1, &diff, planePoint); - p2 = vectorDotProduct(&diff, planeNormal); - p3 = vectorDotProduct(lineVector, planeNormal); - vectorScale(&p4, lineVector, -p2 / p3); - vectorAdd(result, &p1, &p4); - return true; -} - - -// Möller–Trumbore intersection algorithm -static bool calcLineTriangleIntersection(fpVector3_t *intersection, const fpVector3_t* org, const fpVector3_t* dir, const fpVector3_t* v0, const fpVector3_t* v1, const fpVector3_t* v2) -{ - fpVector3_t v0v1, v0v2, pvec, tvec, quvec, prod; - float det, invDet, t, u, v; - - vectorSub(&v0v1, v1, v0); - vectorSub(&v0v2, v2, v0); - vectorCrossProduct(&pvec, dir, &v0v2); - det = vectorDotProduct(&v0v1, &pvec); - if (fabsf(det) < K_EPSILON) { - return false; - } - invDet = 1.f / det; - vectorSub(&tvec, org, v0); - u = vectorDotProduct(&tvec, &pvec) * invDet; - if (u < 0 || u > 1) { - return false; - } - vectorCrossProduct(&quvec, &tvec, &v0v1); - v = vectorDotProduct(dir, &quvec) * invDet; - if (v < 0 || u + v > 1) { - return false; - } - t = vectorDotProduct(&v0v2, &quvec) * invDet; - vectorScale(&prod, dir, t); - vectorAdd(intersection, &prod, org); - return true; -} - - -static bool calcLinePolygonIntersection(fpVector3_t *intersect, const fpVector3_t *pos, const fpVector3_t *pos2, const float height, fpVector2_t* vertices, const uint8_t verticesNum) -{ - if (verticesNum < 3) { - return false; - } - - fpVector3_t p1 = { .x = vertices[0].x, .y = vertices[0].y, .z = height }; - fpVector3_t p2 = { .x = vertices[1].x, .y = vertices[1].y, .z = height }; - fpVector3_t p3 = { .x = vertices[2].x, .y = vertices[2].y, .z = height }; - fpVector3_t normale = calcPlaneNormalFromPoints(&p1, &p2, &p3); - fpVector3_t dir = calcDirVectorFromPoints(pos, pos2); - - fpVector3_t tmp; - if (calcIntersectionLinePlane(&tmp, &dir, pos2, &normale, &p1)) { - if (isPointInPloygon((fpVector2_t*)&tmp, vertices, verticesNum)) { - memcpy(intersect, &tmp, sizeof(fpVector3_t)); - return true; - } - } - return false; -} - -static bool calcLineCircleIntersection(fpVector2_t *intersection1, fpVector2_t *intersection2, const fpVector2_t *startPos, const fpVector2_t *endPos, const fpVector2_t *circleCenter, const float radius) -{ - // Unfortunately, we need double precision here - double slope = (double)((endPos->y - startPos->y) / (endPos->x - startPos->x)); - double yIntercept = (double)startPos->y - slope * (double)startPos->x; - - double a = (double)1.0 + sq(slope); - double b = (double)-2.0 * (double)circleCenter->x + (double)2.0 * slope * (yIntercept - (double)circleCenter->y); - double c = sq((double)circleCenter->x) + (yIntercept - (double)circleCenter->y) * (yIntercept - (double)circleCenter->y) - sq((double)radius); - - double discr = sq(b) - (double)4.0 * a * c; - if (discr > 0) - { - double x1 = (-b + (double)fast_fsqrtf(discr)) / ((double)2.0 * a); - double y1 = slope * x1 + yIntercept; - double x2 = (-b - (double)fast_fsqrtf(discr)) / ((double)2.0 * a); - double y2 = slope * x2 + yIntercept; - - intersection1->x = (float)x1; - intersection1->y = (float)y1; - intersection2->x = (float)x2; - intersection2->y = (float)y2; - return true; - } - return false; -} - -static void generateOffsetPolygon(fpVector2_t* verticesNew, fpVector2_t* verticesOld, const uint8_t numVertices, const float offset) -{ - fpVector2_t* prev = &verticesOld[numVertices - 1]; - fpVector2_t* current; - fpVector2_t* next; - for (uint8_t i = 0; i < numVertices; i++) { - current = &verticesOld[i]; - if (i + 1 < numVertices) { - next = &verticesOld[i + 1]; - } - else { - next = &verticesOld[0]; - } - - fpVector2_t v, vn, vs, pcp1, pcp2, pcn1, pcn2, intersect; - vector2Sub(&v, current, prev); - vector2Normalize(&vn, &v); - vector2Scale(&vs, &vn, offset); - pcp1.x = prev->x - vs.y; - pcp1.y = prev->y + vs.x; - pcp2.x = current->x - vs.y; - pcp2.y = current->y + vs.x; - - vector2Sub(&v, next, current); - vector2Normalize(&vn, &v); - vector2Scale(&vs, &vn, offset); - pcn1.x = current->x - vs.y; - pcn1.y = current->y + vs.x; - pcn2.x = next->x - vs.y; - pcn2.y = next->y + vs.x; - - if (calcLineLineIntersection(&intersect, &pcp1, &pcp2, &pcn1, &pcn2, false)) { - verticesNew[i].x = intersect.x; - verticesNew[i].y = intersect.y; - } - prev = current; - } -} - -// Calculates the nearest intersection point -// Inspired by raytracing algortyhms -static bool calcLineCylinderIntersection(fpVector3_t* intersection, float* distance, const fpVector3_t* startPos, const fpVector3_t* endPos, const fpVector3_t* circleCenter, const float radius, const float height, const bool inside) -{ - float distToIntersection = FLT_MAX; - fpVector3_t intersect; - - fpVector2_t i1, i2; - if (!calcLineCircleIntersection(&i1, &i2, (fpVector2_t*)startPos, (fpVector2_t*)endPos, (fpVector2_t*)circleCenter, radius)) { - return false; - } - - if (calculateDistance2((fpVector2_t*)startPos, &i1) < calculateDistance2((fpVector2_t*)startPos, &i2)) { - intersect.x = i1.x; - intersect.y = i1.y; - } else { - intersect.x = i2.x; - intersect.y = i2.y; - } - - float dist1 = calculateDistance2((fpVector2_t*)endPos, (fpVector2_t*)&intersect); - float dist2 = calculateDistance2((fpVector2_t*)startPos, (fpVector2_t*)endPos); - fpVector2_t p4, p5, p6, p7; - p4.x = 0; - p4.y = endPos->z; - p5.x = dist2; - p5.y = startPos->z; - p6.x = dist1; - p6.y = circleCenter->z; - p7.x = dist1; - p7.y = circleCenter->z + height; - - fpVector2_t heightIntersection; - if (calcLineLineIntersection(&heightIntersection, &p4, &p5, &p6, &p7, true)) { - intersect.z = heightIntersection.y; - if (isInFront(startPos, &intersect, endPos)) { - distToIntersection = calculateDistance3(startPos, &intersect); - } - } - - fpVector3_t intersectCap; - fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); - if (startPos->z < circleCenter->z || (inside && circleCenter->z != 0)) { - fpVector3_t p1 = *circleCenter; - p1.x = circleCenter->x + radius; - fpVector3_t p2 = *circleCenter; - p2.y = circleCenter->y + radius; - fpVector3_t normal = calcPlaneNormalFromPoints(circleCenter, &p1, &p2); - - if (calcIntersectionLinePlane(&intersectCap, &dir, endPos, &normal, circleCenter) - && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) - && isInFront(startPos, &intersectCap, endPos)) { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (startPos->z > circleCenter->z + height || inside) { - fpVector3_t p1 = { .x = circleCenter->x + radius, .y = circleCenter->y, .z = circleCenter->z + height }; - fpVector3_t p2 = { .x = circleCenter->x, .y = circleCenter->y + radius, .z = circleCenter->z + height }; - fpVector3_t p3 = *circleCenter; - p3.z = circleCenter->z + height; - fpVector3_t normal = calcPlaneNormalFromPoints(&p3, &p1, &p2); - - if (calcIntersectionLinePlane(&intersectCap, &dir, startPos, &normal, &p3) - && isPointInCircle((fpVector2_t*)&intersectCap, (fpVector2_t*)circleCenter, radius) - && isInFront(startPos, &intersectCap, endPos)) { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (distToIntersection < FLT_MAX) { - *distance = distToIntersection; - memcpy(intersection, &intersect, sizeof(fpVector3_t)); - return true; - } - return false; -} - -static bool calcLine3dPolygonIntersection(fpVector3_t *intersection, float *distance, const fpVector3_t *startPos, const fpVector3_t *endPos, fpVector2_t *vertices, const uint8_t numVertices, const float minHeight, const float maxHeight, const bool isInclusiveZone) -{ - float distToIntersection = FLT_MAX; - fpVector3_t intersect; - - fpVector2_t* prev = &vertices[numVertices - 1]; - fpVector2_t* current; - for (uint8_t i = 0; i < numVertices; i++) { - current = &vertices[i]; - - fpVector3_t p1 = { .x = prev->x, .y = prev->y, .z = minHeight }; - fpVector3_t p2 = { .x = prev->x, .y = prev->y, .z = maxHeight }; - fpVector3_t p3 = { .x = current->x, .y = current->y, .z = minHeight }; - fpVector3_t p4 = { .x = current->x, .y = current->y, .z = maxHeight}; - - fpVector3_t dir = calcDirVectorFromPoints(startPos, endPos); - fpVector3_t intersectCurrent; - if ((calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p1, &p2, &p3) - || calcLineTriangleIntersection(&intersectCurrent, startPos, &dir, &p3, &p4, &p2)) && isInFront(startPos, &intersectCurrent, endPos) ) { - float distWall = calculateDistance3(startPos, &intersectCurrent); - if (distWall < distToIntersection) { - distToIntersection = distWall; - intersect = intersectCurrent; - } - } - prev = current; - } - - fpVector3_t intersectCap; - if (startPos->z < minHeight || (isInclusiveZone && minHeight != 0)) { - if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, minHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) - { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (startPos->z > maxHeight || isInclusiveZone) { - if (calcLinePolygonIntersection(&intersectCap, startPos, endPos, maxHeight, vertices, numVertices) && isInFront(startPos, &intersectCap, endPos)) - { - float distanceCap = calculateDistance3(startPos, &intersectCap); - if (distanceCap < distToIntersection) { - distToIntersection = distanceCap; - intersect = intersectCap; - } - } - } - - if (distToIntersection < FLT_MAX) { - *distance = distToIntersection; - memcpy(intersection, &intersect, sizeof(fpVector3_t)); - return true; - } - return false; -} From a79b2ef5b4862267a7e8d36accb893f2c77093c0 Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Fri, 15 Nov 2024 10:39:09 -0300 Subject: [PATCH 170/175] EZ-Tune Filter-HZ min value 20 --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1960e5dac67..6e5d78deb1a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1584,7 +1584,7 @@ groups: description: "EzTune filter cutoff frequency" default_value: 110 field: filterHz - min: 10 + min: 20 max: 300 - name: ez_axis_ratio description: "EzTune axis ratio" From 4873cd7e1d882711f2bdbbefda302386eace32b7 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 15 Nov 2024 14:45:17 +0100 Subject: [PATCH 171/175] Added MWP required snapshot date --- docs/Geozones.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index e4d898ac780..4f68709ce3c 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -13,7 +13,7 @@ The most important feature for safety is the automatic path planning for RTH (St ## Compatibility - INAV Version: 8.0 or later - INAV Configurator: 8.0 or Later -- [MWPTools:](https://github.com/stronnag/mwptools) Snapshot 2024-11-xx or later +- [MWPTools:](https://github.com/stronnag/mwptools) Snapshot 2024-11-15 or later - Only flight controller with more than 512k of Flash (STM32F405, STM32F765, STM32H743, etc.) - Plane, Multirotor (Rover and Boat are untested at time of writing) From 170652aee1ab48e64b063316f307d5b186c6089f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Fri, 15 Nov 2024 14:47:21 +0100 Subject: [PATCH 172/175] Added application links and changed format --- docs/Geozones.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Geozones.md b/docs/Geozones.md index 4f68709ce3c..483faaef05a 100644 --- a/docs/Geozones.md +++ b/docs/Geozones.md @@ -11,9 +11,9 @@ The most important feature for safety is the automatic path planning for RTH (St ## Compatibility -- INAV Version: 8.0 or later -- INAV Configurator: 8.0 or Later -- [MWPTools:](https://github.com/stronnag/mwptools) Snapshot 2024-11-15 or later +- [INAV Version: 8.0 or later](https://github.com/iNavFlight/inav/releases) +- [INAV Configurator: 8.0 or Later](https://github.com/iNavFlight/inav-configurator/releases) +- [MWPTools: Snapshot 2024-11-15 or later](https://github.com/stronnag/mwptools) - Only flight controller with more than 512k of Flash (STM32F405, STM32F765, STM32H743, etc.) - Plane, Multirotor (Rover and Boat are untested at time of writing) From 00ced6fcc4eb2e430568a6cb142af13f3385f1f5 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Fri, 15 Nov 2024 10:51:28 -0300 Subject: [PATCH 173/175] Update Docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index f52da9c2c4b..8bbabe91b82 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -908,7 +908,7 @@ EzTune filter cutoff frequency | Default | Min | Max | | --- | --- | --- | -| 110 | 10 | 300 | +| 110 | 20 | 300 | --- From 0a87c219e5dac9dbd878a4ba0e3a9c7e559cc3b2 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Fri, 15 Nov 2024 15:33:51 +0000 Subject: [PATCH 174/175] add annotated script to generate RN Changelog (#10468) --- src/utils/generate-prlist-rn.rb | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100755 src/utils/generate-prlist-rn.rb diff --git a/src/utils/generate-prlist-rn.rb b/src/utils/generate-prlist-rn.rb new file mode 100755 index 00000000000..92496794881 --- /dev/null +++ b/src/utils/generate-prlist-rn.rb @@ -0,0 +1,17 @@ +#!/usr/bin/env ruby + +# This script creates a list of **merged** PRs for a release + +# gh --repo inavflight/inav pr list --state merged -S "milestone:8.0" -L 500 --json "number,title,author,url" > /tmp/gh.json +# Then process the output `/tmp/gh.json` with this script +# ./generate-prlist-rn.rb /tmp/gh.json > /tmp/rel8prs.md +# +# Then merge the contents of `/tmp/rel8prs.md` into the release notes + +require 'json' +abort("Need the JSON file") unless ARGV[0] +text = File.read(ARGV[0]) +jsa = JSON.parse(text) +jsa.each do |js| + puts "* #%d %s by @%s\n" % [js['number'],js['title'],js['author']['login']] +end From 25ae13fdfa70536d3cf7e50dbb7a4ce604a23931 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Sat, 16 Nov 2024 16:01:21 -0300 Subject: [PATCH 175/175] RC2 Fixses --- src/main/fc/fc_init.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e9ed6efe4c1..bc84d380a37 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -524,6 +524,10 @@ void init(void) ezTuneUpdate(); #endif +#ifndef USE_GEOZONE + featureClear(FEATURE_GEOZONE); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC);