From 7142b86cabe5b643fac0f9941b21bdba8fcfd416 Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 13:51:12 +1100 Subject: [PATCH 01/45] * feat: added target for AIKONF7 --- CMakeLists.txt | 99 +-------------------- src/main/target/AIKONF7/target.c | 60 +++++++++++++ src/main/target/AIKONF7/target.h | 144 +++++++++++++++++++++++++++++++ 3 files changed, 205 insertions(+), 98 deletions(-) create mode 100644 src/main/target/AIKONF7/target.c create mode 100644 src/main/target/AIKONF7/target.h diff --git a/CMakeLists.txt b/CMakeLists.txt index db8ec36897b..86c842526d6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,98 +1 @@ -cmake_minimum_required(VERSION 3.13...3.18) - -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") - -set(MAIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}") -set(MAIN_LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib") -set(MAIN_UTILS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/utils") -set(MAIN_SRC_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/main") -set(SVD_DIR "${CMAKE_CURRENT_SOURCE_DIR}/dev/svd") -set(DOWNLOADS_DIR "${MAIN_DIR}/downloads") -set(TOOLS_DIR "${MAIN_DIR}/tools") - -option(SITL "SITL build for host system" OFF) - -set(TOOLCHAIN_OPTIONS none arm-none-eabi host) -if (SITL) - if (CMAKE_HOST_APPLE) - set(MACOSX TRUE) - endif() - set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") -else() - set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") -endif() - -set_property(CACHE TOOLCHAIN PROPERTY STRINGS ${TOOLCHAIN_OPTIONS}) -if("" STREQUAL TOOLCHAIN) - set(TOOLCHAIN none) -endif() -if (NOT ${TOOLCHAIN} IN_LIST TOOLCHAIN_OPTIONS) - message(FATAL_ERROR "Invalid toolchain ${TOOLCHAIN}. Valid options are: ${TOOLCHAIN_OPTIONS}") -endif() - -option(COMPILER_VERSION_CHECK "Ensure the compiler matches the expected version" ON) - -include(GetGitRevisionDescription) -get_git_head_revision(GIT_REFSPEC GIT_SHA1) -string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV) - -# Load settings related functions, so the tests can use them -include(main) -include(settings) - -if(TOOLCHAIN STREQUAL none) - add_subdirectory(src/test) -else() - if (SITL) - include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake") - else() - set(CMAKE_TOOLCHAIN_FILE "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake") - include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") - endif() -endif() - -project(INAV VERSION 8.0.0) - -enable_language(ASM) - -if(MACOSX AND SITL) - set(CMAKE_C_STANDARD 11) -else() - set(CMAKE_C_STANDARD 99) -endif() -set(CMAKE_C_EXTENSIONS ON) -set(CMAKE_C_STANDARD_REQUIRED ON) -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_EXTENSIONS ON) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") - set(IS_RELEASE_BUILD ON) -endif() - -set(FIRMWARE_VERSION ${PROJECT_VERSION}) - -option(WARNINGS_AS_ERRORS "Make all warnings into errors") -message("-- toolchain: ${TOOLCHAIN}, WARNINGS_AS_ERRORS: ${WARNINGS_AS_ERRORS}") - -set(COMMON_COMPILE_DEFINITIONS - FC_VERSION_MAJOR=${CMAKE_PROJECT_VERSION_MAJOR} - FC_VERSION_MINOR=${CMAKE_PROJECT_VERSION_MINOR} - FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH} -) - -if (NOT SITL) - include(openocd) - include(svd) -endif() - -include(stm32) -include(at32) -include(sitl) - -add_subdirectory(src) - -collect_targets() - -message("-- Build type: ${CMAKE_BUILD_TYPE}") -include(ci) +target_stm32f722xe(AIKONF7 SKIP_RELEASES) diff --git a/src/main/target/AIKONF7/target.c b/src/main/target/AIKONF7/target.c new file mode 100644 index 00000000000..7f5d8453e14 --- /dev/null +++ b/src/main/target/AIKONF7/target.c @@ -0,0 +1,60 @@ +/* + * 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" + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM10, CH1, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM1, CH3N, PB1, 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(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h new file mode 100644 index 00000000000..4fe95d6c4dc --- /dev/null +++ b/src/main/target/AIKONF7/target.h @@ -0,0 +1,144 @@ +/* + * 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 "RPTY" +#define USBD_PRODUCT_STRING "AIKONF7" +// Beeper +#define USE_BEEPER +#define BEEPER PC15 +#define BEEPER_INVERTED +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PA15 +#define LED0 PC13 +// UARTs +#define USB_IO +#define USE_VCP +#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 SERIAL_PORT_COUNT 5 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +// 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 PB15 +#define SPI2_MOSI_PIN PC2 +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB5 +#define SPI3_MOSI_PIN PB4 +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +// ADC +#define ADC_CHANNEL_1_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define ADC_CHANNEL_3_PIN PC3 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#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 CW0_DEG +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define IMU_MPU6000_ALIGN CW0_DEG +// BARO +#define USE_BARO +#define USE_BARO_ALL +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PB2 +// 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 M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PB0 +#define USE_FLASH_W25M +#define W25M_SPI_BUS BUS_SPI3 +#define W25M_CS_PIN PB0 +#define USE_FLASH_W25M02G +#define W25M02G_SPI_BUS BUS_SPI3 +#define W25M02G_CS_PIN PB0 +#define USE_FLASH_W25M512 +#define W25M512_SPI_BUS BUS_SPI3 +#define W25M512_CS_PIN PB0 +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PB0 + +// PINIO + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC14 + + +// Others + +#define MAX_PWM_OUTPUT_PORTS 8 +#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 d8cb89ec96e0c8f1c54c927f11a8d8cab3db5c3f Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 15:25:24 +1100 Subject: [PATCH 02/45] * fix: corrected some implementation mistakes --- CMakeLists.txt | 99 +++++++++++++++++++++++++- src/main/target/AIKONF7/CMakeLists.txt | 1 + 2 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 src/main/target/AIKONF7/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 86c842526d6..db8ec36897b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1 +1,98 @@ -target_stm32f722xe(AIKONF7 SKIP_RELEASES) +cmake_minimum_required(VERSION 3.13...3.18) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +set(MAIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}") +set(MAIN_LIB_DIR "${CMAKE_CURRENT_SOURCE_DIR}/lib") +set(MAIN_UTILS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/utils") +set(MAIN_SRC_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/main") +set(SVD_DIR "${CMAKE_CURRENT_SOURCE_DIR}/dev/svd") +set(DOWNLOADS_DIR "${MAIN_DIR}/downloads") +set(TOOLS_DIR "${MAIN_DIR}/tools") + +option(SITL "SITL build for host system" OFF) + +set(TOOLCHAIN_OPTIONS none arm-none-eabi host) +if (SITL) + if (CMAKE_HOST_APPLE) + set(MACOSX TRUE) + endif() + set(TOOLCHAIN "host" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") +else() + set(TOOLCHAIN "arm-none-eabi" CACHE STRING "Toolchain to use. Available: ${TOOLCHAIN_OPTIONS}") +endif() + +set_property(CACHE TOOLCHAIN PROPERTY STRINGS ${TOOLCHAIN_OPTIONS}) +if("" STREQUAL TOOLCHAIN) + set(TOOLCHAIN none) +endif() +if (NOT ${TOOLCHAIN} IN_LIST TOOLCHAIN_OPTIONS) + message(FATAL_ERROR "Invalid toolchain ${TOOLCHAIN}. Valid options are: ${TOOLCHAIN_OPTIONS}") +endif() + +option(COMPILER_VERSION_CHECK "Ensure the compiler matches the expected version" ON) + +include(GetGitRevisionDescription) +get_git_head_revision(GIT_REFSPEC GIT_SHA1) +string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV) + +# Load settings related functions, so the tests can use them +include(main) +include(settings) + +if(TOOLCHAIN STREQUAL none) + add_subdirectory(src/test) +else() + if (SITL) + include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake") + else() + set(CMAKE_TOOLCHAIN_FILE "${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}.cmake") + include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") + endif() +endif() + +project(INAV VERSION 8.0.0) + +enable_language(ASM) + +if(MACOSX AND SITL) + set(CMAKE_C_STANDARD 11) +else() + set(CMAKE_C_STANDARD 99) +endif() +set(CMAKE_C_EXTENSIONS ON) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_EXTENSIONS ON) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") + set(IS_RELEASE_BUILD ON) +endif() + +set(FIRMWARE_VERSION ${PROJECT_VERSION}) + +option(WARNINGS_AS_ERRORS "Make all warnings into errors") +message("-- toolchain: ${TOOLCHAIN}, WARNINGS_AS_ERRORS: ${WARNINGS_AS_ERRORS}") + +set(COMMON_COMPILE_DEFINITIONS + FC_VERSION_MAJOR=${CMAKE_PROJECT_VERSION_MAJOR} + FC_VERSION_MINOR=${CMAKE_PROJECT_VERSION_MINOR} + FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH} +) + +if (NOT SITL) + include(openocd) + include(svd) +endif() + +include(stm32) +include(at32) +include(sitl) + +add_subdirectory(src) + +collect_targets() + +message("-- Build type: ${CMAKE_BUILD_TYPE}") +include(ci) diff --git a/src/main/target/AIKONF7/CMakeLists.txt b/src/main/target/AIKONF7/CMakeLists.txt new file mode 100644 index 00000000000..86c842526d6 --- /dev/null +++ b/src/main/target/AIKONF7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(AIKONF7 SKIP_RELEASES) From a00ca08263644cef3cfb9523ddf2fc53bee98427 Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 15:49:06 +1100 Subject: [PATCH 03/45] * fix: tweaked weird baro line --- src/main/target/AIKONF7/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h index 4fe95d6c4dc..4ad9da0ac13 100644 --- a/src/main/target/AIKONF7/target.h +++ b/src/main/target/AIKONF7/target.h @@ -95,7 +95,7 @@ #define IMU_MPU6000_ALIGN CW0_DEG // BARO #define USE_BARO -#define USE_BARO_ALL +// #define USE_BARO_ALL #define USE_BARO_SPI_BMP280 #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB2 From 227ccea5cec31f71044b7bfc1e2f1d858b270e5d Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 15:56:50 +1100 Subject: [PATCH 04/45] * fix: disabled baro, not even present on V2 board anyway --- src/main/target/AIKONF7/target.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h index 4ad9da0ac13..350b2142ba6 100644 --- a/src/main/target/AIKONF7/target.h +++ b/src/main/target/AIKONF7/target.h @@ -94,11 +94,11 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define IMU_MPU6000_ALIGN CW0_DEG // BARO -#define USE_BARO +// #define USE_BARO // #define USE_BARO_ALL -#define USE_BARO_SPI_BMP280 -#define BMP280_SPI_BUS BUS_SPI3 -#define BMP280_CS_PIN PB2 +// #define USE_BARO_SPI_BMP280 +// #define BMP280_SPI_BUS BUS_SPI3 +// #define BMP280_CS_PIN PB2 // OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 From 9afb38db35185bf15320eb1dc971e93b9c600abc Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 16:08:17 +1100 Subject: [PATCH 05/45] * fix: maybe misconfigured the baro lets try this way --- src/main/target/AIKONF7/target.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h index 350b2142ba6..bf9079e2dbd 100644 --- a/src/main/target/AIKONF7/target.h +++ b/src/main/target/AIKONF7/target.h @@ -94,11 +94,11 @@ #define MPU6000_SPI_BUS BUS_SPI1 #define IMU_MPU6000_ALIGN CW0_DEG // BARO -// #define USE_BARO -// #define USE_BARO_ALL -// #define USE_BARO_SPI_BMP280 -// #define BMP280_SPI_BUS BUS_SPI3 -// #define BMP280_CS_PIN PB2 +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PB2 // OSD #define USE_MAX7456 #define MAX7456_CS_PIN PB12 From a7458d15ef93593bc7c4440dc52160d4a678b5ad Mon Sep 17 00:00:00 2001 From: Max Bainrot Date: Fri, 29 Dec 2023 16:49:44 +1100 Subject: [PATCH 06/45] * fix: fixed missing serial port --- src/main/target/AIKONF7/target.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/AIKONF7/target.h b/src/main/target/AIKONF7/target.h index bf9079e2dbd..f738d688a88 100644 --- a/src/main/target/AIKONF7/target.h +++ b/src/main/target/AIKONF7/target.h @@ -53,7 +53,7 @@ #define USE_UART5 #define UART5_RX_PIN PD2 #define UART5_TX_PIN PC12 -#define SERIAL_PORT_COUNT 5 +#define SERIAL_PORT_COUNT 6 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_CRSF // SPI From cfa0892f9dca3fa3861a1ffdc193a4fbd297b130 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 4 Aug 2024 11:27:40 +0100 Subject: [PATCH 07/45] wp tracking improvement --- src/main/navigation/navigation_fixedwing.c | 41 +++++++++++----------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 71a7f99fc34..183c5a85269 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -401,33 +401,32 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - // courseVirtualCorrection initially used to determine current position relative to course line for later use int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); - // tracking only active when certain distance and heading conditions are met - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) { - int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog); - - // captureFactor adjusts distance/heading sensitivity balance when closing in on course line. - // Closing distance threashold based on speed and an assumed 1 second response time. - float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f; - - // bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy - // initial courseCorrectionFactor based on distance to course line - float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f); - courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor; + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) { + static float crossTrackErrorRate; + if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) { + static float previousCrossTrackError = 0.0f; + crossTrackErrorRate = 0.5f * (crossTrackErrorRate + (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousTimeMonitoringUpdate)); + previousCrossTrackError = navCrossTrackError; + } - // course heading alignment factor - float courseHeadingFactor = constrainf(courseHeadingError / 18000.0f, 0.0f, 1.0f); - courseHeadingFactor = courseHeadingError < 0 ? -courseHeadingFactor : courseHeadingFactor; + /* Apply basic adjustment to factor up virtualTargetBearing error based on navCrossTrackError */ + float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); + adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f)); - // final courseCorrectionFactor combining distance and heading factors - courseCorrectionFactor = constrainf(courseCorrectionFactor - courseHeadingFactor, -1.0f, 1.0f); + /* Apply additional fine adjustment based on speed of convergence to try and achieve arrival time of around 15s */ + float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f); + float rateFactor = limit; + if (crossTrackErrorRate > 0.0f) { + rateFactor = scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, 30.0f, -limit, limit); + } - // final courseVirtualCorrection value - courseVirtualCorrection = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle) * courseCorrectionFactor; - virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - courseVirtualCorrection); + /* Determine final adjusted virtualTargetBearing */ + uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); + adjustmentFactor = constrainf(adjustmentFactor + rateFactor * SIGN(adjustmentFactor), -angleLimit, angleLimit); + virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } From 42322663f644e1f134ec3ad21a48cbfada3a312f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 4 Aug 2024 11:48:43 +0100 Subject: [PATCH 08/45] Update navigation_fixedwing.c --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 183c5a85269..f762295d8b2 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -420,7 +420,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f); float rateFactor = limit; if (crossTrackErrorRate > 0.0f) { - rateFactor = scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, 30.0f, -limit, limit); + rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0, 30, -limit, limit), -limit, limit); } /* Determine final adjusted virtualTargetBearing */ From c58c958e4916645c7e76fdf2c5fbc1f4c147cb8c Mon Sep 17 00:00:00 2001 From: flywoo Date: Wed, 7 Aug 2024 16:07:34 +0800 Subject: [PATCH 09/45] Add FLYWOOH743PRO Target --- src/main/target/FLYWOOH743PRO/CMakeLists.txt | 1 + src/main/target/FLYWOOH743PRO/config.c | 31 +++ src/main/target/FLYWOOH743PRO/target.c | 48 +++++ src/main/target/FLYWOOH743PRO/target.h | 189 +++++++++++++++++++ 4 files changed, 269 insertions(+) create mode 100644 src/main/target/FLYWOOH743PRO/CMakeLists.txt create mode 100644 src/main/target/FLYWOOH743PRO/config.c create mode 100644 src/main/target/FLYWOOH743PRO/target.c create mode 100644 src/main/target/FLYWOOH743PRO/target.h diff --git a/src/main/target/FLYWOOH743PRO/CMakeLists.txt b/src/main/target/FLYWOOH743PRO/CMakeLists.txt new file mode 100644 index 00000000000..0a1aaea5145 --- /dev/null +++ b/src/main/target/FLYWOOH743PRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(FLYWOOH743PRO) diff --git a/src/main/target/FLYWOOH743PRO/config.c b/src/main/target/FLYWOOH743PRO/config.c new file mode 100644 index 00000000000..0f1fec5a816 --- /dev/null +++ b/src/main/target/FLYWOOH743PRO/config.c @@ -0,0 +1,31 @@ +/* + * 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 "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/FLYWOOH743PRO/target.c b/src/main/target/FLYWOOH743PRO/target.c new file mode 100644 index 00000000000..ef25c2f8eb9 --- /dev/null +++ b/src/main/target/FLYWOOH743PRO/target.c @@ -0,0 +1,48 @@ +/* + * 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" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42605_1_SPI_BUS, ICM42605_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, ICM42605_2_SPI_BUS, ICM42605_2_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 + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOH743PRO/target.h b/src/main/target/FLYWOOH743PRO/target.h new file mode 100644 index 00000000000..47419dc78ff --- /dev/null +++ b/src/main/target/FLYWOOH743PRO/target.h @@ -0,0 +1,189 @@ +/* + * 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 "FWH7" +#define USBD_PRODUCT_STRING "FLYWOOH743PRO" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PA15 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_ICM42605 +// *************** SPI1 IMU0 ICM42605_1 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_1_SPI_BUS BUS_SPI1 +#define ICM42605_1_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM42605_2 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_2_SPI_BUS BUS_SPI4 +#define ICM42605_2_CS_PIN PE11 + +// *************** SPI2 OSD *********************** + #define USE_SPI_DEVICE_2 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 + + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 + +// *************** SPI3 SPARE for external RM3100 *********** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_MAG_RM3100 +#define RM3100_CS_PIN PE2 //CS2 pad +#define RM3100_SPI_BUS BUS_SPI3 + +// *************** I2C /Baro/Mag ********************* +#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 BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#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 + +// *************** UART ***************************** +#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 DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 + +#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 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 // VTX power switcher +#define PINIO2_PIN PD11 // 2xCamera switcher + +// *************** LEDSTRIP ************************ +#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 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 12 +#define USE_DSHOT +#define USE_ESC_SENSOR + From b3da693c5a35f58c12b152f64bb3358c520eb3c0 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 8 Aug 2024 09:06:54 +0100 Subject: [PATCH 10/45] Improvements --- src/main/navigation/navigation_fixedwing.c | 26 ++++++++++++++-------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index f762295d8b2..0c49ad4fda7 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -401,14 +401,21 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); - navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection))); - - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) { + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); + + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) { static float crossTrackErrorRate; - if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) { + static timeUs_t previousCrossTrackErrorUpdateTime; + + if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10)) { static float previousCrossTrackError = 0.0f; - crossTrackErrorRate = 0.5f * (crossTrackErrorRate + (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousTimeMonitoringUpdate)); + crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + previousCrossTrackErrorUpdateTime = currentTimeUs; previousCrossTrackError = navCrossTrackError; } @@ -416,11 +423,12 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f)); - /* Apply additional fine adjustment based on speed of convergence to try and achieve arrival time of around 15s */ - float limit = constrainf(navCrossTrackError / 3.0f, 200.0f, 500.0f); + /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */ + float limit = constrainf((crossTrackErrorRate > 0.0f ? crossTrackErrorRate : 0.0f) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); float rateFactor = limit; if (crossTrackErrorRate > 0.0f) { - rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0, 30, -limit, limit), -limit, limit); + float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f); + rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit); } /* Determine final adjusted virtualTargetBearing */ From 791b251586bd0221e37a3833dfefdd5a557bd378 Mon Sep 17 00:00:00 2001 From: error414 Date: Tue, 6 Aug 2024 20:23:22 +0200 Subject: [PATCH 11/45] adsb enhanced --- docs/ADSB.md | 9 +++++++++ docs/Screenshots/ADSB_TTSC01_settings.png | Bin 0 -> 96151 bytes src/main/fc/fc_msp.c | 4 ++++ src/main/io/adsb.c | 6 ++++++ src/main/io/adsb.h | 2 ++ src/main/io/osd.c | 2 +- src/main/telemetry/mavlink.c | 18 +++++++++++++++++- 7 files changed, 39 insertions(+), 2 deletions(-) create mode 100644 docs/Screenshots/ADSB_TTSC01_settings.png diff --git a/docs/ADSB.md b/docs/ADSB.md index 345af30a97e..933cb264cda 100644 --- a/docs/ADSB.md +++ b/docs/ADSB.md @@ -14,4 +14,13 @@ All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/m * [PINGRX](https://uavionix.com/product/pingrx-pro/) (not tested) * [TT-SC1](https://www.aerobits.pl/product/aero/) (tested) +## TT-SC1 settings +* download software for ADSB TT-SC1 from https://www.aerobits.pl/product/aero/ , file Micro_ADSB_App-vX.XX.X_win_setup.zip and install it +* connect your ADSB to FC, connect both RX and TX pins +* in INAV configurator ports TAB set telemetry MAVLINK, and baudrate 115200 +* go to CLI in inav configurator and set serialpassthrough for port you connected ADSB ```serialpassthrough [PORT_YOU_SELECTED - 1] 115200 rxtx``` and close configurator +* open ADSB program you installed, got to settings and set "telemetry" = MAVLINK, + +PCB board for TT-SC1-B module https://oshwlab.com/error414/adsb-power-board +![TT-SC1 settings](Screenshots/ADSB_TTSC01_settings.png) diff --git a/docs/Screenshots/ADSB_TTSC01_settings.png b/docs/Screenshots/ADSB_TTSC01_settings.png new file mode 100644 index 0000000000000000000000000000000000000000..b98b73c445313543b1018f3747cb5de562bd695d GIT binary patch literal 96151 zcmeFZd0bNK_cmOo!91lloH9+NvN9`&^r$&OWhX5MI$4=1VyRg@=5XZXk|j#4B}x+Z+(^x-E`KlCX6iQzxT z7oxs6K7V5@H8brd4^ilSOSpo@PyyEY};ty5bs7*`o(I6G2OPgs~$-*@(2uRT0p zAY`D?7aCrWXJgxKErjpQi=&^jH6dQHWu(s|A8GjkrZ@`86Pq_~+ZhxN!$?NH2*BnW zJIv~9%I0NPhphRi&*+hX$)9s?S>NW0w5)-it? z1;&Ja;a6mL*jkXIAd^P>eXSxIz!F*4NS~y{HEkWl#q|U?qbNeEqbZ2Xp*|3Kejw*a zg4+7>sCRXm2-xp)mh;meQ=u4-h|6EY@{pZ+>gtxh7qJ#47Wq10DFxcf%x% zk^E5@v}Zz8KmCZjz&^%J@d}NJ_2kp*YPS9D!&&u9mYg6QNDv*_4H9zn?(9=3#k%P^ zrLN=?MO7{81EpR5Z|oYJ4n1EPJ$s_((!>b?(q>1uRduk+DrfIfVy>zv7Dx*DzK?Q953K1O5}cut5$Lkz!94RiOBC7DKbN_@>W zBQMS6yw2hu<*^~b$9H4>V8|EY&r7%Y;;uszY&U?LyJcbd(*f@fTOavZ(!cG+D`kff z&goE>bIo*`mVIsA;nN;N7zv5SvCNI~oG_<}CZa-(=K3db9k#Ezw>}%@PkW7qs!h>w zFf(S#xujSvm^m9WQQ;|U-b_>t4Z7GTAY7KsM?V$>Skgp&c_GbCY`-^D5`lOzKUQbH z`eOgZ02+3n!>pInQ7JDM_9LNR%8;Wc!E%intXZ7lH9u{v$t)j2~|4GJ20RQjsSu6o(=?rI91mNVUp z^WuDxwa+5^ZoUv(d5mpVGfRoG2@N(JSz4vzXm?6RdwG|kK6G)~9v@sEh>JeY^>2II z^AVo!GZElnJ(mJcd%?~am-msb8q^5 zP6L5coNk<8Gp^V&!UF~(Li&D2*41br>vd@RNK~_-XhawP8f?qoZn=GZ`?4jOrWEfu zIPZd2gGp(@6M+mpHG1 zL(gPZ7-icLjvywiLHp9QE(=54E}oj0p5=1(IS z0knn|woX5G3Kul>iAK5F7G2Kd5ioM2tRHn>do8IWM2w1US(Mb8&L&#am zp9z^UJA731^xLGvOwloea>)VCkJWY!7tSmrM>_OnHXT)Qgl*fqiQ-f|Rl=*Y|GrRv z$0pfxjd+}&<3tN8N$w!p2l(&OXNiNkM_qi*n+fqb@hI9@pv>fEd7xcDTA&4shuWF* zkCzDi{Ru2{)VkTt+;Xk0#c_I;$Ob|WGPg)S?X2PYcHQr@V7_$8!keNrEGPgVPHeRBF~N{Gs^);QzQ-DIz+{;Z+*N_<4? zvZ4FJi(jQztJ|}0u;OloM}K{F)gx21OnZQ(mZ^HK&qK|jR&{7c2j)M=ae~@~HYP?T zhmC(aB&OcMYzZ=A2f^M8F05v;$uS$6CELUY5h;%w@!HRm4#k?(K`$JRT&==^{j&5w zUgzQYGPXB(vpz_fx%z_s3ck4}p>#FNc6(#PFKtovI{$I)l1ok#7CAzDvM{?36V!b5 zNJB2hR^waJQgM#_P7tiQ>n10IX+2k*v3r1sR!6`h@qvO(KE@yo(ex@-(`Bx!{d-c6 z{|l1ZY)7o}d&$^Xl)b2H*$@Bm9tTr_VHMA!c!yewR&~gXhiU2lfkMs-KCM>#+VaHC`yd@q3s#A^hP5%{2rEr+sUcqEb@=< z$93uD!FBx9sVYhS!J&=7P-#A7RCj){0DJ)qyFLA+F(w9fJ2j9yf8A}nSMv)!lXKux z%S0atQ9pY1iN+`X^I5OQTpsjq-_TxqPiqPoq0HNMsA{BXt?jxOTfQ~o1JE1ZMf+{I zfp??)IQcK5hj)b7bm%3kNu~Pp}*6-&F{&?+*apn_hA`}C4xmLWCh>5H_`wnF7^{tOL8(WC{&u2YI z_}R%Aet44&0KflvT)&^_4TdVS{`2aspj9k>ruV=6hIiwC`-}CCabrr7{BjjP@w0~UMyX4PvJBJ6jc#bE0dOm6_q4H|!NPopRnQHb|EX>MV zgu!dueU3R?XP>d-eB`3fd|BU~jgfl(6;>~lEyhpB9&dRe#aPg z8p`>i3VdsK0z5?g;d!=<$L)?8d%2M8*qR`&8fuh3< z3#{D_{DIUm*13!PSl^6-Ten(&Y0z2B=O$M0_%L$U{dV!}@)r?(yX*Lmns- z54h|q5cuax-|b+>^}P%-#tutT>k0ShBr6m%ymT^2MGr#z%W`M0{GM#~=b(K)OF|S1 z+kIauawmfrPCfFiNeW9R1w7p)HAAuUmrvcJL4%H5B#yyny8{9@GO{RtxbXkk_z zDF&Veo8wk8%bHI9fI@zRC_0QxF$!;g?YbO;97bJi#^mOK9*xIX+|;V^O{c%v;18DJJ(1%TmEG64oC#PHdb`Z90prz#`YyNp;k#va z#tM#uF?#mld0tXgbgQ!vqPddKa9PeT4!@!wQFO+@@nXZa0CfUWLwL zm+Xi$Kas*d_bz0eyk7c9noX24kL*T&SZCkjxO&q8^2MtC@uIrbjC*y3aa;E)Lh@H(;C+dH!A@ucRWb8(BjgK4PHc zIuOByoD{1Pjeb1}Pm8&hW_r(!gs*`fHMpI~x7I7#U0=gg-1_aXJE&1g`KZmTO0ytS z+*0}3T+R8;sMTy`)?d-IqH8(8QFgMu2`)OclQl8(zK3GxK%IV~!P)|L3e(?JZKmBp zI?Ps3hAc9M)d`{Xe&jCzi2PXC?t_LF-L=>vmpyRb`~&Z z&ytRAv4q1L@h$$mK9a}!VQHaV!c}?G4@vxgFApI6?>z6iYNjT5z;KJjGd07JPFlcj zV$8?m{(YW7$=-4OckIQ~2Wd*U37#WyTd1{GdcTaPPlVLl1{o`QFw`$ed=E{z>AB)Y{7J^J&3A15 zS3Y4sPrXFP1?7i&=$FyzP~5bXaHQ22l+*YmeJ6y+&E!0>T6??uT1>)+L~Jh1bWS*1W`6abhc>MGbW&0P$x`5Bt(%nmb z>RLBsL66Mv4DHyeVY6UWH{ai=igiBV+p<;z9P@U~7V~yD_!j%*&BGq2Pj5s8b(yF^ zqz;GPi!Q95@j@|ATaF}~ZrA+^s|j%1=3j_B;|qh8VrzrOyyNI?9VWsYP)oEBt-tdV zOwl`RqPWRzeVQ>qUR_>eB}}s(Zm#zYCUMD~+Nt-HVf8KFgkWd=jPMV5Hl?+6;n9NF znxKPfmU6{%+jWHE2u_#7ySUi|qg^#J91XrfQs64Mz9zSR|H+;nRo0VXY5&pH>@C}K z+$OKeO9Ileg4O4Y91cLgVEWD zO-e+ICI$K&*HO-q$lokDQWR>rb9X5yC4Rkm!%l0x!pWp>16=82X|>{XT#LLsSSI}{ z%k~%Cf1w^xdUFzeKzJN2p;9L@RpiV55W*}8G#6tj|SBrJVJ z6JQDAycmUq5s^6EC!Mb)8W<7gPPSD{T(DE(Ph^nf2kH3X0R0=7YeNfL3yLFo`+46c zf+$uxplem_w!2m_k;NO`r1?(Pv(KdoqM>>}cKoBquKfYK;|RiBeu4-SEBh*|g+-HF zvge-yM!{XZN0g14?#*bFhKiK;N* z&g0MJAEe*pEmKm>EXUwpw65fH8x%JbgOM9v2ssg{?RkPD{U_uzO=?t}yq+P$>~qJp zqwxLbZhOZyZmDP9*%F+s9*s=j>0h)5i!vM=g1x|MZ>R2aFFqO9c5KSG@7AG*MJ|v6 z$aJ*HEW2!2I%0QJ^hnBcB&6>qeyc>ZbY=9jAP;2HyYX8G`DMRqKchXMiMB})dyQri zA_T>0Mk&iDX_P287qIoJY3TTD_7P*WNZ06i_ubFNbaR+77WZCMc=srZ{nnaUu=;Y1 zcN|$twWj%-xEf=7hovaAD7PI|G7mT3_P2iVCyK)-h@fEEsqom*AnLc$;>1R0fZqDt z*JX`N%+1%*qskTHTk7!pNDc@6b71Uq*FH`a(=5_xZZ~Q}8|UYFH+XC{sGSqa&c9%c zElz?Lg&g00N-^O^+{0LCwpdTD7JR#FWH>gmZbGviVi}f+7mzlLCNRP>)f7>Z!w;KAWT2M{R*=t=P& zv{=l{-erdpJRmUc-yzD};~J%`2w&0=JS!+d+prdfKC|814O;&iy*&r=jJ+bHu@KsF zI>W7v!7bDyPDLNPFi=b(+Eg+`wJ!J8QMn;2`L49M?Ey9Dte#T3yw}JA&08{SKLd%r z6H%11rM=ij{7W7BI1~{*y;L@@PS{rO7T!8W*-4xu&l6u(wHHVV_=6$o-a3C}dq?NR zDi%+b#V@%TFRJJ27zl?V;D9*JNfr)H=yM@2$BzzfcLec=)avs1u765gOSTV-I>f+4-|9V*jn3n(qx$A`xOxp#K9 z$}jk%kU3>d%v(RvY&I-?S{!jN35z?*(g{5RJS}m=P z$tt`1eVyL^^>b-I9LfTVAe74Le|XCNJ%1)N zWX1A$RBU*RO-@?UY;!cX!Q#=%xro7QNrN$b1He_xS`1k1Pl&-ceL(1rGz!~|_yIKn zQHZF!HntN*Jp;Tc1loGt#J2m~H=9_=7wP?kQ?0uo^gu-dtX>ZYFc9O5qBFVBzfb0F# zhzBddiuYq1qcM;0%B&7z1RYg67}n`5=iN1G*uk-g8M8U#^TKr5+jtS~_xPQ>oF@)M zolZa%8WPKv^CzYv2Ge=_P%3P}YV}(=JzfbSb`iP}HQAp7HRh&eLPpXmr_)Ge0!=4k z<;=J=kaM?k{EH568e!emd<`C+_@??AIU}ick*lVKbEgZMy#UZ%Zp}I+e!_w`j3=E$ zb$izda8-WY28;)#hyzC{4ZEyPH&oCbdW^P_;OU$qPd#*DoE2JY)nb#FR5JThU zZtUyX+=$oMU+iLi&*FT{;C`Rx7Ffj4P5=cy861~MMqQz;_1p$MfVv|NW|wQW!=tCZ zIvB&VPsF{uzEuAPUb{U&=B%INMz!V|rI&F+b9=m&2^XwrVzek5oqoDJpn&u$rEVqP zh{&(NUuBh5uV&UZ?9Mmj>T~6UjF^S1Q+A%&WJ7U8Zs+)_+}bg1HmbZ2DD~&lGqr{C zBoKvExNtiHBu2l+%r;AIQ{^#&rcEfod9k>x>&y1>0^uy9>BTQ z1P-QuyPu1-j~6}QretihknUVxMC(r4<)sT>`1a__OLtS$R7#TWz;6aQ)J<&xTM=3S z_`@oXA?lc$a?M|z^Fqr~j-pi?lv!qBwRWP&t=Nk{j*d_>(vo;U48Y}|G=?DLro16; zoW=T*@>_X5^06latrF17P@Q^n*7|m$bRLY7w?o=+uF!k2Sm&#WT#ccM6Xx(x%uC3t zCxL}0-+tyHhUUz2qubZ(h!9G|G{&SHXLW#dCGhzil z_pr$6$!PapEdGn|x4oE4CmW^ZSi&xbLM|-4fvdoD_A3Nf^_F>U>p~JX1(>%Lh^YrF z{H0z!GSLoEB(mB%lKQc5wJ+xru??r-?Dp98RE=G0gUrE4q}9e2Mc3o}rq5UMiWR#; z2SD4>!Z}&tKSui)HsZN1+|Zh!21OKGie3O@s$*`B!gBjx4vT`_RI=9@P5xnnmHdg? z;6B3#YT)hUV(E=4!rOmmhO$s&#X|d|FG4IjGnypGR){IYws!yE%;4C#LK8uZVJcpB z`0^0W&lgsq#IH-_g|9^e8CG&04-Tczr&>?n%SRCU(9_}%$1vb{M_^_GE0 zF;aJ4?hh#I99p`-V}Y&E6!fIo!g8swl$ zNbZdeqGuU{H=@D8@U0#gTl=2Eo3erix&yJP2cb;)aYZ!hR}AcwsK1)ewKAnL%SM_4JDjAYEx^MXj^YE ze49OClC}FCix?p#xjnNdGTH__O2q^6S!&hjjFI-Pj(m%3 z#X=8l6r+Tlp{Y9>6OjY+nciU6-5|A5=e2;MIaSM;K4srt4z-!Ri5JQxDZD|Hb{XJm zfZ6*>UIEL>DkXM%4&&Tlqf{)|BE2`Xv84WG$D4W0uWDM6Df|WGeph2f*VpsBMuc@M zJ)88)vU%KFpGScjnVOXR6JEvYikX>w=4_nIS9~mpS?0pXt|LXB(b@(@E)bj`!+IuV z5zpE^=Cb$&upYADIL1YOsP|hzx4P=UGI;Tbf~0~w$`gDOoeGQ z#V}j{aA0D-vtI7$b=Im`ZP4r?moBuB*O+bD)$$-A>UjAX1Haikr42V?XP5H_qz{Ld z^ZO2qA~zT7-=qD?)3s0G*|!pBSsp3tTeg_(puL7A2t5yrV(eRi@YCh()iie4X#ifl zl&)ey-whYFxfR@q+QrZ9A;5{D1td)Qc)UpcN==(bY0*~eY3)(fUe4o0KCNgk$AvQn z$Q>-LT}Co)tzUR4$U%@P(r5F&0idE-ICN+qSA1o#oh$Zl=jf?`4;Z0zaxipn*wYZ3vF*W0s5pTWRWLLqPEfu^n z#)G~>MR9x1{7b&)#P37mXz;PNA|LJ@6XGX$TGSjwTXqpRVPlF3^5 zYEe&`RjVIuXx~7@bVyN^!T&I5YWIkXxJ(f!;v6m7>!Z_fJdRItao&@I{(3`ceP`dm z4LpYImx*7=dvY{|R|^pi@++@(USwBjx~i}i6`wUMWFdG10doVN3IvtIzvsTo_9V6s z?($Uh+@w1e-tM5@Ze%G?uar^b(+3;wO?&S4u=m`-RZO2neVdmQuur<()z2Y*o88FE z$WTBD=ys~#r7c?H`?9EO(2UhAYkIX)v$J()`3Rq4Wm47{Qwo{O zav|EB&ND+yD-03Svhram(=NC}ld|FO>iVAwaKZ|{BO~gAnx9Uw<iCXCn|tz9y)ZEC;e2^{19{i=nU#?uIIVvJ$7qI_=jrSBxP5GJ)J&3`r*m zh5%w5o${7Xlq>y&G=}(1bUEDf@-TG{K+8qw2L|-m2yG$;VE=AXF4d2eb7R`L;gcB1 zjK9z&F#lYB%TH`k*T=&v_=N?z&>GcRZ>xusk8CP67~5$@ALdHAKnzTw}I|h*?%D0UnO6 z;FAjdT3!iTDX?+w(tl5WSAa1jmcH9^{zGw_QhfFg2rnI0XXW%n1{O1@nY zeP4EJ>}gCM{-~-#Y~lBTU(e(Ui8MJ9gD?l>SsuZ&rLWL)pDQYHB}Z$xz2h%ydw+KA`~B}? zClL@f_9nY^uHa4hWy4rX{98F+h=d>T0D>Onz!gzDys_mM&q<1jNezl?*|oJ|lRhALr{*fAYt=*?lz9+XA(9-#(((yG4@{h%eHjAemxa1#;*!}lfZUWecaL@dV zF(hj45+sv`hCRZA?Y-<<0&?BFP~8mbX?Vqp#+cKmAAp(zc4M3EP}cXL>TUi2LT{8N zZLzSV?gVl6zVjf)B@|qJudF{Dh5L1Gyl23b#%U2%j5L$`X^eZy8}!7wW3WF`j%Da3 zvV=Ib1Juz#(s)eE1{H`_qb7daTgd*&t!V*M3*YX~K&FTB<<9C9npqC5Y!WFJbG7-Q zeb`?dMNN!E31{-4S3}5`D}9QGc2fV3#b1P1L^OpwT}npFAR62m zo0;pN7S8j<&STS^_h{>Q5p-m>jZnIJg~kf{=H>*^jb%!#jVB+K@Sy8IcCWoL$U zNB6=-$9G(!+N&_pG7h7~f-sx%0Y@pcCgYHtLek!+&8YPG3>z2R!>0Ve99+uqH}n1UV{P;Zro5jr_U6P{E{>u9p-< zNCMGA5BorW1ora%e7}=|GD=YOew{YG_acSN@4~3m%z~jyD(d&}@_f^UqSSon34fN; zT>+)HQ?-u&<_c<3UA=7t04FDChH!O{MG-w(A76Oyk z{Hji#_rTs0_)8C;k3=#;JMk%s6wJ{L+Hm^5JynQ%R2F=;Rio z`L-BMefVu}xwM`o-iBMrF7*ma09EF1wVmFXwlAyg#hs9z>(SCOPIOw{H!R|}MrpGM z9)iI>fP!{yk;@_akMOhA5XJG8D)qm(fTxTqLpt&){1nIU+RGo66w4L-e2YL~g2+<4 zzZD=c6Cbm2iua2~1L~(qI#IiMo+y?vpmPl__Vjys-d^@?oS)|Fmh-Lz9Tqtn z)*TT?lX_={kFx7VePWkKvifL>IZS;GHCHjfFih@phuOk{`C#k4Zm&{bVvPV>O=NTy zXVk`_@i7avRijlMw^Z1*jB5|@q5&V~14!|9pfgq7q!xidi62B1u+JCF^YY~_k=olJ z&o1IXZPdsS<+`wM#^X`6k0I1af7f~aBgTlsd?zn!$uf~pHJ=;dvqO5PlYr@U!k3;v6s&4C1BIN*ch~ z)DZFWAn8{nlgm8cvdJM2!Xh-F5$3BM)?!_|IH9A~VPm^HOQ~0T`aWF*dhOcbyP#SU zNaa^G#slQgUf<4N{CoBkF!+C8dOwO z!93MkIzDao5+omVAyP_yYKLkd?}aw(2L!P9`g&v#1q|Vy@cK>~|Ez`##-pg0X@uP) zXjHoCDWgoTA+AZ|0|{2Qpj=lf+1FPQo6Yy+*r~8eoeK9*Z58o>{rHcI1z3)j?Zsrs z{n&TcZ=g_u?qxjQi*hj;pj5lJSOkR^If6Isd1tCSXw)!i0ttiw{V*~#P~QiUfo6pr zZR6^u({*ew1-K9`hs%sKlC_Wt?FE*OHJd)Go13iQLy2>^_5;)QJSmr`o9`d%RYMU! ze#!l{{;HRqNQmzIm97Z#M{$Luw(?TB=S9fOTXexM+}J(rL34-TTytHfR#%@Rk!A<^lac5)_ASa_kY`-}|hvqM#b6GY$peZv#4 ztZ1mVL6vnE`~Y!OOs^D;^i7|LV`P|&BJi*-{EY&SVc zD9bg>0Y!Z5HmMyEGMg^L9aLb&3-N(1Ybo65scWK3^FzG88g&^n?c$I=NLup5=9A;vHv(XKd!SQE&OX6F&6tWP-v;Fn*IRP3KdEMz%Zh8(u&O6rhG~aC+ zGY++%aMS}-3uj~E72cze@ytjXVW6dKB&K?zq0g@L$*eWF0}>Q?VK_78nm<(CgFk>mqKHc(myu+okd$p=>G zlitpx?y;0$?vAFagXZjnxd=+ujZVZ&lZXR~J%t+zzf-{XM&|~mWO5;9oAuEis=7>| zNIh=WD0NiR+U`gKZQpQIYeznNCxeX)ReivrFc_d;6#}CvJr8_P_WY%hw8ylmS#Mx< z3)oCR{M-c@rNopW?Heno2w(w!Qky>D)#eYp*6zz{SC@O7r7~MIQb(!!L~rfss`g z@8?FPA>T+FmZ{Cvf{;hJxN$K( ztWo+dbUxdSq`f@?hJ2_M;p;fgFYVzyYsw?{mnF41XKQUGLI@A=`wavd{9@Y%;v}9A zFBq04sqe6F+wx!9TdB8B#o3nu*M%)le3)Ap3yCq30Y+9f+-r1S=T87lhKSP~{k+)R z))I{}YL1%)RgAY$NEBImRi%PpvnNeF9Tt{;zOK>Xgf zl^xm)%8MzHel)o_AH$aSy#$sOhnarpKpah7vjeF4N3p*gXp|0^stW8$oPfe_aUVyL zT9m5PoSj0kt_(zh#1e(*d2}Z>oB}yAI&My|o?eLBQ4n#9x99uf`a@1$#j4n`@5pr_ z%L9?^xfXC3^tCCM+_=fK(Ps)t`0)dZ;ZdVDU=Ler`-Bu?A{nO@NZ94dY{{Skn)^27 z+>T6cl-m>#0nY$auTmMxGb+4+vU>}Bmxf*~@>mLc6ZSv~=nXC7LvgPDN@tK6&d|7S zH5Md);OHipEEbZ9{-#E1?SrAG2H8(tq1oBlUGns`-#z9J$iLW)whida4Qs?(Nm@Ue zj4Xg!I}3?M*digQQu+naQXZ?YV}%3KvE}UUTvK#zD6#)a5~$0-)nuwyL((j!Bb}W*Fv_r`54UHIR%96)zM zkfhWewW})@TaMQLMeyvVrJExul`fk$Id=wgoX#n%wt@c} zqUbvrbCY=C&)tFDE16dhDCjX7K1p449{!m)D%VL9e3aFoj-mGTPrFe!Nv$|k0A5N= zHCGB`?6ZD(FJ=QA^UQLD31NGag;_~Xl{KXYpoTLE!Z5N5LhMcW>4$|d0RO@Va7be6 z7j#VkeeZT@|BJl0?oCaZ6u1I!VBnbBS@|PrH!2dW(L(>=fd0JokdJ zkhA6}O^*iU+4to0e0=F&M&~cZ-l{m$7&Gd)bAH~tK{U9N7?l7Vn>wdK;fccK zPS1gT#ZmXkvl>e(SMuXt(+|mG$R1?_($8}x!P=)mna*0A${Q6-RHXws4A9?_u!v(W z4-{++;^G?d4dBQjV3Q5^fQ)$uMsMf}s%nVF zc2-1_dOkDCo5LQY4%aPo4o3^x3AiT_T;~=o>ntH1DWE46*2jljWT?vgC7gI3?A1t= zWE^~_OxT3H6B@43HM|Dr#_;Lh=xO&jx%w4(>~o{w=)|X}SN2PCLMf+8eTarn8YH7b z)KSZ3ip5E|nDbvm9Rtac(-j0?y%yc4218sXe^*=c3xoslb%+5;C;N4VUih4JL64ekhM zJ+9K%M!`y2(O}I)5r!@ZORq0P_9FdXm=45Uv>Xb+dM5t2sS>sKBUU6;Ns`W&5YZ+! zd_hy&0(iWMbks~sU+3nR)a$U)1e$k0O|^WaF?_gbB0g{2N*2^pHD-$}xgI9mQTOU< z4>%_x=4cbucVbmIi9J{Wp6#P7Xi#1o3{j8ZLLJKXUJh=IxpW|YNR6!fglT7#&wfC* z$g#ls47`!I{3ISWS79MNNWEj?H9g1=F_lg_WP0@H>VUOhsRUer@s``5vFPG5%%QPK z?nsh|rL%Hd$`NmiG#+_T#{XtybG<(@Ct)>&EVUmf<=e{#6Ff+OvY(38%@C zUZcvuhItNoD(XA5_@i6=*^Q$8AT`xju_3BkI+M~;rt0~mo31$jA73Iz>-u_@C!nZ< z7*pd|V^H!tqu;#2BmSrC!JJbg?%>i?!7XK0sP!4|iMlQPA6d5k^e+ahz5ZNBYQW)gO6lWpvrj( zm&bj!y-CfAa2ci_i5GcdJK;6&=N(Rf0v(+OxT|JYpO-hr%+xu?J^COMeYpx=2wVx@ zbq($ED(WP7U>w}kS5uezU|j1Y71p8I4vl*EV|Rl7REBF$eUUAe<8vq`C?2Ii=9Nv# zDsSNEB55?aS((+O&+&2X6>>o~;qM?IZ3B?<_|(YCZOBUc)hm+ZC!;|b$ft=TvzOV< zt8|9W)Rs-c8gT)TDlK&3fZ%(7%Ae(_^YN%|HFk#U2EAoavsJ1Rq1w@{rGdhx_EEw| z$b;Cm=6WU1h4$%617S}iLts47WE08h{;|vU?IfWi2H_dn;DRUwz0I#IgInbQvGQ@z z;j}Eb(ABJ0)U$2=t>Tzpz~nOcf&BxI&*R;T$4vu_{QFts(p+QaeNKnv*lyXSx_3ZP z#_TM#N8QN{6nq!z4+^g5R$kQ%A@Lp;B%ly|XjaS#VKD-=b=5+LDXU4Ky`K6Mitt&% z|NMHWVlwSMceAHsw)_hml>Zgc85MueYPbJU{C1=Q-+}UdVS1j213s36UgHqkb8Wj|-S_I<`0v91aJ(qF+0J1f$d!2W z1PFi$9XjpmcP>sy_ie*FMi{5LJ6rP_B1iuR$W*j52jP@|9B9YF0imeDXM1DJ_@wbN`bw0r=Z)pRMR$o7EOS!sf%SNeZm~ zuQyqjpNs)xLKbU=zkj}sBMr3rFSL96Y$Qw;`pj^#GZUa6XKQfs%?JP5Pxzs^vpdc0 z_+p>l$-G8s)sxY=PAKq^0}HgF0V5rw02o;xtoOu)c((bKzzeHwq@GKoRDOJjBv7Vq zR~rvdPkTibe>T-*`?q!XQm3m-zF0FV%<8oU*8cTm6`ye8`QwKc8+?u%q7A z+h^%G@v{8i_i<*P0ifaTF7IFM4ywGIDtk*JWp$>=s}q7roY_`=f-Z7_?K2Tk==N4o z)f%E1htWn~Qx$xSKN;Y06~@vUk#w6oe5^F%P#+K|ysCST-N5VGt-b6g-17&aROT8^ z6@ITLSL0_&(-K+kwL4U4i+^})-nHx&jS|AO0NLo0R$Bx6Ik+Hs z4r8>g-F|+HK0iEB5{a`~|J{UF;ct)4shb~K1hx7N-gHqxWb_Bb=B;_3to-_(!FI{T zVV1F7p@%HdIK&3;<_ST~vy~5qF# zmu|IY?GUFolp}NK^50K{jx0=|+RK9i6yA%mN98^9C5)NhVrXt1#`y24;XeXowdvcXT!gSoke@> z^uTt<)OKa>4UhSIncQuGK0N7ON-&JVC5`#AHeW+*K=<0RZw9l)p=r8%Jj)1lc!aws zy&j#bYL8pL6dfQt@qlunDBPUy~-hWNw)5b;Osrt&G2yS5=3Z>t&a8nsEeYD5& ziihW$pR?Bq!N|H0b{(Z~Et}G9F}W;Q%;$3qW0uX)E@1Ix&fYAyx8JAAbMp@~{Q3mr zI5S64PDzQR3pU znfZ9|D@u{}Q}J$H!H(54Y{qN^vQ{Mvp(UPYkz>>f;K;Y5>beRDE0R@+jx-Ka(eq`29p?uj4_9P4T0bz z^z_Tip*YD%)m_FxW@gK?^T#e;lXDq*5kC0dc^JJ1ypS1c%P1s#@D*x{f^>mya?TF) zYs}2Ixsz!eP}nn3C@ea1XGa+3y&T2a?Fwae>+cVbmRp4eDOYcWt2v&BrYDC3Pax+! z91*TCyG{edXa$8}UrUrd$zMJ<`00&hAlUADk~x|@$tto5e-FD0SlpYpzufF8(+KuR zDFVAU5Is&@-<=6u$kNqI525Kxe>)ldVbm7nYP*JBoxW29wYOjPjW?Ma{LmHIP{t2Q z8$rv|53|4Q0DUBqD7XIkV|oR~@!i&KdRL86j=qiOZY7!@0_D037O|v$Y{K6JY7hI# zR3?iO{CwR7uZc~!<-nRLVcm7q`q~4%u{Q`d2_%BIG?US4ojo*AXtamDBkKd36rl+{ z+Ty**XXdfV7Z!;?gzPODgb^{+9Rd37hnxEQPui5z7Z)HZHAP}PUu)L)Lt!smp|IDi zzm82B#rdUjS-n!W)_vb(zvC11CUY>JO~KI^$}JsETU)Fl@>|5LcPbIj@C$luW?XJu z>F=-909($xTNdfXn}&!Mg~}HV!-8%rq%kuZ(A~vVkqBvrflK9x2>1BB_=$H8WtEaX zF%CxGKOk`uHBm1`g*wmKjnu#7r^hD@5szBr&Dx(H8D@3p1+zeLRa_)wQ>}0Wc)h=0 zEfwifw<*g%{oTj~1DvY9ICQ)4O5+2lDd(q8v+MNYT2Hcs*@PkXF>Y~pAEe%69E}L- z&PKpXYj^tLVd8bR#pM+ZJ}$mX@(%v#?MLMCB_m$(NGGUu>xBZrq~?{c{pTyeA6P;% z(Fj*l(ve@ELsG7372rpckw0pi`~ zA@f3~H(b=}ybI^LU|dy$2Np1x%&!X^#v>7BOb4JmE{#SVOFLN z@X8Qo8hplq=r3sj$kM{_*Hb(JE$d6tB zO5jonJRF#+_=zOjd=QR+zi)8mu8yy$=u#-iN}yiY3gnaZbg;L-Z;bJ)Am-~JfEAd! zId)uF_pEVkKsB+{x-B446gppH_2tMI*2Mp|35={W23o8>doTa^T>J8b$O?=$l_(K% z;H2nPM>kRZ;$8SJpfxD(Qo!=(lfT3PXu}9k$)G1W+w6Yt_QUlF&F!?gqTX7i-e{Y3 zvZgA0jw-eJIL8H;Iqv-7=Gpn6a!<0sKrX{ZS)Gz_;fVS{**!7X;%=&HRoP(EZy5EI zP~!C5^U3lYe)Ex{#IW)0I+sVzQD)ytOf<@v|L_SV-7)CQEwDNDjd}X61`l;#$%BX> zwf=~>E?GkeHfCkosB)U7LpkqDVug}!o02*nV&%TP)WNpwyA**`O)C~hZLu`vAZU`u z`5zAS-w4-VKtbBVlkyRfPjBmC$HetdL8Y?e-XbBP>jV8Z4+@%WP84f=9m$&MVDal; z#J`MAlY>1?R_{bCL#`X?!L4CCQy0$lXdC`BS|d6CHco!M)HjvQ!eT*fhh#ejL1H;{!`WV8~Inq0V6-<-c zjPx>nSBMxZt=(Y_fQhR*7awOC(Qb6mPjg1CNk4J_pp|oB80PuZ_iU1Gh-E5FDy*NT zY0c<^nc+6=%qoD1Y%ROxshQ3^f*-zhy?3o*w{O3*Ji;pr^$N6}P7p5R$jo<2=GU{z zj(K_-gB|NE0?WA${!vcnVH;E-scHb;?J44F9HSwxRw*@*s*un?-9|N$AH&6 zjzB)ft@c{)pXn$_8v5j0!zO{Nv!$vGXQJols|v&z_Wz5x1%&1K(X0W&+~npPg<28q z?+bRf@i?!g@A^ZRxj$!c<~Qe-Me_@xE1C-VAXypa#|^xEPsXwNT9eu*u) znkMhcZxQSM>FGX(%hlhD8V(Nsy!h@$ET=Jcj1B~4(}E@sTlest&&%DYRoFc8HgvUO z-^@fkI>kXA6i$FiQhQB$Kmpu1=4c7Wa;|nSOB5JGOD+u@|MYB&ch{|iOUu|fSFeRZ zaKJx5y6B!Cw#5I<214)A=^lqWdbLyuS{*K0E-qoD zVWQ0^nor#u4i(eXN41$69Sl3l2$Lc$(0@2O$i)%V%emV}y5jV1pLYrNbt+E1x8ila zC^CR`3K+m&ysH)yAI$253j}6*pOL1MNfrC1K{v*(UA8PN)9fYn{h>(2AK?94L3bg= z*4L?^M!mI2zvL6VHX~-`pBE4RoK4y6NS11b<4oWRylgW%M>}Vv>~V_@51^Yd`3T-J z2t$-XR?p7$KuRVKv5XjwwCeSdtw-;hg4UyDTRX1%>Vc)vPMY}C2j!p<_2sg=zA*&>4sYRyjxN!D%e2~Y&_FePdyIg06XN$KR zYHhputLJi3t=5*t!>>&hn>Umyddz-37Iu*1hyNxStASob;jI7}KrOJMg~T^kLIR4a zr&FHp-g|Hf!Cgxv(5UZrs|uylYNlCYP!Czp36tp zs`vMc<)PN44~Q`0?dN7Db>}Kw#vrtXvaaHNd6CC~-vDKO-%a&1mnF=DoK-hhwR7PC zD>oXWRksGWG3DMwlQ9*4%39!Rw?=J^H!&ey$$MRud8EpPI$G(Jy6ygE%&mXSVSUdD zso||JLhx1WP$cXhXyWRnLls)YAsaYZ<8$gg;xODsz06?@PWdBcWmxWm16H&SewTjW z_IBo+R$uKk860tTKJ`Hb5nv#1YOjMiV_x~KtlrhHP3p=?Ge+Kb9S4Cb+pp$(O$zNl9p9EWhX?Jq*T#vksB->SFb$dtI%VQeleaCqA(j3k(( z2b%t9Pf36mi@c{k;Aof`&x>^MeH14;-|iT&rJ(tI>~Un+N`BxLkOW^rmET|tS9+he z7Og853&Hq^yQp_!ANHXs3SdmnVH-q(mtmL1#~S&P!YrB&O*lbWO_bugPPC`G)DPFA z!Q@>gn&U!I<^~90&q&#HYDy381h9;MN)~xWk$>?7%<|Q#c2>YBX$mW`aGnbeU|85W z8UgC*E_DRz+|-fIk*sN%w60_ABOow}?g7+(cX|L-0v5V7ch0-L}HbFVy?FrtAqG$p17H_TIU|HtRgM%r`6Y!y$y z`6T;GPuC3C2#9UFAg6_af{|r*(lzA7kg&~FI6P7 zAva~>PmrVCAj8Mmo?32VDpZ+!;QPMm1N-Wgk4Y_G#?9Iw|d)KrI>fRyqd!qxuQ((F?AZB~=v!U%-?i&; z`{c29Jv~G1K|Q^jQvMKn8W`H5gZp-VTqif4*Jds zguH2)%e?ZB-bfvb4x>)@W@}XG}y;r*9LMMW&@;0O`PrX3p;d^r;LYA zFzqropNp0?W9q?1;U9b4cz=iE@8xgb$}U5HzoO^AHcBa#BRj`!aFlQsFK z>i0a4ZPXBJg^6eqm^-G7Y{}V;1(0t0g79Hetgry2fA;qFR^<8qQjnu1%e%fcty-kw z{E0GlWX@GXW9lz;E6Gy?0jB}VBd@@)qLnz(3xN>u{vb!*s8uEG?yJ2FRIc_4K}frz z?&~UVN(|*;Ii`D6C8e7>Ck;zRk`M?+;&ql$%=pVbY`GE{ieymG^ww>i{1u=Klp(i1 z{&ho~ZP%{s{S3s;SSdW3A=1dZCNiwHz3)N9#ohYv;0eEM8WK|fmtC-L`XAahpCZCY z3|CL`5-$RM+fvU+UO_qd(|G=WY9WsZp$^7+YI^^6G+cG$M6EL&ILDxnjl0;fs%je^ z{_Fu)+gx8$`K|Pn*7uRl6EBVn2PBFZP0O|PpSzW?Rtt_5#8yRXq9*7;n#P~M7Joul z@4NANUjGG&WI2}%Wx$oP^67wB%S*A^^J19Q*O&B*FP{)KKF<&*`kxhAcj?SJy{5M7 zY!zbNtHQ&uNH5-Z^Ygh62wR$Z+cwahfRh~{e2)k&aZR4E-8$Bdzo>dGdcXDy>7VD2 z_;;gf#&;|i7`Ls`_^y>Ee-E6Y%218B%U+=yMbjPGpxb=k2rE?3pYow~`uocG{cd#n zL8D3y%7;qq70e0j<}bknKsH29ulhfZ;peZv6izBC4qg?1PgE$Oj0dRvR!1inBDy+s zkaz!a?$P0Uu+ASC2B!Z9AQ;%ZM^v%?k)cvj8Y%p5gfvj~`=f3is^8$}_$I56?Os+H zVjWXv%w7XfB&Px0y<%>TZP>pC`tQnqc5f0a9DTH7=&o_M6QQCF!YQ}9o{uZ4zL)DA zbug>b3>_Njzkzw@`&rzeQpm*gt{!R0+n&uPUBzwv$gcnTaNzaWJW_99UAsL)2ve`U z(>z4-?$-1nnP1-f`Sz)I#Cb;`d5I4W8VUMKhZ7YpNQ`d2GclpvSYElJLMRWgGTe~% ze3+~V|JSVGL86|DFwXpGd_lb+f^jp0H6Sgvs2yg-eqgAQMRn)#ZNXf?D- zg!7y1y-LFOYZYRtEb)Y?CGvNq!otb)jIlK82jmye4ph_us(WGCe5)5*+KeSvLfhQB zii4-Jd*`}~+t_!%QtVI#BI8y<*Pm~*|K0PECw{0$qwch&{Xy^OrRMc@SBf(n#ED3$ z*x&jc$589vjHmYhrdgHGw=di3r|uB{rn0zhKEg5$nD26=@Sc47`T3hcQ`svnwbmUH zKibSy1Rd|xm>zDeQFF6Ie0Y?3$*{bIc%Tn*g~0pZc2MS=YG3Wu-XdG7%rNX#J_3Hff-VK!gnP0xragzF zle`_V>@!zB;`ayjik!@Mj2e-I>YaK4&?T4j#VaT>*%j?kq(aI+Dk1le5fDMLlPKjY z1grNdekNp%4@TA-U?|Y&Fj`LQ5(2;r2^h)oS{2RjyhX8j3%}w&77ucDb?fn)>N^qJ zeY#D1qM?$uP za&QN4)!~Npj## zUeH|8kyf=YJLDtI6gz;BldyoCy25i>3qHn!KnzgNjILj5t57uYFk1CPdn#a`C#8Bd z$RH7!h$HPYV*k|^P{6@UX#MmSGLaU5aU+zpPR0wc+B=ycIB$a3mFtH~EiH!>&2+RK z;0b>TY9?I<(8bSBQrO>i%>W7&0Z7Y@gO%Nna7-i+Fraul9ZcVFcRHvVC@1MnjH5@H zlN|sBT4!11KUJbVa=1qojKHkso~AfF^}p&Aqx|z|f>qB5c(e+x&rGE1|KH~L|Mk!2 zVT1p9F@5-sTF=h|Ha@H7C~hW@7~d^D8%%SJzJ2BDrEtq;t&H_714L;`zyNc(%E!+;qNzJ=Fhp5)6ej7%Y7Nrn(OIjHZ)?Od?4Q2wd>jUvM$Ii zd@ek2esv;R_09)?dYC%J5r@h&AnUUNW1}_RZAfTMKt)U~jP6xekF0B0O)WsG9$Dxn z_vUH=ifm%_49`E2^Ht5P9amU6AMT-)6twb1X!G9ka5P{aAquJ2rm_A7Dp>%omOPbqm_+6iV@!HeB}^y8O!M6$Q1jy($}7w(&ejvQ`- z9&NArdF_kE#O%DgdpsgC8nS-=|NJiKoo)p!WjTpI?P1BsD-zg#pfz;&9_eL%x zBc!vJdjXm=;y{F@`Hy;8EYJP@`#ZvN2PCf|c`@@b@jbN${u~BJk$&3&0X2nfz=QZu|iXsM8nqz~llrt+eZ0?a_{7nkh233=O!Q#rq=SC(b=%qSF_iqpMJo*8ZO;0-O9XB=SJN&-TkAZF}`fEo7M^@2y0KjjuzFE<|YE zz{$|B3Taw*Cf(RbwOz?KszqI@4Hh~&d4qxSV{A=%KEo9}2b*p}ei^}P5CQAAM@ii4BBJ)-zv6G>$bqQLVPGMQ@v@P_FW&f7Ljo@J8nmn$?#_i>mT?Un*6*}F&^gWp&M+B z91S+(F$TJ#t|tJDcB4IDyPWAvj{*LK2nO1n&_DqsZit)mrpztWv}C2qwj~r!WY=)_ ziJ?SU7CVBtI#wxO;;Nt_#4+aJ+)3!kh!4b-7WJWmtfiAR`0JYK(R+5LgSqtd#EY_~ zmFQX1T_1>0gCf2)VYO`16{mO4>`wYj&D7P^3#(pPKW%aQSx^&fr4totLNkw9*$Q7v z484lC^$Mkq)g#>=b4YC4T8sN_MMzD)*DVZo8+YY7phh!O?UGuI_vTsT;FeI$j6OXt zk}BCvF2tY$`Bm+@`3tBwlc^aQbuclok+D0*+ujmownfI8YB7;e9Xt6MeW)Hy;YfLe zTNSb&d&}M4+(7eD&6dCw<$((r`r;4P2Y3Re8-XXTiaku@kVU5p^#MK+E$i~Sr$8U9 z_H}nib!hKUr$$%9M$Cfv)ZyxH4QY$+Jbis@)bq))!&U42JE`FkE=0tmR;-ZMrb&_awOH+r@Yd(xYCB9j4$C*YtOq)S` z1~x3Oa7a~P_RV3Y=qR2u%;o#l2VBGj@VC#yS%{YSRD&r(-a-RNJ(pjG8q5@%yU&OF z`d{>SF;xX5eI~yCqw2oagC%ZDTO#OEmo}8EU=rJ>wQJ**y`EeGL{Q$2+zA(kuQL+g zsjHyB2u}INeqL$!a@wC0fOXabGy@54{P|>i!-M{Ek84l5Rt~BBywCLvHJGPzj?~rY zzT}Atqr>eX2lN_rJZ0Q;Y9tDd>{wD$EruW|Up%mvTSSDS)CzBcNW4{%wTBd$1sWHN z-~{c}#Z4dmtS%% zJvtVc14wBXnZrgt6_1Wk2Scb-3PukX_`&9IJDi7=;47;FY*)|kw(I)QcI@?RcaMOS zVx?T5^Rua_!W_l-QmzHk&5!mpY_WK`z^3P>q|&r1?ofF!o!!{3NdXe?Q@Xa zX858>QJZ32T=!PBam}83wt4mmdMa6T%g#^i*$dY4l0g19lY!LKNTzV|`PAA#?A{ZH)B zcFXcXVa`Z52HPcw)t@tXvP|(tW6#;~F?mA`;@7$beTDoQOrOdV=q>jndPx+=&#yp&1J>*;l=Aof37_x-Q) z3IgXvuDcN!zNlqccO6NBs|LMO4c@X=vI2Ym@Yu%MqDb~N(!=_2T4IMSU38_TX{mZ; zq9_(`f;+7o1o7h0_y7AefuR#xh3Fr$Js$NKdj_%`Ks)n(>^=SVDZz#l&ey@+Dj70G znRIvT&1RonMboF+n*qPhA9Wnh;dU4Ok5($>mI{ISnnvaSaP*SOmOQ)Oc>OQ6@{cw7zhCH z-^vz$&nl=9US)hd&Cbqcu#=%X?PTa~z&!8TAB7mtM{!o?kDk)~k6^|Egh%}RCt`lr zRwnSMW2gV;IsS}b8t*dVm`nV>%hEm&G@X}6h9c(eC-c25_0J_W-v=Go5}1Di{m;3- z`<^R|5U{!bkq$J{xZn98;{Tsm?B4^b$PD@S{!0)1P8?Ra6NiZ&Zr^>m-u0If*#kQ4 zU98U;kW!xV=4$Uoz~5KF9OFPjs{UGLRCG53cD{KCL~uiQwV@Y|cck2qs(5=p-~JP; z?>&+DoT#VqIKxyBT0Hk&?7;Y4L^zD>DoDtG@#4F&J&Yh>pZ^X?^wgye16VLx=q@jQ}V4^lHUqH?JMr#zN4#$nrC5U zGuA#H`a(ij^OHVw4(FWyp*p7#!uK2{oX?NtjzhQ>w2f~~K7!1(uKCTp7LIXWrXZfD zmznB`Hn&ctsoCcjys9yH_Q0=g8gsw2)|5}CU+2Ev*w{^#?Z=Zd{Iu#D)kx!I)uQ9i zFB?WXh|Szt?{a+&-tS3;R3=%3L&zk}|lf%wiuV@Ov==f*Wo{neK46rhcZZbtAA-939jKV@yy=^@N95|``~OZEz%YjH zbv*M|uc+J+&3ul1?!C`mCpX+wI(z$c+aa@~R_}#)E(*se2E&92mvzb-T+BADKboH=GtBIbCw=(OnZ zj#vSLzh>Hbm4d0oXu>OkiP{J#OVfox@h@`&a32Zu<7}Rr@o@bgl0Lp4kc=cxFq4LJ zb78)MNv3syfV+7FL`W4$3_Kf5)9+M{EY4Vp^K#r<*dN#7cDB7DWYFib9T%$uS^tYu z@}Jr@Tc@wbuqE<+J?OBKy|9a%>a%wl^8E3qEmW)@uSG5=-6Ay z$Iy63BvsUr1o-TD;Y~O&;tX${XD4L)er7Sq&YHHY-zaaT^f(OPq&3>lBGYbKcT8x-?!$$6j-DzVW{wgx-&hw%Pf-PrJ3OH0tg0H0 zR<+z2WI!RAx~mDpbh#nKU|Fk%(`pDwr;h=FSQX$cnQNBFL`2vVMmiJiMM%1 z^09pwxoQbD748PVPm<$X_R%+ZztpbsawfZWZuOv^Dd6toeUkMB(zo63dc)6kTQaUHyWIVK-d*_E->J0~%e2g^4#_`9 zUR|)d{Yvl+Yl)im%q)qCWB&P`uE3A*BthtnMXsld)95VE^^sMM$t^N7HB<52w}S&V zXjOx*<_45^=PnuE(@(sf8b`&qDKzJGUzo*|c1|jE(5=&N?eD~YrE>))e*`sK&&hzS z9Ncc$7}_Df!Rbyt1nvRUD703iUTS0l8)qs|4DQd#rz8W4epMras%fo7<3*ZB3Qrq2 z(08SqEC$E@2hg&?VgAInrQ=r`^toDuLv^m#WZs$*0FetN&PzqG6<{v=rYA%>FF6%* z>IN{WberYGBM3lSs-=nbFqExW&0_9!1b-=DP?voNYXT6ukM)$P$SIoE^O ztpg34Svz5!tL%EkA@!-NMo;UViBLVQN)`22s7Q3!1g>(o;jNbs zH8en!)T=~I$T;5YLv1%yZFsu$C96YuW*0HQyRBC1TfN`nyOE&CZ$6GTd8J)D4*11h z29$J$Q7?Am2RDP6XY9M-*c)0s_tyZE-JipUZC)*$RZ}S;Ej4(!DEKxF)SAEHIxS_G z8zdV4<%~{8OQ`lY&Cj#Y)#hM`F!v=NrmNekHN8XPmq(uae)Mv_T4IBckWjx_tpY} zW?XD9)4byT+KKSVSzooA#YjFs)8y9@wpCxU0}i+AQQp6z2qb7|WYDkpXRq42Mnn{Y zRb3mawokr+E$PfpA!%nH~##MbEdI_OiY2n%RuW zFPV_(?h3kxW(owb6J1HvXuR_HL1N)MiuPji*t^y2|U zv`KN7IJol)%WPVDee_gcESkcZZcx8oaK1VYT&}H~rU^P>Zka~YK7|_eb=jp*&uU1j zTH+kgp^-X4H~op|6rDR{&T`Nn$}9akk=03v;J5lJ%U^SqvyYd(y(WU0^l*ryJ@w4y zd5+bU6GKNXq!vis7c`zZINiIz`;>qKM!?c%#*CS~uh;SAf|fWNxnmX5Q%aSl$#0)M zIsat#0i`$F1(DDbIJnYB)zIjzvS;s{HP87Fy@#|c>E9u}=3w=pS-0mCJw0ceu%O^u z`k>PB0-ZLc?0+W#qxjUXgSBK+=Jdd*%d0!%>s}3^H^f+#w1Y_;SKdH;Kb-FEiG%Wl zV(TkgGme*z1!98z3Kc`#hw;#|1c&6b1WfbUAw*t>X*Vd|R)nCP)5w;S zt;Mu%kXsUMo~V^J(tLrcaB5lAz^|%AEr9zul6n`5_c|ILhWC8x7a5gS^x7J?$u{5T zCmZr=cbJdWp6$&RNQ^ml#hR%@3gce~B-d{sB}f**dmyu}ho-?^dn zjs9;D;C*109yZ)jl3^EI=gT+X-CJE!`h>%MyL`iDUcKQ>PVFm6%v_E>~6M zJ*>0E)O+s17vPuhhZ5Z}s*3p|I-;%+6Ih9R6en&XV7aTS#05n4erzHB-{KrUszk~=q!wQ z4Q`zw+6~qtQJ-k-_2@9(rZCTA76ZG8X~S~V`I0-=y6QdFcG9s|?DW}?b*uC%OU{Y?Bylp{O*k)t zY0<GqF1Sq6m_YBk0oh@&a0YFYvf6w38xv`K{R|TTFcc%v9Jhfgj-^E>Ich&O65{ye(lPD)tbWFjVUoReYu&L1*{ zIX`3M_5jN{*aaNhbuHA~0yI)L>u-EehP^$E)u#8AV3N1(C(2cp@#ItW&cU=vS*5F5 zCkr$hHf@IhlvFtGvC3HxO|Xv+tZMZEd`^VTebbq^Zg!H$aNANmoaqly>5G6j`nvo& zCNY$RAjB+Xst5rmn;&Z)5LwfXixLhuTng_#U{i05WR zU}1LG1J*vvk&QDZT^Cqt;|=4Xg-`rFQlohr{qcp}vr_97%M=OoDsT{a3l*A&>I1}? z#-1pI?wvu|w(1FC9mL_y;j})#_I3T_w6B)sF^3v{WPRhd6^D}zhweuC#zyb@`~=Zec$bmLUshlw@D&25 zWlWDc#G*J0|AxE)S{K061V))EM+Bx#6C6h7@sr2MbHSe2^?+bsrANG6L7@RDZj{Pd zzns#rbsIl;LXUq6wOQWfR#vQR5>;O+KdXFR;Y z8>bugViLA{q3|6wE{C^WX!r7)$~3qHoTaVd-jcT9jqF;}EkA!Nbe;BN@5KvUNzqs- z1H1%&!n!omGwHjkaXh;Se4K+MvB3^*APLJF^*oF5F8I>$E@V4mV%L_I-oMP~Cuz|Oqr?%|Mb)JEkG?UuH^#KkF)3aIDbs4diU9mQe z3?q2C1jZ+Nam{dcI7y1(#(dQSELJg5 zfm#X6tQt+E2w&SrIa-Nx>;4z?UqM8oW7s2-#P;*oZ>-*vw6CC!?F*7f>VYH$Lq^MH z3eC2pV1$d7*rOxZL(`kq{wUvugI{be(i^v8E<>y4O4=daie-tBTGlSD$S;-v`|HEY zvNmJZ-2<8ew&d)Kh2^Dg`7HP^g$kUOAt5fI26!CtEm(}jPtN%4-h5S zWn*%Dg2G7&$MyiVzp|^&bPcknw?DN}Tvi7baM8c;)4hj9bwP(wseb-}RX5+u-VO7PaZ<^BIn-4LMkr`%Wuh-!%w)GKEF;i^(^uu4r6PS3LyCO5#9834Dm=}p67oF|B^AI% zac3sgw`^@@>wG?J@YZ~z-F@nDt+qwhETe$^=G1So0i&S&=EHwjjOpX;{`mIu-VW*e zMsG9wD+g4ySq&$AQ-vr(I67-uXSWnRG5*^CPcC!O292v{RFZQ_T?}n6^BfpUmE>H| z$XRkRoRwaJ@3`{+-AH15#93%<&}htw}w3ZsAL_{1n#-~NCyIbUhnf1TWN z({8K)HrknOh!^iNcNUB9A7A(5qkHxb&S>Y-)7n%n_4yl4H!J2WHm@&~+1(GeHt#)%e0TM4oE4B~gtW?XD*^k9fIm#Vl>n*;@KFxaSgS<1T(QRzE3t*-fmW`{M*orz_ zrF|$A2e?a8`jkFv05s62kk7Ud`8mh%qRb$5UZU?o^f8C-+$)Qj_oXCPuI*My9EL5Q z3A3DfAb3(N&m9sHVXw3XZS zrhy`dnv(aT@z0i&M!~;p@0EaTeF$7R0oe18fZNIP?^mUoZGp>BR_TA<7H3RkdB2B+ zGbZ?64ztbfpRRD}IW!sRb;bn@(BD1jc4+!&rfY>#k`@!Jf~7d!JPUR@FQHZ1bDHa+ zjcS5M2vjdFI4Y?qFCiwLP%>eDtok&g^280J8`(Nm_TN7Fl#;3hp?%Byes`T_1RLKiYCn zf+t_%W)>+jGeW;@)M%i&nH)J9uO0iQLv=a|(MDgo{^YVNj_@e()Hu8CO*1rQn$ zc1J^7;Y2;CDatziS3RMDgtuvbG5)E*U_|J1#oPYb)a^ZI@FmKe3#hlp6qB&jqVB~^ zjYvHtChA*Jo6MA3filtE@Wqn;vKhLvxz#YbOp}ly;qsSHufC^>Wvy?Kok^CQ?WFxg zRb^L_3(hmm5tpWrRb8i+=RIC}Qc_C?R>xD5EiHI=z zpujSHQu)V*!1#PtXB>T|4pi}|I_&M1S+~UhOW6c;1v!MvLE1Vis7-jFy9*tc(ufeV{d zH@SI--B%z)%zin!vkuO$?i5DFjJIJXPLegOLsuO}uc%)V{_5K~E97KY@#>)lTf8+L z;@bIED$A71PDkMeCBU&x0_R(9cTS{$x;UHJW>o~cpW__l&z4+1W9eE~k>^QHD%ao- zbKoziE-5RYQ!#)H&!lfexLa)x)RTEt0Q33ghxe-b=bAJXs2}f020W)Kw9+e6VXFC` zYsrnkIe(Ommo&$k(9Rm~VXePJYvW6|UK6gdazTx0IJhJnJ+ydM?L?UM!_8}lREp!N z-fd7c`-H23=q^=VqrpsVDlID>I%r-oC8?Ow0nD7F zm-~;DX3vo>)r`7M#js8Onc}Y_CwP>MZ8R5h$^Ih*rwer78O&Ej;_`S&s*KXjy&|jg z%k%QX_nM~@WAOx%G~L_RBhz?fz~70GcsDu^Thv@WPh7Dr%_uZ#_Me$0xFmXlixdxP zCPGB~+sXmrUA~EJZedCP98#6teS>vcsU>`hmzmfRQQMPmovn$R!RTFi;%61Q+W&Y& z$;*{_*>yihJM$AC>zu>Jb;Vyxs#f!D#RPN*oBLn{5=#S_$NVvofUWj)Nw)ap<3bCa zgTJHoJX;*;MmV_K)%!r4j60*Mi!@2*n^_=oJJ|6w8@gwCEIv5bMh#rp#C31dTjQcB zzSvgV=H9DbZU+^+Gpc=K}J>AKsHb$R3X1dx~; zxCLfCqk;s`aQcJcQz~#44lXf94}F4!7JHuwO%?^xELcX}>-SH)D~=iOrskY(?@FlS z4t)OrvXZ$l=#ZfG%3RDzq_o&5S>$Bk7bQ8p>!uGTk1+ly=vtd&>k(X?Xcc_LJ2XJ*Y>hCYhG#?pXaFo zI^EwIHM{j?B56sP2Q^a~bd!UqyqC>n(k1%o-TnK-*oChMZ;PK=BfDu8!>;T`9YyYUjX|niKIl1lBxBlW)K#X6t-eI z^iEHS{j-Ai^RwGEV)wq56md7OX1XkJ@p?#=f^9O z@2Zr#6p!80;Erc$+;reAGF6msHG=xeK#JLLt(KRCP}v$3DfBQ?a(wxsDIukpcWlv(J?rK|y6f-a*@1RylQA*3|uW zL!~w!I^WEc1pQD(;eBVD^i_^}r9M*74gBOxEIT6RQiy%D{uubU>smxe`D#_R(pXm5 z!I%sj@wsC?$V1uUWJN+Ir16j|UNq*L;6Cjxg}du_hT@fLkwGE{7cdjt-klFwTDSf0y=(p7ESF1oe4d==?7h$4zg^|p%tz+( zsA?XWlv6CgLg$6t1^UIXp{QzmKU3Mek%$QK z2&;An!-#zEa5%^ZML#KCgQtg1U+CGyY|InaM+sHOwapzPr?@px_pfk{er(f?!Qx3! zH?FHV3n5m)`d4|?Q<0<2w%3d>aE97D()41n1_7FYrP&l)ysa&@zN@zugMqV$T z>w<<%mSFoU6nLf=WLl4kJyR!8L&Tu&K+4GIBNHW2{J@4b_(j++JpZPYxYX|c~%@twHSZw z^Ne~}VS7r{e%I%wo)Y#)QKl{)U4BFmvlKhpv%XL+>!*ao!1{3WrY2qDz2mKngk{J% zcjHK#xU0fquV$$O$mPtPsENdR zccN0rOXofy*lYLtsK`Bml9Xnmyx7Lucd1@%{6jYl}8cc2ZN1%g9k zopn_kF6PPJfA##lUi1}fl0kNyZlxPsYaev^*SBtrhx{Yri``jTjoM$=3>xIqisdwV zCGBUS>UY6(8Hm8L7G4MCF2;aikOF|0EfC5j*3*!D{SD1N7>%2c)-w0{ltWC)0?2xd zz?xwrRT?K`k{PSo55Nhn>W4Ki2dnke5$rT%AIo6eQIeBcf=nz^lVkUAj6KUqWwl4(ezLL1UQy?91<_lD$2w9k{V`uKkzfziwrfT>bHW*-w$= z*FH#q%=LWIGDVMOYE4!ij(c<&*K=B zpPh;b8!RkM@h*dtMi!n@8761P(RVq&@j-;A#$yE1Y}tD12(yJDg|HxU54qO46N+-h z?-)n(^FlnQZ!JUg`E7;7^c=1kn#_JGZiA%>1SVZo1s(et;^4WpPDB0NR*74$*$e0; zg{6{JYFAqih*83ZUiL<_wMOBdD$@mlWrX*dH(?e|trhMThS*(9cgJdNUFOj}Teo0l zGp?PZO{rZh^P3wt-8|D5qFfL{-aO(OJp8s0m$zn9;`TJ#-^2>;)i^C){>3Qow;+>fSr&qn9oGUSLk8QRf)~D(j7DvM4VAHzq(N8rB_!(P~A(< zLLX!tv?)w$;jZvYfhA>j*|YuMx1rc03KO-Z_WjyPla+y8$dL!j_Kd!rWS}fCLU3yq8HSK$`b@1ctYu@OyQzvSKK`~{n z9L7)$^KgIcm3|Y@o%pqAIOf*jNHEc_wvF}DCn|^mCm_nL_um($>QGKvIE6kv-xWQx z+q1@F?;IQ&8Q|aBXtX!M7y5CI^AdevRTG^NS%{u`w+8f2?{I_PsbMUidhzR3(H+I_ zsaCHE5jW6-eRmGu=BkTsW|)0mKB;#Aji@)HzPS%=3rpch6Q(OCPxi!%Lfg6 z3Q;f0WGBbMKb~lOXK`bSXVN48;4{*oTQ!>WA$X|97rBse<}r-HV4bI?wxn;?ym`Q) zemSp}1ibo}oT5Nqj1CTWDt3+~;%U6X*{B$E+?|M;UdUZYnagwYYHStX^SfVYo@-dD zNz3B3cIk%`Dr_a28F|bv_M%Ub@@^tic0x3?W&Qrmr|`Af+o8~Xh@FmDxJwo<%w+Vv zL!B3mQ_8z|bM^SstAT%|MgHOV{@d{fvRc-C7amhv1!fKlc%$trIG0QpzO3X4WFFS5 zeB($)JB0X^yi2Cyx)nSINgv4}+ho}V^t}an01p>H`Cv6JVF{8$pEmadsZ`V$Y8x-Y zBz#A;kg;vb>aka|I3$g)k-@1z5lcLLuhRRXL~fXHt(qKn$l*<({`0$9^h{!CJvuJt zD+pV{A?vy8G(}S!1!3pscMJ~@*=X|`cNJX>ailWdsGxdtau1_H8Fh8MZ*ZIz{-NQ; zmz1+PI7TBZ@+f(r8sVDB3fH*J!R9ArPhdV8FqE-t{Vh2CM=T!+A5~Jt5!2&MYn#)( zC#*klal4-gAHKoTNgDME+3&aV$8KNanxnjpx##gD71Dk8@51x*JoX0&X&}+|0>7Rn z%3ElZ4epw~^f5j(cG8(3pAe2%{CZV5_!sDC+>GRR4^~l|gH@*E zE6xe>ZQ|j!6U_5M;naz%gyq=VjN^OS;wz2#zdq>KIU319(Vo?R6|E(s2_F_f0uH-a znDhwb>({@{_pDLP%)9sqTf4dY#La;Iz^sEB0Vu9dfS)Ce4;mNb361lG*?7-|3Tora z-5};DbaH0lBe{Fe(u%9IwQ*-^x;*NxaruGS7b_g_*rh*N7CX4eo%eRMMxS?BDfv9h zkoI~r$x+qfGJ|rQMxl614SD;jek6~wz90}3w&D4 z92cw^TTu2r*`rBfZnqsodBV4DonU?FWoW^@E?(M%W z(7Zs~!Z7h3C^#Isb~b&$^sUSAWSC3x-kGW3fRS(~b?2kYX7|=s$EP4j$6a}h^q>1f zdz^3yM`}B)N0B+rPs&Gc8^$FufR#7-C4)8?=ILn*!rmWY$&nv7aP;3 z$6Y($!MnfLww-!g8JyYVn)3#6##DJC*5zbb z&C|su{1=O>P2xA5_iq}5vzvZ4NR*9ej+DI49u+rvXqKSgsi`loU1-5DWC>&Alb)zO z;vR+%H%*h0Fo6)pZ_?D|8i@Ua=AYJnK-IkW2?7(!jcxCzs9N7Ir3dZUR<@e+=0yfU zTBaXCFFqXlx??kmc%S(|%bnQUlI~Ut*}GowHBy-xyqe{6;5I!n$z4357!@f!B}Bsa zDNRdtYYVp;w`w)_f|(BbC_XWDQpZ0PndP!w-^UGl^-fBr6aZ=W+GA3{{|}cIxWV^3 z!#nOjW1H~S{%g3|aWUL{5DN+-oDbAi8SJvU8A)I?frr!6&?l=|`ELE(@HIqGdc5e8(B0JRudRu0 zd_sG2IOw6!pI*Pu1IXECe7>!T@O3TPIIgDO>FJL6BQWO0fla($PQ1vS0M3YAkXP5v zRFl^#kE=D`Ucvi4+X~gh*ssjWeu`RCqQVMEx47GXId<$VMtZ0p12;Wh6T)r$tgpNc zwC{Dkfij-O7G`B5H*d4oGk0BksTaBC&CM`7J&-D`T)*r>Nc80a#*0yGJjj$E=THH4 zmg2|#qIS5d#{rqnPkLtSrQBus0Acw;Pg5_m?j3y9m-GCuTW+4>(_We8+%KT}DijN* z?0jDo+FF9Dj(GmMA&{rB9z(ceRtG#SE!pDOtpn$4(I38~$EPmqEnfAaux$L!%4yRY z%EIsVt~7wjr~Kln^|ZlpCQZM`z9M1`DTqw1f^(^0ZoIm222L0g~Elq8h| zxEd&aGSny+CQ!>(Qfo%HVHj0iPd&kRA|v{KMf!i@M=0<_a0iA8bCN#e+ZB`#GF%s$ z=lCl#-@{Yg7`^g~{&K1WCJm|JkElH#PX^Pec6+Jm3K#Ns(yY$j^9<~?l979Fp6Rzf zd@~}S#ZK_>i^~}$GIt>4XyhiYt%c&e1LKS6rm^sZSS zQ+o^&{fRXXCNQbd+;?=0Se;FlhWdq6x>S z7Q(mvB#_(`)fbuwuhycE*X($yia!BkUUM`!Zd&|R*A5|&eM%W?vm`Xuw26;o0GDaw z{QZcxN5U`=8w+T}@CA+pi!>fnGCJc9+Rbl_!j8d6-^q*3uSNvafqVs-~! z>f3DeTDGlk!}3Y0lC7{hqQA^zZ$PVieRWngLbp0Bz7ih>H|3+iB#Oz@h&|%2u3}+K zIK=AvEvSo>#~%z>hI7^{CmSo_Sy&rwyB;b~Uisvtn%C};m7sq_lW0Ct({6{>;8g6; zj{BflnxQ8~;O6={z%}Y}=7MVu!8YHr)eMZE7Dpeh@hhT)qMV{Q<~fC`IZ?<9bs@MW zNjQuc0@;~qcIM|7ZxEm)p3AE?U-d<;y(#?xBaaH*$Auh8H4`RetzU>xp*vkZPqo)g z#tWXCSw&?ft#95L|A=-*u;V4&*ZZc(*1+16OxE&17{G5iK7uu?)`cq%oVV-pQe}Y_ zg!nw?p~*ZuCUl@vjZg>=C!zhoK=UVX?b2S_XQ{^3J>pG3&)^|?_Y8}L6rs)jdZLH@ z0gw_j+Y(f}?z;nTR4Jh~p^!1Go5H56)}G8-a|ecMoA|?WM1X3kNlqbQuyYVXw~#I- za}Z@d`Rp$7at`ltlk5(;$({XcqHfi1pRY}Uuly!Tq}pnvz-??mI{5(fWw(6;CfEo} zrIH!S#i#LC>C@8S%+W~t?Fb?qGlh;lta!m4^*W;~@HBGl)S$G*>8ijwHfC}LQ2!*c zH*pYkvTHBM*2ap-+I3z7^V-hckHQ3Wm!;vYcGFmPh?L~9j+s~fa`s8{>gVf zgwXwaeb;jH$2865E}VXGV94HGMnq}iXHU6kS}daa{gHcfw(G(3`1wNU`*iQI-o}$v za0^%C^Qfw)N1mr*Ki4lG!hZ6z_JR_cTvyYUx8H$6ViqcE&0>MpjDN+2{b)FCYvaZ3 zCw{mcnNL@FTjJ+a#i{5%>-k!Om9Q#n&!H+}L{)_OE7EgbWhDs&M^cp+{3D|1iEz6 z`)iT4ClTao!@aOQm-;<_+vH0)Z99KI{LxtK^ri8ksRS6KQnh6>jE;|l&&4wj&%c`uiMMrbp4%kJ~P?JMEZ9x-rb zw(XHvbWYAdYDss=WJ6_4Y3>B5lm#W$O$RwU@S>&UScEB~fW42XjffB&K^YhZmmS;a z-&9so7h-lhY_GUALp7ec=*jd-kFPD=#8~i%rRm)HJQw2<6`|@=xd57PpJ2x$j!$eL!T-16~1yb;TmM^zrdiV4pY3@Rgt6B@~-Z! zES)M)sx}>FM~B_Eolv)NAhPL4a3skmi;YgW>lhbYef89=iOB~9Ik0g1vAOK_o^-xz zf9$Q0-yN5-+oDbGpRzVuL)S(NS?q0}a7QH_JdJ&6!VUEIAR~-yH%D7$D>2;ni}X~~ zN-O*_GvL#JG~<5D9zpC>D4i3Sy}Ebpb3iXRLMO4c>LU*hIumT1xScsqt1L1xO=kp= zeuLQLL3r&N(GWOxZumH(5P(%0pxf!jQSTGcBdy)b ztUtlU{&l!k`I5EHlS*AOG+0p!gSh-b+T%mi1=dRL6>c7%WGH$UPbyqb*@ zl+eO>P53=dROdbUG~;h*`R=YX;$n5_lNpCoyU!X9UP&K6o(og1zqJ0e6QrOuT`T@9 zEXc$@?u^4&$=Qxcmo=fdpe?DKJslX9qjKnLK?tJDi;$nGzyMrg&p=Ddve-H)pr>80nCEY;HZZy3K*KS66eta2mz6=&F zsOq88HMWE1#zlCOECY~?Ix|}m|Bi8g05pO{(bYCVpbvSiTTkU=d5jzO8mPYnsVHN2 zR`N;?1C#y9`EAJ3>181Xm5|5F0g^tNt^UB!D8`Ou_LK0pZL2`w4eLLC8O8`}D%d>d zzdXnU>p}47Uv*P7=ya596q0B+7_0Mm+hG_%YmhN-z8H?rusXL54$}@g5+w@WxpFH% z%G1CC^Tq{w8AhZAux6EyZxrl2TAKPDxF@5D^mITp`=EZ&N&{2AO+OtK&mKr~py!`c zU(GV|{jv?D5s4qWxpu;TQETx6N%G-@=lwaG)Ip|S=`&?ko=*Outpz(soyk+aSAIF` zc%q==*jdkHq1@_~j?VVSIu0kEucUW2o(A=}7j#+21RkC>f{aG&YYpXg!{vOvpQG9< zmlG@c2pV$5C9KE;)gXl|ID!l+RTZ`Jc0=-BRF)MB;jLR=q51w%vmXp($L0k`qIOSHLT=UJR${n~cf4roMx$$+sYf3h9WbqAnBRlDBp=^O z@W|X9>*i|NAWUlPGT@}|XxPw<*n+c|01=Gsg67Z<%D6q^Jx&=LA;hiyt;u(~f=3kN z5y>@S&H3@`M4%Tb`e|Br_P7oufp=2yw~V??fbfSh6OZ_8_P6gk1ImP>!}4h{G3{?J zmcMxQV`U2*l(^Y@lB`1YsIwm;Co3Kdzf}qWVefK;MiZ4oPgv0S#183mDbQ*4u~nq5 zQU7jS>=;^9_~I3&snP|e+_7#F0@(hlL)-(wq-D4=<4HoEGIKXY=UGJSZdZyi-VKX3 zGKrl25jS`3y&FkxFq=ZZEsPSm%mDf&Tkn}ohMcXOq}W{dS0(JAe%EKLQ28VoEM4T`R3H||4f{J^{&@lx4zyUabv zjh21!fnh#IIs|Da{3hE~!FoPYEQkY>Tn=IWmaj3Q)x7hk;VWSk1lN^>e4MA}BFoF?Iy_S(0nX$OQ&BkdB} zb8WjFr~P%~pQo+7GF2-%j!5%2Lf@KD)T{^k?4ANt*HRU0;se965~QA4oAx0l0f@YM z?Hiv6|0r@^vL^1YEaU``jTWDAExJsjZfB{{31xZEHujtLbE(FzzdS$VK&PiJ2`U$8 zeGpatG!Df5VkKh1^|^^3X(Z^p$Q5O3Y=7lZ;wsi~drui>cap<4%?8kqmoSEVp(Sc3 zG}%6u{v0_9s$A+LzoonWmq}{$+al)&CytH%OzZG~fyw8=o0*(f!7wP5gjmjV^4>$; zbH@#SFj?BPyXM(%=WQQ-fAGHIre4(wEE269yCt1|H3d1E9_Fa(~o*Gk| zcwbg!wdGaJWegd{2P?6F=Koj~pY$=k$if4Q?ec2gdayW{-SW#HiHujj|GmEhXdwxb zY8AnpLpBsf4;P=g8)04vht`})=dpy|w`;(E3Ez988$DXfd>4c+5g<$DE!o(;SGK& zH$^M(#`0sI*70n;!rxbmAYeAnV(snX=fL{Ktc9P&-vKrlkQWKrh@k%1Os?*gKvpUn zT3-I{;-y0U;k_w8pAOyeP68`eQ_8AWo!xfKFWT|P+)ekn1GUOK(gq@~??Wel=*kMb zc>}sL?P}-`{?5BTw{+jRNK2@0a4t_u+;yk;;PKzKpZan8aT$YWkz>=DYsa)>XB3PV zTjf37a1C=EkDt#cVP6|6HH+37hl|G#IclJ! zrEfSXGTz8Zn)r&ePkdQi4ayc7_`6f#q92Z+N2Sl;FZ8~^i7#p1mD8ky(-_}PS*)y* zYN7=_T9?m+5=tgnl)=o2C;UuQ=yoKocEveLe3c3dpTdPVlfNekGh3roT@KEt>DTNR zS=Z5Jvp7}mrs~!(vY2Tti5Jb9p8WD*aq|w|>%jQ&E~Y8Co&6P4Z+-}w8`W93u+(oa zt{y$=X51B#)>=?CR4XjbDa0#qXEu|@awllO;@rBJwB5b;w6Nl1lDqcxv+ijMtUV!b zob3_s;x@5L?k*;jU)_)^?qfB=RNuXVH7^TJHg(j^gATk`;yE|`(^qd~-KaPYd7aX! z8sHHVk$++Fa{4dh@Un>g&CA~GQR2OGd9Gql8&@dbN2#{o2d-ZK_+tp6*L)LespI;X z2)BaM3VELVWqoPxu6Y~5<#(47DqDTClz2;Cuc4U39R7H#W*G6T61ePEbgZ9!&^9Jw ziTy2i?X>ic8&^ueOGgZxv)HJoz#HEsf3NAUWGU*-Ba%$DSV+)0IF}-s8K&~0=0UIP z{e7VATTF!RFW^ystn-yB54q5mS_gVk^TcyQ7}Sic+PIn8{%{y=(o7Yv9LDI*_zq)Y zMy0Vcr225e1(Rk6s%RXO@V3L4Q0ZT!vE+5b_sTp}U4Cy_tfF4|W|ub@hjRYcfdZcM~lt`&h!`QNGrPq=M>m& zou30Z1?P(7ke(tRhnL(YOWsT+8W9-Y!tg_p_nATO4SzM}`&MDhC%?4j^ita!vfk*Z zh+(B>_bccnhQgoz9d|$;o#kK@zG&>6ml*7Dt<)u3`#sjHzqBrX36ACX@&V$i)+Dx znLaeEy3;F7KA}+8g-7qK?wCCzNX34YO+U;jq$jQb)Oh3cY$`Z?C&he8-fuh@LMu-A zELJr5sVv4FZgS9`Y1^$9YM8;^>k-=CTMGKM&RGy1#fZW;HQcB{-$8>YN}C2_!E(leztTY)(f0e9Tn&apNHkRYB_r566fV4|P}~5+ z86!SEed{jko62;j#rOBz2o$=@p5m`6pF_;zAu%GN2CL-SKW*`42+Pv;6k+4p;Z+E< zbVbrI(lURmCmNAl8|2zTbcZKphVjEHxp!=#$oQGyY9%oeZpmx))#Z|C;xAwu_gCvv zs@+}F5#ZhbF8fmGK4fxay+orp{$Y%$b#NsrmV-({%LigFkNOfnxh#waV zu>W={+?A0%NU!!cw{F{aH6DLw#`{Z{r`!w~cO;xcZ9mn@dnANG$yiR%rcgmp^IzQ)* zrTNQ#2_N50sZP=Pr^R#jHw&R@!*-btEBhp;x=PkX*lI=if3z_PI)Hm6D8cu9gnGr| zbCR%Z0}hM;$y1u#B*BW_7Nz%kcFQ6fgiz>Hq(yHR&ejUFU|4&^mH6O@V;my1&*J`FMV}&m{4`B^24K-;dn1)<2ZYG#V00ZDB z^XoV;rl{_%2Nh^vFp%|6Dn8U;;k8T>nK|m*UbDcV`fivUw3$*Du22D|1=Jm=t}@Ux z?Cm%VnioI251cld4S&6L7tp3E;8FjgO%Gow8L!O;s#ptL{kza@*I)^w&z1Trs=EXn zE_drK2%vqgy_YALSkpVi;g#Mxk6NkEodo*sggMc7Oqj$8daP%PcI8DPgR9V$UuPpi zOsv%z^q3mrWKj)q=-u40ayDy-$@>^fRlSg>P{!NDLa1Tog(oN}^ZSsIp0nxEIdX{+ zrPaUDm5|Bmm-|^_HzR>k3)!1|a1U7A-;a_w3Si)d2g!T&NT|@~NcG-O+JH-?sfJ%q z$O!>?D=Kty&2ZK1qROJ9j^>zFvQtiqe0Ew6heIr>6UxIUA8A0VGG3$$T79tGyx!5^ z1M;QUXMNUF#U<5DL2F*`H%qQvY#P*R=#CO_MJ!Z&bb6UDL5s!hSzoGjboOm2;|CQ| zXIL6;m!wufwizvcK88=jwW4G`<=n>Ftt`3Z572#vfAk z%_C2A+y6sHb=ilD1frL~BdROhTFnT4F6H)v(+N!$(!m;TNmzp&l0#u{8JRUYN3zcK|KboDif z;4-;x&=N34sP8#SHqWvPmBtlB#5EsiE^~$Va5VMuNZc|SyHgnKKZLQ)x^yD+F+3J~ zz9q-}W4EYa5OY@5rb?9;JL`_+SSwJ{EwuN^KZT^m(qsd@`4oviiA|0nKE$Vy#jXFz z5mv0i%g#x+xnC9-O*Dt_>*wjG*l!I`J=0R(-t~eBC$J!$MFnLrIeQ107bEO-NNvsI zvKjT_$L1d+_BGQ9Em%%`0KK*>(jpl{cbDbK`GDc%eXX|$RRiog6|#rx2{0n*KgfX$ zpGG`xaRIAbdHsPrME3a$1m~VB*-N0l=S3RCMd7%BZ5XKbp9t&Xztf)Sxl9?>!YF!gm zcg-7Sz8T`?!S56Kn362p*#nwFGjLu^Pi3B~i?3oRc_}D-@TH(7e=4x>UT&KFE^{C8 z`9Be|p;Cz_RbXZJ^iHf9C0gRiP>cgmGsNfF9%dJ2#O)}XhaO#+H12#v@+AoCoIU}V zI7A1&rzJ0^3W+B=kXvgW2}?c+J9!(6A_Knoq#>@^b(ebjlylx)hWSHS$Os?=o@5AI znxA^tU6x+9oosa*5V4h;_OBLDU>ZJK(#^eIpgKLXj@(Jdm&DvKfqtjpkA$o`;X!0> z2<>mfWi=b3`}mfsWmzxsN?Uzgxp~8*c1knJ!lZ??N&vuq)5{sF6MuOpEb$C~jOv;u zEm~{tC!|8Emv5CQAbh8p8-|^AzLAa$sO@?)To5(*QJd@m*S<3enVQ&+^CK#ol(8B*NyD8ms6vrXO~m}wF9|E{ib&GtrE9^Ki;Ij z@YRlx2hRDC0X}J|i&v}?4|@F$+!oQ3737(Ho_3!GYx}oL&p!720~`6BR>B|3W_ZI+k9k>DUF;QN78bNFEt@y$9#M+j*c*BJ;FZ!#CzZhy4eV9f!ZI#i|XR zp0faxBjjwkQmiUe1#};$#fpZh_*gDhSmi-MSL;tVs3Wyhc>F*(Bh@jUHv@k^4qEB5 z^MrPx5SGW1PpJTI-Rr3JuHkF5h7U)W8{-wmX51{ai>Xn<@U`-KHQQ*=>YJ@k)Q(Q2 z-)K9!s8kQt{s|!yOpWus(o(;fA-gvIf&KJOE9CH%c&9-}|0PRSELy~is`r=if#NK8 z*FB3~f+uc^_;zudjzKcI44O}T18zV3i4N6squL{5B67iWt|ixR{)uLP3<+>Ek=k&l0=6kpwYTE7+}*mkazU zwNVOxpjbF59LY3jH8-f1>oe3ha@Afh5;Zj_4KLhoA1iX?!)xc^b`*3oAW`H#fb#!M zBG~^jKRJ!RJ8)LIx$L#pXgZL`wTtq&#KP69A!-uLVD&a`MgS{K1f=J-eyk`bD{>Da z95-}lXPpP$0vMug-`Mb}Hz3higIJJ-T^N&)Na18xlNPG@JDNM3Vd<#C8;z(JfA7&A zBT54d9|s&jmr_h&)@KnPbahmtJw|Kay1TN27rFe0Ac(N#cKwAI3nEyjN;~25lWrON{F!ayN+$}yEIIGBD zxDv%(2FePwRe1W<({%yB?qkm!lFrWzi(t5k?zhwP!}!>2OMDDyso4%AKL2)Z{sdAl zOq+2t_mD~oDq0`7?e~O}4dVGB42BOlZYSWl8B$$1fmRHDVmpFgFw`Guusq&JEa{5+ z##v7eoE_g>X1+@Ap2B;;SloYQtf;m#xlw(yo6GlVZv=cr`cL@kxZQJ=AX?NwVYu5O zTYYPL`XAY9mvyq~2VK>hw4ob|EDmP!{WFXIdm;Z907k1! zD*l_Tf~;=UXOyYiYH}6#0tSM*R}*0juSI=l4CS7?FWhIN2}u8tFV^+Q6&rJw9)bd$ zwIg>~+nJaM?;jSaehbV){!d`;WBE>(I+`n)5@7^U0pu#&BaQa`b213U_ zYyJT{_B?eq3K*r@;L;&rnc5Z?RB&Z(P6l;JaUjM1uA7%)mXN&Q)MGEs96o%`?GC=Q{xfytE zIMZ$MNLLz`>dl}}e}k_eFx3GBz4h0Ax|8yf1pwO&tRN2-A>Z&HGOYY}83w4&8-%eJ zk9yMId0`Rw#rifl5DNswAf9EQISlhxEp9Jjds6Xv%tMTiLpjBhBk<;Sae*&m#&;QFW@g+tyeo|ZY z{_x=-bH{jv79et}FTkPPn6#4@#bsxq^bi~I*(gA!WdcB3i^v07aI`U$i;ku#*4a42$7KVxS|>}(oX~%5 z@t-jN(SM-SpMHYHL;Ej+|B2oI!-(+BWfF`QqU2HFykG$ zlFee^)M-|&99|l(#Uezo9!Z`^#&QTPsvW~HdAA3CRDm1#5D`B8#NG9T+9FHedI605 z#gM{1;Y@>gVezyU-xG2h#NQ1|Z@&FgDR1POy;kMT)FJ>;xnV$CJBDNs<7eN_!asIM zGdq_&(fKA@wU_@PTmNwGYzu+?C8B?Nhk&Ht|4Y*U^w>?vIoEW2fzJ&qUXnre>erd{jAgeKm7C64}Wx)Te3vc*Gb?5 zY~tTP?B||6MD_j61QS%~^v*wi=?Fb9;KaRgr%2BE*Q}Qf)+*EyUKzu5g>Tv)|5bRB((`Nj@dOp31y(q~ExQ%#YHT$B)ix$d!S+^g zD;6^`*A_{~W^2jMZIl>5jD_zT-w03O5{>h>jiG(UCx13iIc~a1`*mxNr*TY((?QAg?l91VAUyNCXdDwomj;L}*J91xi68;6!s7yBH zXdEnXoIgDt(zdb_MRv`x2hr&6GJ+z}fLEG|3cYMOj_`WE`?-WE=C5#H0m zJbB;jFj5JHG`6dr%`_P7@lQoq%L9xbVWP-J@}uN%eg)TJ1L+}#f)m%;H7KqeTHln^7_z2;_PnG6l?I)o z&oG5F@m%4Wa7m_heL0_2TD=7k=eNF3^(r%o^2l|O#W{wO z{Igau7Z&bJJDU@@7MeC01rXa&gn2|=_HWy~tvE)_)c52ZQ8yxE6?@3Hj!NDTPq2W* zGwiZ#+V&K8i4Qlu=tl~_3^P?M$wuL!&Es!NTA10Z72lXF#V3u-emik@Yt@kG z3OaHMvARm=>F|U5wO@XaA3>KLItGYd4<-L9AjlNObyEnMOy(u@ zWOn1RiI=gFzD~~$ojPjAi z%#|J)JQJF|;3nZw$y(YHLI0qBksJ;Wbr3KuCH`Y48pg)u%DFk3yDpVmXq$=<&AKS+=Zrpw0}!#$^-c%c%WJp8I5*K45!7>M& zmNpt*RnSUmH3@F!*A!nVfigFM`N^(;PeW`c>GLIkd@W-E6Hxdgh=VM!9D@wK@8l{p z&w$*xM9rBgKS=7ILXnNbpHK$UO3~^5aP^i99m4tG*>b)jwUCoPAx*}QkcB5kiL{5? zaw|%%E-9lTm-73Gqqr*lgMIN=PVpH;dhT-#78Sd&lj}1$pWbPPvW?BI zqS4QC(eVy@haDU{w-he0?~rXlCcy@7GnOn2KEWTgDIM~3=&$EFlsxk~g>>n=R zv+qLR6B*jaobOaZm<%AM=;`Li{uxRr@)@PMd?8&Kw`-TZiAo&Z#QzdI*k5aaMo&d- zb~Yj7(fhR|t`UgFkwDI5tZi&?Pso>gRi$Hz2cBa!-$7%^)xwj;A$km=8vdzSUzkDY zujgx^m!=n*mhs>lZ~?vns7+>qsP4@9?4yd$FZXR8K)h0Et}PCq2`fm z(q3wLjmuqgGVK$K(fPh<_%cY8j_B@?;E7E6%d@$CT_cm9r!id$X)b)snO3jaZe47< zD4*Tx$xg;lM4f6qM&9^a{h;g6HeI{S7}0DjW0?DZFwfuC;M+7qEuvSnq}mr0P|JS7 zeIJSw|7^^&neowI=tml1`$Tl4#Sd8+zDsE4HB<8{?mbc3gG#(VdLqr-Pg)liKPt@2 zKn-6sUf|E>MsK&eQqa2+R(}58tYx-(+1ZB4&(B!Buf}QL6Ny@RcxLLuvrr~S!$0^Q zGRGLbNBpvn@jleJEZh0l8X+g-ib#7OLZJ4l(+o$}&cd+I7^fi&Eo&NeQpS^ilCHTh z!L;T=6!MgdaY-vAsBKTDf13UtyqUNkNe}3A5|v;@(gYDk zXOzRirxDQNLR&QdXJdm=e&V*|{E%h4#>?<=mIC?Qctb5W0dG4RNbMgK401nh zl;g`HpRdJv{SrRdJE+#vi=G%R(Ye2yBAE_ju*n{K#9~7K22y}+pjYk44N|dSfw2oW zVHXxFhJqta8AtS=@qxxunfyel8CjaXvk+I6t=TanU-^RY2bbv|#ZSL~8z9fQo1-GD z_abi$#T_ISL{4 zsN}*tXLkMoJd-=HjwhWW=4pj7f|2EN#Rl-2c1)Gl_sGm22wi3Tux328Og37B)Rf5j z=FPCfm`Az8&Q^)<(|yypzwfz8bZ{#UHE+hkvsL>v!M46|C$;+e!3AU&iHbzdX(eq` zA))n5nyVn4zm3-*iFP!@W-o^wNaOJR8TZI$dxBX#{;^T|_64`$vQY{5h`+hIb`!Ta zvwCP#)#>@Gf`!_4uMCL|-W|>%k}*G43ogx+4rb1O%@@z6YO)MlEBkk;${n|HtPE3Qpfq9z-CVXeWcvg&3y#~-2*rj z=T~*q=u?rov-rWM>Md~=k#L4hpS(Xk7l<-dy#{w)26?P zr6I|~Ekw{is>V-nFee@xaSe+^Y_LgY3t71VbBtM8O5<=>uTUAw#mw0V0x`V?*kg%b zz=pSuzx~GLCRM(9zWz81zuO@iU1pmzY#Ns)T_R&8<0DuQ?fr;;%mYTAe`QUisix3&pMhWYAF9(+N z%sPqd{wFj7PV0Z*iCN{`|8Eh;x8?jV0v`ahs5)A|C#d|M+)@azr4firzqlvg_uV12 zGPNapHOs8u?RAM=^5#mPp!UMIjfmYoaCFmk?>?I)Z?5(oTv`+W`R?7Tz+Q!WC5u=| zJLS5j=aLtJW=r1{2KFv*t_xYUB=5a1bZ^%;tmXKf-@~P8?P2+$1i%0pvBOek1snny)PfLn?Dl-Et%HWR+^v^Sf`wHG&;aMob6scvj#=roh_^2Bt0ln0t zLFw?_B}o0DgZ(ho6YnyeIa_7LNy{Dw&dlCUReV!%px>rE>l`ANb0Sr^txV@lpmphonw>P1idlMC*mynP~4h9u3X0D$>mKBr0+=!okUjHb@ZeUGCdbJ zAB$ThpU2BL#%o(>xMKL7wJ3#@N5j3NEaP&`RK-uRz3>gSz6&3UrIB0p)hXtCmxf)* zS5+|d)>H3Bqj{wp`84`yCM)Ph=YdI7+N{DIRc-T)q?tJX=*1+Z!4kTwrtD;(1U`Ic zXVwR@{5E|{5msM~94qQ7Bdr%q^=pzuAN^TO+7jBwnaQAFt-d(gimw!3=)Hl1n2TSq zH3_Nr{v=6`PP+n8SzR)|lTs8#R)NSbhRavbV-!~6bj>y4qCE9zMZK4EgP(V2Njt<; z*RaW`CE+r@KhW)xIi%W< z_7TxTZnm+njQxUBATeh^;SuO*;n4` z_MUF{wUcO%PL?rSaNR->9Ho&{y}w76`7=opg?P3o>OCI5a2ZQ_H|tC@-iN(<#@_7c zUFNW@Rxtkne?BC|qf4Q#q=%Yes)9V|W1^y`pFYA)t>kW(I~!tj@6oQzSKb-!8RzUI zGwkq5><@vkPnPmt(w7i-r`d&T?|VtP?0pnpraAoJrNS4zb0ZkXTO(4}cE9#Kp>4w+ zA!^iliu{=EyRb+?|8`~Go8>KXvl5$_UiolqG(nW#TUYP3AlN$iVzP(h7jbd=X3^js zvjRm2*#$u%LG)-E43Np3zQa}0u8?P@&!(=g(U&yIMR%s-k{a?(I}?AUY2II0FPnb< z1Usyz8lyonAwn;X;D{Ne3kwV0R29EVq8;S)xax$ZEDvjngh)bElV=umLtvW3;R0p) zC57s2`g*#Rn!?8tpy(7wjTF@g6b(Oz@lREhwCeRDt-5m^~ZS2NHeiFX2(iSl6jPTpQtFb52?Yo8ev<51d7tsH|LY{_zU>b=?Zc7*GI)np-H_!#*e`D{>qnbSTwozJJX}1EkwoVLbi*=UZ#2_JQ zw_B-2WILiVM2d)t5KtL2)t3rYgwz3oBBX9qM3e}~AVVG@K$HX`GK4V!Vu+AHCNfXo z9n?;{ea|}QtaZNkJ8S>rTI%yWd7k@s-}i60uj~3jPSM52G@PnI)L4>N+ZZk&rl*ye zx#lnxPd}W0tDKtu!SYsp;XRHWhGMhaG*u(@Eh;4*?$0_N>nu0LOs4Ij`^1OIMw&jw z6AHSNtwh7N2!nm|=(eMYDSOR#P~4|ow9qK|O*{Mw2e0^((yB|EGP4Y2Vhm&WZt`uj zb4Z(P6n)CxrdRe}s7&W@Uwmn_`FNh?s4KyCdjX-0^(qMCM+W?ECCfNQ$N zWe@PS%>wtdfvo=UsrdH~Jn8~Vt6>!2uvQ|ZPiE~Q-mP}YcMBmmjmc)UlxnuL$QD28 zF^C@D!K2G;@7O8a0j(rv;;<&&tnnS0uTO{aHG|glEXoqx6{V^{dLCWtGH6bg1spi4 zSTa^{hfFY`F1(BsOLlR5!{wEZ1MWqYwF)rly3<*Qo6)n5FSMypx|KW>@pm zwXnI68eQT*;2&I~Z>ew?H!1R6RZ$5iUYV!J^fZDfp>dKzYj5VE(Io#KeQvW0M3>XBV;j_IZx?yl8!fU8=WD!cTs+>+!=4qkpSMF7wW9y6AuwnhRL1&Avl!%@O(ws4jii9)FvV7r}8M2@lqeOu%Y} ziFJ6*kJ>Bf+LeZOI0PpSKhYCJHBy1ZHiR3O!@i*zENR7m-lzTtv~RFV-c%TWj;E(Y z8uM-3dJt*a(y)$Fe*Wm9l}_}R-CQ>mB#)cVfcki-PuazZcz$faQ88`;hBnErQhm+e zi<1wT`NJ%k_<#o_UIwv`)ktpFIn;(M2*XeegxmbsQ?tNC*p|a{Mb}1)a03C@;88+~ ze~W0KLF6N;A_Q+ij5`Xw1tf zo!m|l0nIBJNDB0jHARi2z^Jy#=P^M%Q%jTk{b!gvXwQJ!@Lu}xUtqh;JQYlJ*;Vh>RkqTQba~@ z$qswQt@HMjhU3=OpKfil{~kh;{FJF4 zv^C+*m?Ph7XZAMZ$58AA&f4=B@d=Js7-v_C=wXNZ=_}Ta8Jyd@l$m9Qnu>TMxn};_ zSVu%Um?4#D;!f?d&t;y<$HC8_*u^0&mMK!qb8CjJL4t-Y8cRqLk|fA#@U2Z(`{Dqi zjm0B`pbi@zVDPKW6^A3&gB?B*-9#OfXGl-rv_*QlAA}3mY%_UpcjzE~Y0#x%?3TAy z71=1LPTq@l&UNY_jCR82l8v!4qo3M`h58sN3k*md$agY$Z=OCc6~U_42Wct&l=ZOe zX3Nyz-*2U)F`6Ft%l1qr9v9as!U|hUIm43XflQbr17^)3z0V-kGQ*%!zBJB_%CC)P zVEtz8HlDiB;*)m(NL&Qmh}Y=OhYe_SGRew&FuF$Vc3e?&!|Kj8FhPQc$*Vh`VQj5Gc1LR8@fSQ&LUsa7V1382$Mdcw&)OJlOuaLi>e2ty ze(_;s4}vAMW~7L&jKsCQbdQ%!hgnxvh;EmPT6ONh*IGm`?k4MWaiZQN8`~-3CNF9x z=PaJZ*OeJ{8C}=4%b%_lP}|a-btN(S;a$?l{jR97+N!kRly!KWD_Z+0 z-mFqLPNtpUy;E=ehbEn;x%NGN%dxdc% zj1fAYL2R4FOzzL&58u@)ay5jTgvHBU6ph1?8xw^{nRo9FoLzZ+zD8UuHknH`n6fSDEW2=6`6i z4A#d+nX^gp7@cHVd_KnPLkORqR?E%^h>8JoT(w+{vGd=dHJ*RH=3T69v^HBOzgkYL z)!FzPXBVr~I{k#4w)&p?#whouj&0nhR*F(pDB;XUUYipi5X0O(Tz76n_z83_arJ$P z!Atg0SBp59Fit@;V+k}5XYP#~Gq95JeFWn{h(rh%>JvKq3hs^rN-U<$jjyo>aU+(`W^4LZeC%+kUG6!X!U(K(17%APWN=o7Q z!N#nGi-~ClILB!%f|$~@Mb*)vCV}WZ!rB=MyZL>u*;~wcLNxFsg# zcAS4l;*G(^Sl5U7W9}?Y$v_QPB`CD7$BAj=Cgbb^{iRhBj2N-XZ*ZIKOHRj-g!J=J z>foK_Cht#bhDvsF@7QHtYYE9Nf+(dBhZj(#4*oWHXD2w1t@A@qg__0(8nd!6XB4ZT zEi_$5B<_f@sX*XNN8~=yg0-oH)D4|%Sj}4TDsyh0oHyR73;hnnb_T4D6sFTk^gq_Az5T%Cd4kQBizG>abof|5IzcaG9Z6K ze2|~)69~3sU0h^m-mx$^g?g|AK0rkk;;uYQ9mpEZ9iJYk4t{6BgK=yQ96#=>#m~j( z0UF~JkIquvM{lA_eD4@HKgbS6>Iq|I=EQ+lY3Y2mJ1A=IFl6B*=};{}sy3`t6CY^e zLS=E#sjyMPyaRt%*pH?2e;#2LI-C|@P#(+fQ8>N4`HX)RZ~{o`6O6-g^I^V#IVM5VyFlt}_a)RIR>e#E&BGQK^Q< z1mc#q-pS(|1z};6Jw~^BnP|T`;i?-UFv#Gwljg@X1}JE6{p#NVbKX48p-1`8HEsR~ zy0nJK=s)i-`5VXkfH8Z0CYDe783yQI^G7N**q#%@h@6NG9%c}JnZLdxqk{~GHMdCtfRgtxltw7nW565i)hpfSb}Mp|xkJ%|yGL#O z7QPOw)&N+Cabr!1y}2dY@LAQr;)C~HZ0gf4ovkQJcqEVzrEW?QG$A^{AqG#?1r}F? z%s9-)wi0aZWeP!F=5#Ng*QLmpud9fueRszmC~6f&Daz#;j(VJW`E>a9&?FBcPO)_r z2@-D1^s9~r?5l`=^_SM>>40s3EOlDdedVUd_%y(W@MOO3w%n=7DWOKAft#bw6lxRv`G%-Jm!mUy)gmzsup+OtAc?LY3sLmyVFyHj|~E}VRRwZ zTM?kWh?Y&#Dcd!vIU&v|j=#St)?ssEQvChtuava2@k=6#Z0ZDPm-})OHMfD%^t)-lZ&ikbfusTErcvPCX6@2MxfGQwzXwT?IH@{jWG zkGtW;yuIBoWB*w)?pB}du=hdoj_0S$-3Ux);UvdCDVO#<6lpq>U@)Elbqv`?r9>~_ zeMr%a4cm8&NN$#RRxCHPV~+5S93|c`v?%{kJy}I4l6Q_Sp4puVg+2e$%wE|hcsG}Q zXsY^1{4YhWt2I%{`pGIfsL?w)B@UkrVOM_YPZZ`Qq|RhG*cPe}41XpSwq|xu?q3wq z$nV=JzIwUyfpX;MWtLwcVYmwl2-=#uPEgOgzPrj#W_^*DEP7x-rb5O6sEt^i&~LYx z!D)sqHL)U)SImcF>yX!!*6Dn^PZ<)lYI49m5&mTTDO~NKgLMM`)H^y*g8Tx6kk>4^ zh2Xm!P_+H4U%MU+T=kb0R}HV9-c2yZFdP$J^8A}1@N*#m87_Uk#qJ!Npeom@;aHXU zL!wztgvMZNGI>=vQ6R6CXxwk0J#!X>YCUPe(U~c=v5&3r)stcKK*4f{rJQkU9gF^P zOkt1iM^w&7#Okw`5|f&k@An2>k6s?cen9C`-ezJi4}5wc*l*^$`Q^lBN!Tdf#wXL^ zL!vr?NG4L{LZV5SL{tV4KQ)bEOv{LOsG2R!0ySKwTdMT_Hc$pu!1(l?XF*kfrC!xNhYCJXRz`IjjhhQ@K<}qIn#$a?!9piEE+3ub*1D{yl!)_i!bY3HRJgQ#5+&_OV8_55L7*693;(`%=h$x)> z;W!Uk@J>HQ0?mbg-DNo_jH^m74c_tpc9N4`?dSgH)6WOom8E?$y218-vg}mvQ~O1S z33n_N;9O#-rHWi&sUqjb5A0njz)}TspKOd?Zgs#dI`{j7)wb{a z{^pGJR-M&(R)_nlquxC_viZI5_wg(37DUc~)YiP?-U=Qa|L*tU{K^k=&WS#<+Mh9i zJa8WzELduegDYmaW(<7zfBT1%0oE+>ob#_|6)&~{V3rc#)n2n?XZ~Ef1w+%0r2_Qt zuUNbXJJJvT_218WNPVrW60qC<@%7%zOaJ6rS9Pe<0UMs#q%Ksodr5Yj0h@6sre3+9 zn~R|>ZqN75O&DWclLFONC!d@jTUyOr3P;cLpsefqVPVQN0@QAuohuiXs2)m4OLIzI0r>-ItrRUDCj;z53c5}L=UOL@Hk~&R zXck~FL~`W@ERt-vsCI18&*K~60&TN?ahMFrNaTgp*{$a;JXlY9m<$O9Fi>6qgxL@W zJGR*B3=o>?SsG_`=mBAPNwL!~keX?oL3Y2zdtSd~`&DtbT~Ju}tRSEjL3qD~k49eJZ1rG2Dkfu}T@0X@2rM`L!ZG&SZT)ligmp>j z#5*N#bS-|N8sFbOVfE;c@yVwB-cL5MBC}@D-fyAEo3=ZCzrPq(-Ez;m8Yo+xvOM)I z@%SM?BtJi)oW(hywBo$c()fjc{QtX;Omq$Oe&Pp+Sx>6h08sxdQ7-_Ar_l`-axs7v zCr=bf0sH$RxL|k)w2iaK4SfIi6FKJg1X?HH2I#H$__G!&p@qy-nh3z*&EUX$b321{ zGnp683q{VhiUJG|V1A^<{5EenTyJpR!1N6f!1TocU2oBG`Guwp%&xf)%)W``TVOHH zZ%t0Im>itB&hUcXf{%f@hs%MvyWg_Geq(B!#nem2Ztt=@={IIhVFNP<0E)m2t>G=o z9GF;o5X|EfZy83vnZy6nKH_=c#1rtv_Hr*me3IpzQ$CmO20k z*9>H-=UZqzcBO1rG}r%Nz2t?3SPllJ{cKhWcbgI01N-+a2<%VFa*MaEwn91tgLwHJ+Vi-N>!Sf*thU%yMede^5*}Obykh0kdM@T*g9)$U@M-l_BRt#5YLtbg^Bx`XM;o@81dxFoIdRY5X-On^s#ZdihM`npBg+Y+%NzZT zUH$@zVn{+r(DYG^eNqL=tQhf@B>7j1%b1OpDOwplw4^d>U_m8@=TuTXdO^xn)74OOy?__^p_SFK{jkAWut_alyWZ+pSdMkF zZ-%TJu<52jz;IOAP#+0|ZeDyb>wCAgi#y#q3>jm36T!N8AR zUa;8e5l{-Mzulra0fM>Y|J?S!aY(?~$pOz+NY-MnU15=odg*E&O^4g2X)P%_Lc!WF zpKbuMLJKk-JU_M+0c;&S4g(27Aj|b1d;Slc;$^xOC=pQo@&Qn%pnuzgAfxt8ir-R);+u-yCq zbVE2+N|u;TyCkF}FSk=aXdfXVFm#-soKu~FqVik779o1u5##Nxl7)ob;x=u8Eu zCLB1SxwLd`#Gig_McaB-0)8`g`-`!^y-B%$KW8f!rzP?0mgb?}?#90#5VzzU^Ekz1 zDMp<}sZRp|GoU^LR5FimeH#t``ck*Isa>LaLb)FB?Lwb_f19pX+}jxrV?_CK;wcZ7 znZ-7+dI6KXod)m@d|an1<_7r|kQsj++x$8bYc|y+94|=K!pt(5KM+1qv%tzd;%qcR>I;}UvAG`{_9141-LbvS}ESxc#l@b=nC|k zWZC#hn)X+1K#(j0)uc}|9lhKfECSE#9mdp?0}((Q5Y#?r9QVMFp(Y>Vw9gfw_3Etv z;wt_TlqF*WElIQLmqwZ1=90f(4S+Rm?(#S8H4%V_mQu}fH7I|q@`(HjA=mXPq zceOgnb!w)f=qwHS!uPVC3cH0R3`*##|)cb;rdV<3? z8`zPV|IGtH$&J$O@aGnJ5S}C9)AUnw%+nN8r<85#L(MT8C}xEeZfb@;KuvE!eL?`# z4-dmrlHnUo2h7;t`s*@fjU|rh9;xxSU&ms+W3nbntph=4 zj<3+Lapt|S`lSyNibPMk#viF(56zdatAR5+u4U+eST$re>FHEsKUFX@Cvs=&CcZ&C zPVCvoz;3@D@PBnTiI>ieWjJazAuwT~&2;xA+UBl+;!M@$M^3j(X6-5L?0-F?`1ah}U2%se&K$zNUc;A{@QdFDcoys1Z(_|lxvgZ@JBMX+6<@)N*3m$nb0(0> zUNTe*C|bwHHh*NbfDK&LA8$=f`u^zuAvcP=Sv&v_y!`06Z?N^9orPea!GY4heeR=q zbi|XRn?!M+S-E=Q>cf_P zH&5yokDU#9U6-D5@H77BQvjz&;=Zm{H8h}giPC=Dk&6$;Cl?-mS%jG|n37Z_vBcxM8<DLF$&uHBTS?{@|SOU=v*k_ zs9i|ytyu{U1W}muX*ng?yxaU$0}IBJ`KPMQ6U)neZ%Ut}@n*jSFULLRf^9`*PrQueSU+K zuKfIw!+T6JO7leH;ndA#KbMIgWhpSYf1IL&mN^il`*GCc=zfNvQi0JL0?`yZUIlFS z-4!jp9U{f&b*7{pj}4=Gd;44bAS-`2gt%kPx5jnOor<)l43n_304&$n)A>FQBXR9c@XDrn5ZK+?LxyA!qH60ddd zqNF1$x9ED4!ZE0I|A2^Me4I{m=YYW#eI+m2V7>fXnaD$ni3AbUI1H=BtWiqt0;Pau zU|0jaDXibEwBK%8mTA$F+V(8ey@+}2dfw4f2Ph7X%Tu54mQR+GTxB2VJCde7%rAO{ z6jzz9Z4-;b<0Q{j*z5PI;?6VIU1aRRu)lXTpuk&`M+G|{WtP>`(f9*n5>kncoh;YAF0zo@(UhD5IuSjdJ-MRl8d z1(%+g@q5iSM}!+x+Dg1Zp)uUSNB4=gkGPy1>w?kA+ADdQS z%!UCtWd|}dyRZmRlcbS1=!0Dk8PI_i+!4mJ9VQzv(VBmKkNKiit`hMKZak3*$}m&ho&SMHcVPc;xPI(?2vyfwY8 zj@PD?%OV`BM>vFHj`pD&52fx|0)_BnG_{-0E3Ig9zH)Nw5@`W36wDP6k+&+S5Ik<__%Un%F!hSdTnG*##UZ_dQ zL9>0doDl3m@mH9|*Gpmw!;cuZ{x;m0$$u>{t?L!w?Y^WVzS_V! zYxOX3#?UjbW_fJeA+UZghdTXy4Y=I{h#8kYlS5h_dvxt<=lq-n{Qa8znb~Pcie|bj zl>bTiW69JlH{LpE`C{O(H=YKuK>FL!$~~O}Z4mdCeHj$f@IKw8mVj2p?(^>~FuJEQ ztAZ9p_a?aA@T(_Wi5F(hp=YlxVBUSeEaBzS=1ThKxj2My1I~H1?}Zl{M_L~A=~uFa zWg`ag$LC_8JbjIN+VC)Y_afqH`d;4iN;YSy!tbuNFVud79qUc|RcGT;9S5 zd_65PqWV}&U$Cyjhki}WduFn)0!-gkwL!dNnnfpsvX*NqG8(r_DaaxQ9JG$^U*J+> z&`Dnn8wErv4v#YHUh%6Vh0P=XGC=2i=>XT^Y}TYJpocPpdu457Njm&*3&GUtxD88M zT1S(#JT#6z7%3pth2U7vs)8@s4t`RJz8t*3j>--%)n89ImkwV8*Qe3P-ItlY9jdh# zVxA52UcSE&_Ze=gLL7EYF4qm%vBN|@bb28##~?YP-ZT>22c;Oos;6$gWPFq^l$AJA z?5H$CMD>`p%ZCe%s)%wFrRc`d1eH8$M3=TR&3z1iFdW9W3)+plD0zl#;d~6^Pel?w zt3_O3o)T}g!Q>*1XJZstAM_QTNnAXl6xV#7AFo%At5R zbc}=v*`hM$a@&^qs6^L9EqCRtIK$0zb5_%nRcd<)%j5tT;_h=lE}`*RuTH^e1lL|P zeqZ&8#i?3DSd<1aQ5bw$C~}3du|Cz#uqS*BT7iKw2#Q> z8}$enhflGF!}1eweOp7Bw_%Q;2^WqR%XoRr9!*o~bW(1-$v)U&teiA*X)9sw>cO+Q z^tOchb()($4UJq+h?>JeAv{koWCYTcQJO`& z!Mm4+lp0f|-gLf?ah<3sTmMS7p|Q(U|o2g*p7q(tSM-% z+}KvgtJm*{zrX8-5X-A@#<>s&JbKadq7$oCgfF-I?y~sHLm;ID7`yK?!r$y8LWV1) z0)?C~PQ>`AN;4??L2o_Tt}!z2Bh}8J590rW;WX?1j2Q0?d`RSGdU#E zG4Sm%y*okc7bAHVRw+nX)*co}aKa4k!fO2rEJJ2cHZb`GBfwrJnj%$17sw((SI4Sz z_UfLrNin}ao-f@OCm^=0pzk#h~Vxn#@Tjo#DmNedvNf zW@x=!r#Yt0t4ia{CgDhhLj=0>eA0CoQG3|IEcT)IM0G11>TuxHv)AtZPe>uWB&^c< z-HCnMrfW++ME!&(2%PnHg9dI)vyC0cNz2_>6X0VMOZvi<{$+|}!5QVjcR@| zuq_TBm$RKS+x1I`D?+8np7ktYm?HaG;bRiaM>+3R#@c7D6r#hzF$$wedJXR)yJT+W z)Z#AazdHiy?lH@#^zJ_9)S{sMDf1E;=SCVwIC7O3!4KzcWZ8v;XcJ<`rTn{g6Wx%V z35Qi-!lbg`IK6!zl<|O$Uh5i*%<2r$b+ihv6Eg)=hZU4)T4L_#X;w(QzWW(0BxliR zq%&4q_P||ipLeGx=0|ECqkywe=&gFB3WLMyNE1I1hVML#icp!<+(ng9Ytyu|M(#u{ zw1Jk<3-7RbQxh@!;*$gv{qQ^$9cJ}S9?_srDa3onE8~_&+=W6ar)k;H}GpK(GKxPU`(l~1{+X) zWI#D`Ni+BT^Hmw7pSf|*DqHov_R)j+_%$h1p*3mDcI8|^p8U4`y<*SQ`F=< z7i7P|IpAD+D|(;wQksGdCpbu2kw6BInlTzSS%NrKuakT9l8|>gI)~ir>vWX|W4pL< zJ|4zb8Dac*t?vBiG`FsW-Na*uH$LH&imV$eMa)770HbAiNHjVUz7Me$hoB~eqYGOf zI#Es_YjIBZ@F&OBXnW?BjGg6FWD`TZZ}Ehv5az|!9p^j0qE>&7F!Vp77A!qJ@0XgB z)sq(B1M6?i)|QoknxQQjAUSu@9LB<8uM(Br0RScjdI-%Sp-hP$$7BRpH6Yi8qwRO?aJf{LJ^ zk^M)BT$)lI)8_+g^G_JQGZ{Ky(@bF?^G6b4W?bx~aet~Yk>Eb%NYrag@!b4+=(}%* z>P43yDpal2A+*UUc@$GEi-|?XKBd;-cL614zP@>6;?za9k4~;;FQnE=X8|3~<|-0k zcn74h>$8pauUuJ-!rQ!fJiQuuGJ!rb`PLtkz=&N*N!O!68wT@)olVo{wGP9DM(yPo z<#58Ih0gf+gUy_7r)I9{1I_)0r!ez_Y)I{$RC9kql0r%R0S9`bnN1pvo1{k581waL z!Yi|$csbaXdLv)HpPjVuk6Di0Mh3CVfl~LDCT1YZWSB`D|9cbojnUDoAurmI4R|{ zlAXfYXDvTY3*a)oTzHPzdJ~Ca zx4Obm6yNwC*o;!V6qz7dMPOr*CLur3(2&|NkYTJZ8oeY5MD@G1F5G0JDI$|Hreb?T z$e~~}|4vLhJuTOySV>x4idzew`^>`O^zwNVeH3c-Iz&HktM~gET*S)=vH;7=3eM&` zV?le{Bwgv~ark3Ikh=k|l1&rXbwCL0ZDD$$)x*-6Z18KE7!<$MjE-d2y8&i|Dn9l| z9fEi@33pG^;mlnvXEzk*za{Eo1<&frqF z+2s0_Y=sVYi?aFRnh{sZr;IU!qBmhn?{&jFDJ#R0RdI{z{gfS}%Bu00kUmN+X%fHF z9#x`Ig0=%p^PxCr^~XLc>b#nZN(~q;z_^ zoBg@1!fPtg`^Mu9-Ykf~$ZXOEr0Q%WC4daaZYd;_W*H_!eGGYe?Pt)2LD?&%7SP*L z@awKio-(El0kh^>>-)8~JISbziQ?0XsJNx30{4^l-d{lSNOn0R@?46lw6Gx6IVyYm|Ov+$G5N2L z+MRnXkD!q?&rAL=RjU)0R%j5*C}p&htvSf*x;_4jQ6@LX_TtSy3(4c{F&*c~w<-Tc znBeg#t0|1$Md-R{%~fRe8qcT1I zVxEs_FF3_|RalR(ZIKCLe{w=N8l?!kjfn4%G=v_o5CeFX8t-YZXu!hhps*NDGWzTW3 z;RxWQ!Xi`_KKw42G!2~b>EUp{=Gc0<*DL-3CnD9@TQGPf48LDR;O5rz*jc1@^V!Kh zMjqaH!oLu8Qa$xh&$`yY*yD*eAoxk3@8RV~S6>GL?^t~w4#fIr>IJz8IaK6rtoD&K zt5ZzkCkQrb_w+G1pVIZ&BfKz~!374RmshQYUBmA4)Nt74oIQPZTnw(3S@O4l;7SYi z8#@~2ahkE&R)J}m_gKDHqTM45;QBj6rTLHN$Mp>-+#uB(*8@`!9I|`@$&kdG7A^I> z2$j^GZ0$%mwS!morPt)`6sq8`1B{qEp^x576-%^>P}Os(t?h8r^Oj))2o=a_#nhUU z8qvokHTCP|V-&tdh?4D9`O$IOng)hPJWZ=_qtTSHh<%+&JCd}`3vEJ7T8|dnkPtUF zRs_sPu<`|CC&f;g!ZzMxlV*38NG@{Zws-E5YqGJR*}Z zUdia@kMb9dDcJh!(h_mmd7)x}l84(!IBD1=ihbD5rMV3=I_pG0&H>D;GTcQ{7(Vu( z4LYAfOT21q4(KM5ur;G~*67^xJfjmDrKy*uFr8KTqS<`B!(=)2piG-Z>cw~%xw(zl z)nrkxq@3L@q=@Uh>rT}e`B_V&PQKp0!DeyoO(myq%mLI@4mGr-8k~R?`6)uO-0>K2N%DJd z4uig?-A^v`nP|5IAV+Xj(F4n-2ZTKc@MH@ePoTaHDoSt8^k0|9ECr|+_ustG#3D2i zCd2O9etGN-3KE3I0E%IG*6aEfpnhwO5uQ0WJWns98l3iCzLoSE&jEjp=de6(rk(|U z6~->!+{BQ$`%{qK`wgZ&;M!l`6JS4ndo4OsE>K_VmTdaXFwWpgeyvvB1Kk+(Ecq^( zKus>)Q@GP6orn4M@j&gO-I3#q?eSYe|Ow&&#zxWcew z*CHB0hnL2;-MZeeyg-w<5A>?Z+&9e$n1LYO-3`PS2$GdxxZj?=eN8(F5&>D=Oug-w zYmnjfOSg;OfKDvAMVl*&%t@O062a@^v|rIPm;dmU>wg15{o~;O20mjM?voop4HdyZ z4#Q7(NTqbU$P7o%Q%j*WA~MFCtj3*6!nEUt+}z0OmL8=%dufsHn~>x!`kdVL8M?9$ zME-=>ACpvl5em{;rL4FS}BfdpTJ>#^OYU6^C#ErFTb2PibP3WT=)S z@D39Q9O2C4 zC}%11o`-T%I#8rZ9Lh-!R8s5e_>N0mBueUk`_w+L1{wD&7Q#Cx!h_lb-sT`~Zh2zf zCs3$Aeh7%BE5j!yA5{i%?FSw)125kydF?NL;n#%hPQe|VuuY~LPK2-E_JW%?Y>~B$ z$+^9Eg1MKM#=Wl2{@&KR6Q0|T1+0F3-HpW;+%5KT`}HjqPGgvQVc>|nIniH3G*=Dz z1{kVG8Dw3xuwy-ON{FRZt_qY8=UgYgJq>@!oOvcTo?Y@VoS6+Ow+mldIs(V{Wl^E! z;{G!4!BZZ!2n7WDuqvAOaizGf_SE&}O2n4f_`aan(BAdc4)EUi(;jD{3tFnYLpkUA zIOESBH*k&{uN&pu`6Z9^1K9s9I{y-{wcS5s2Bmh&v&VoqD`12)9ec>*~pE zY^Z9#?Z2Tyj7Q_PR%?e1F60cy3+f+;#AhWj>b|NTBadHJSxQb2&ly31jXNe(z!P;R zSN*CnFH{|fN2waL4nb75XFhRV=0YKA(L{pS7UO3; zn?8EROOkv#b(gmn2{f-Yo}PF#)ZcqEu>%MQoBP7!4`)*wPDY6Zj-dA!mfogk$LmKf zJ>HZE>D_e3O!r@ntlU-X@NLg-+^vZ+5?RJiheb^ADYx6Ck0}yu|Gks-rnSfLuc0!p z=YI;daCxm7|KwGJSrBXeXtFa;G)&JgbPD+D4)}Dx{abs;B4cJ$&#S%g*|G$d1P&K z;iVD^Z~X#Pdlv)cn=3et;wdWj^(7kupX+|zz1PH9c{m1AA1slZ! zwGmZs*vQjM&RSQ9Xf{0rGAp>H2Xvu?liyQmF1D)XMdo-<7O!r@$v%Rw2SUB>xM~5C zB_w6DT^Oe;U@>)XG*Cb8O;+4&{mCQogrQ+zas@1d z0zqqUCSFiFh9;N@?po)ua$+F8(8vZ#bt_+%v*ECYbmqV6<|c@_j`6nsSN0(??670mCCX7r?kciK>2ucokdy6qc&a%wr6N1_+Eoe>~Ae zuS1`Xtv;qvLaXbp5laAzN2ivvYs(e2$EK%W1~96;9ZatJMXk)yiA)lU*L$;iGs|w9 zyf(VkMUpOOj!o3Ir*5gDQ4?#-Vo4wd;aaz2qR_G4#fw)!T*OTPI=6OKi~Nr4;Eq$O&ugGz`dH;hYI zE~19;8f8nv_~?L>5i_#T<#A%-L3Est_K~J}V6yH+VD&Mn(muZ1afFo{sqMBY|LuHS zXPJ*LjxKi1s1jl*-TaSCzc zP~2A}BF&X}Vj6dsSH3!E5#@!q7v zGqG&Qj(h~2J~Etd#z$1Ij_b$$2}xz5f|drA>rbjM zpV`W48>bA2NNR3k?K4H+D>s!q(`L53)Q4qvfpirDr7>lmpnw~8rO@0+Obm!mI$G%I zkfsS9DH-_@oiy}V_Ynrfws|NEiM5+?|I~_uzPaH~HgfYJ%bw`{n>|st2)!0;Kr=(u zc@MdsCYL*p^VXSF-nEU{FMoo1!JcC0G{gn!^Gum1tdD=3FIa3nT4Ad=PximT(QV>@ zJWYQokorNu;QB>_Og3ipxFCJ>IN#3W6aQT(4+>DX9U=x8q#OGNY={gTl2zCqHXX0M zdk~b!-$m$k=1@;D$6Bq42OPl$y{9fsVzV%u+Tn*~G1zwC(-dG`sjgIgA6 zLtDII`Bhb@?+C7{LQ1QtQ7r0|f1(!k)}?7>5Cg@5Ek<#VJtdKLu(>4;1J)QX(_WPz z#tEIUb&;(ir86_XAGuHKCip1G%iNmO#B-)3*W$V^lCq)Eu^3mq$AwEsWIX{vD$q(U z@G7vbPD0W>p3^dzA8?6E<0pIr7BjATs-2lEv>veZI`+H}`UjvhWzC`%Py^7_ASDGl zp?d#LcgQkJKmsKbwE2fa^@e)6wzHcYhQkFLi^Rp$TsKyLIp3(Y-c8Ib%*AxNV76bb z+3z@@-wP|Zc&n)FcRCZXrm@K4GNq>KXXS@8AncHdFkD5K<8-PvU%= zA~c^&ccY7Z=>nQXV~!_kK6W z<9?))Yf_N5PT{Fq1$2AySin+{5}f3xjaF>*e2D4EHp`vniszFa@^1Lmg7j*_f`ZtM zXRS)RT+xMpVhBk*DmBWV=8hCU>)fBbRBv+k5{TG&J0M!v97l`MoKF zB|7oEvJ!{3DgW*fJVPp#Id4<33_l?QoiO!%FNLe}AjPKH`yQZ{VOHG)$!XCg0V7`7bkPK{2Kw~6s}j{NvYPO1bxf6 z6g6AO>-j@Gd7mJ};je=BtFc!pZXj9q2gE;13-{M(Xp3lrrL*p(&ukSGEh{2bxOhWQ zJL*jiwbuMkhYc)D5IC~GJTX(qcwUCbI#`#GU%u=U!j`5!KWOuOJ#R1&;Jn8w*}CjRq;jN4l$lKXH>y|5KF&Q9I94BWQtk{ zaMZ_TI-YTC^+?gkmh)Q@MbqO__k?gb5n+3?kB45t=qTpIDP_Ze zGKp8^1ghu=GL#e_3(en|pZK|9YB|h~sO$>-ynPagF*!{|SBuS{l{j?pMuh2`iB4)N z{3x6lDILK}9xt>eWfBkuKY?J2>{53^{YOqk1SIu_ZLJw-?%%1Zrbg}eWI-{Hs&Qt0 z!RPv(&j_}DBWNCUp2*X0lf{vQ zUWo($if9DleQ?z2^#*}?MM$=tLdcyfd$x$1Jks1S?qr9~z>11EjpZ`jY?y?H*kf?U zs&3X=n`fK9FrZMXJaL==8W&So;6R%(0(I0jQiT%XR~oZ8-TX86Lmw}sGyP9y%848j z&?5AY;GN+BsgA>eK$&F*JC-`rLIJ2Tj1(2-FTpn9@Hxck4j;v(7p{=$b*epf^7LGV z(BDOpVWBWx_j$dGt>!L!P|-f?sHe+*LCWx1X<$O$iqyI)?gwZD?#?1>bn@mPb)oet zx}kgolkXUqx}o6f>IYzQVCiT_`}_aGQ0Dl1%l=+{bN$ceDd)m;cGVX!J_`Q@!bv?) zPW{w*JUFFr199@4)ND@g`1zo+hY7b~^!BKY3FZWBgn8-KIEk5f9(MPC753%fQ1);8 zPa%{g3fb4lmXIcdC~Mi3osu;%kKGtUQDoouwQLb4`!+~O7!)GLR`zvbFm}JsOg+o@ z`@YBf`)7`E_~X9r>pZXXIkjT{aW>M2*$7XLN7IJIBeYI;Y(;abJIy-}KKn6k zbikNvC_j1_BBEgDLy_z&fc+Ym?B%*YCcTUOlT{UsMR6y65H0paNG{d>k>7s2w8QkG zuWUKUxA5M2EA^t^qo}K-QDIB(dzxozyH-|Pww+a9azWfrg))%OV(ni0G`jD(@=XTr z?RXC>rd(KS;Fj6;>C5UF=lF_amxf>5>5x&;ChJ(K3bRzTp>LiKftR+~Y>L&nY~>fP zit7(pCd57De38~a;HSo4oi<$Y%|m%JZ!LY8=Jb>wIcLYob&}RARITx6+HbU8DGYdy zYH>NfVqiS+Fv z*rg8}rP`HR8XTL@mtE)O}uY>m0px@E$$}d&Z$MGSo(CvhV zR4lYR|2gvTfN3ax`hL^#QfxSU_NnjrrZH-mzUr0ca#jBGg8NohM?b!gODrnXboajf zGC40l?9e-n|I@qS;74EZY_0rwjbW~wcV>x@_iU-z&dgKPPm>z-_)g&gU5Ls1n?p|G z;>U->!#R&I?5jsd4AM!g^qlhzY2T|I#cpSubI#h+VF`4FKA991E0K%!k6gA?T}CeR1113*E+JF#Q}is$U(}veoR} z-pt%JI~tcOpiy~!xYs#Qhk5POB-%i`{BnEYLWZwF-*@QDlVBdKGQj;@B130kZN@^+XmcM{$keL+-PEpa=FAGpe@rH znpoqf{i-9*eZi{|e1+z0@IODR6dRBWr#DNn@zcJqWcAEp<~4X6t}xt{_fG*&U%^+w zH2v_S_`4B#Iiho0P+smngl#5-nM4Rm!jg!MR!v$-g!Hzxe!15KMR8lI_%rY`rcNt_ z&m^24T76a9baB6$EwPicjW>3GCK9!ZaxNQo#!4#5X?+u%3h-q){Xmi(PAUW!nS-kC z*Dbv~SbcINen#tdhFEFVjdNP&oHTRC>(x)FIUs&G1)JkM?CgFmNlS)ljkRW0;jTD| zNsS76KEHfYc>n9Y`5}v%_Jh2p3Of5AG(yneuj9hU-uUT-It-0!hW?nAsKQum#?X_K zy6l@>0`Mwy;GVpX@R&%t4Ns=Z(F$FLjQlshc4g_ekYp|Kw=|up-h(KOEqwj-aCOM! zv|-EE+q$0cVd3r0Y2=ec!|gV9k`Gcl&GCI7 zHg@jQoX=oyiO)*8++lf7KpiNDJl*fN`_cjD2Iv8^|)_? zeRTXe0j=j7<1b4{+eF45- zpUs+Qy_?u$YCw5ZGZ+jJP*$}M7EAP+wI5l;mnYg3{u}4n({We%k_O5ibWlhk2)W27UhY(dwhm)WLLfid)e8E%WjsF!qexP zw%d4PP^*U<$tZeMwY=)1{mZadTQ`4R=bdX#XjI&lZ{D)Lt}HJEO5M-y#_0GBmG*;N z^Q2aX&NHsFcT$+143{4Vi1<883iolScoJIBqu~F+PpqA}wx0Tm)x_Y)5N_ik3jMxQ z+k@256N%R1*T%Q_S}w!2h*VE7Y{lVh@8J(Ap=-}%lua%}7y z4#eL~z`}AMzRYvV+wZITV(o<})UYxC51+&|hz(4~vVOw1ws~?a`WoV(6qz%^EcfvjcY7M6L^~L26^$V#M>s18`)*tUv7EQzC-ONU7gjW;C{|` zLLJXnD{)3WE*CTamCK-OPEfAFk91Jg+a+hpXG+4#uA@Xn_pYm=Yub6=Tpn2EC)B7v`r3c!Q$;nw+f_TPV>F(sOzHpqy7rK z0((@c5O$v4?t<;ElZ{>rZM4%>!oHCerZ_Ukd$+{j@<7EUPoxx_MeUG3)}0$fs?CD@Vwz0P%OlCWm?P? zuNv>YdM|ipvf7W^u6U~#4z-Bu8Q-hbwr){yp2=*$k1`lD-ryA4ziM^#b*j;VJa%Uv zdAWO1{>;+IXx|xC+V19>WV~+bv$vQ(_~x)<(O&TFuAM)J{xC)Ri{NO@dK&vM^GAYd zRnsq0lwZ%1IPA6w1y7i6M3I;jY`c+qG=Gmzr|e!Q-`#1f4fjV0$xcf4m2|`kD(Lt5 zz|YF{HeDAI6H^fJ-#ci~@#)^eNgQwSN*tA(`9g7oZI00-lhjS7;qkJD`!<;Oo%3pw z1vl2G#$HrPmqq&x7Aa&74+J{F z)&)Zp?^5yo!t|^jE7xKssiAG%2R`Z$vbiFXjap+<`3@T9=B0Bm*OsNJW7gRaqqa!P z*8>&EL~%`3B^SK?yVV>df?iG=bAnkG>a???di0G~yQI=G=Q@c1l(u{4c$`0~_Y3~Q z3NBrAV`t+)W`erA!Dh2KTf}1Y(xXIwKJCTIjW7?H-JNyOhF1J>n6b(IIq8Hc*sG-X7cJl$UD+bXgWORIxZ`;f*k-d=hs193-NzGxQ9*r;_f4lf)fY|CVjeqdQL2q~JxLc->+2jU!b=q|>fUa)tg!@W`B74|g5p#Eh7} zR&dxM5$845)sfg}-8~uq2~PEmg#8(ak4P^@lzOgAoEp=7dU;7@Off$@bIWCKYz~nu z5rX%!!eKVjcGs7e_bpJ`EI+32rVN&=el)^I9SvZLF?SEW?7j`GfC_r~4QSr}2Rkm;L9M4AgX`F>i=J>FpBb$Gaf9#A7V=kZ!}F3^ z!M~ZIbc%*=JZ`E>#4tIRyX-yNgHr|*u}|Y%FIlXiu6WI&3$g-N<67%i$?vblC3&Ss zk9S<|c6DziKk}Z6T{-G_=Np;d(bY87S5s5??^sFYrdc2ub(GE3VB zObaMuJMd>p@5D>kRe}x~iX(I8?)73vDpbMyRy0T6Jq#?iXf(NBVsg>R$*J6Rwky=Z z$!XQ_ezDPdTy|dGtUwER?D+Ef9h#648%Q?;zr!1* zXX-H7&uoc>P-Fcf$EHJ-NdKLftJ}->}23x888MMIV}g(?7`9D zz72Rr;j3JK@IYi6{Mz`&HU?^lE;Vo1W5|eYZXRhuz?*X<*)}1BXE-+xjE|3F?dRi< zY?p^h6e#QK>;34TjFjh1ez~Z=m!_F^?U3(Vtqbq^Z96*wZak?>shY>ltl(UkZ0sQ4 zuTmZ<=M&xHxgrR4`~$Axk+Fd|x(54!3|_Amd@xQQNRW+|$`}dI5bl=gpe3o|gsNY9 zsSC|Tq-*mKPQ%T`RD-}3-nl!Pc2JcbR&k{M$lK(}1~2HP`FrSQT(2xcpmn3R=i1l^ zmdhB+%GX;#5xdo-SUv$!8?vM0TSf%7Nf*+*3B1XusQ2P*KNFu9B5*s$AF2++6V&lal zK+NFJJ|0Z9)3O)g%bUQbP#uycX!H}V+#yAY=s~eJk(7lVpS>Q_N4skW2vVN0K#fW6X&=6QVBW{m5kEy0Qxn?*8V#Pts+}q?~U7URhjCX!D2A2>3 zaz+S;r4GwFsQ4b_YX4NH&*VQD1Q^nsG$!@X_(>hhn=8ZhP+6@cFw#b^IUAe$-yBXe zH>zoynl|F{yW*4!prq0lIgg`i%?%a(VeT6OlYC4}m8AwcO&2Fbu<*8nDA45dK;c95 zM+q70H4Kfc#S$a>8@&BfJ&aR-2N!}92}q>*#6tvCh@#LVnu_o1i3CWY<>z_%&OqXH zMsV$eVefNMJ(r73t$wNT#JLCFghj!3~e5J*aS^A}f{1}Qu^D7`Z zbE}Ih3`(pH>>^%j4SL{iq8I|re^>;rIweCWtsek`rHn& z@dZcy7vn~0@UUJJCA>EN-0E?o4lP!0nZU4Nz_9Gq9Ad(RekDwJ+9~!W%(NF{HSq~> z-Xmkz%F3dk!mAx6o6%_OXrbDhwz$&s29&c5A*qq+08^oP;B5t0g~>X7va%O9e^cuh zv;_M9W*U)TdVN@~rhh?NV=?xQ3i`@3lE36UGt_;STr_!rn- z%wJ1d%UTl{J%+y;nuHHM1a;f@zqWe=Q$neN53*9w_t)n(Hya zZJlIM2Iac^uX6oGK^Lthra?unFfLuiB__UN0QKjxyGu-)=F_Z$F;^&ww$G&Wih5Yr z752iS0kvV4dP{j5bJ1peaclj>Qc zR2K~12&zI@S#=CAK&XR=F+BAW*A$2i5omknC-FO>M*hxVNQSZwiF_fAS`h$!NHHte z`hjA<9OYL=9DxNa|FwtsdKRbB%PRg$yas@HiCqMU)f7|!=4m@XReA|kG4DYL#2nhB z2da{iPy;`1gb}NefFOcVk)6~xP=;dXlm}qj64%WllBM{YAU&D?q(}cPL2*N2z#WrN z{^vT3yCfOnI&s$yfB_#w(cwa4KplDjEtvDrfM0zK{sW>*6I}I}LCNJc%DFb_xwj$w z17yf|2%?Z3vk#arPhfi!F^*sHlv6ClgZdQY4&#yr$22BKaKtV>ZPJf>3X1X493}U2 zDn25&FB7?antme)PKVQw7KhG&>d)v0{k3ukCHJ`LTm1H#4+tnC9eEz)97q#UDN{xN zm2v`Ufn3qg11U1r1}AQa+=2k5<+pKcUAS9nJkb`iK;9E$06|=TnC02G^$aWWYG4bGZ5;l*P=b)daf~ zNP=8OBePUgex;!_(uxC|JSkrvBA~4f8hyQw$G-ZHNn*Z1-U$`^`N;_&#%Dp9r*(Qa@QXG=ZTNKB~+XFuY6y9Y%vQ?`ilu}HowwK^|#>a z%>VIQ^VL0W5-8RNV^pjlv!)wVd}=^GDEMqpOzj4p(q2F>CqW--_*TZgkm)Wya~7x~ zUoMh=AoulhaoV%I{tdhTJhZ_rG^q9%z&ehI)S7nRGSGSe(*Usu^roLTCjDF6(| zePZ<`nl(S$#WyfRG+?rmG?I7zPA80A8W{kjRPy(ce|6sq3PO4RRni1r#cmk`sRyWD z1s&#JRsH`vE{2Z*SDIeaj*7LGf%DDx#BWz8L%@MS({r^D*T)#1BruBZ=+Xn_~?CLoZ?p!goB^$cU2}*U6z0$&a(JMNaik8TS?jhd$6qy@ANQj$zs%c@O zRL}sacpSQ`N9Y$dv#6B*2}T^avB1g|NQNnax%F6W0&|J&m(ZbIGKuPtD*CT{Mw^gR zUIV0+;pd}X*cYW#^SrdEtX@!T{O!`YAlewkz*C*szlYed^6Qe@ufnoJ>H zR!JO(Ol@sSv48Bb6qbdc<0z?dLf@jg?0slfZuwezEmJY$r>3U1xh3{S$jX}p{LHi) zMEpE+4-T59#PNrBKQ_L*Ez^Ube53Z*$tmH*KxQHx?d^To@z0K2myk8bhcFtyKXb44 zLKD_@TrzMSjHrT_KRkkJkUTvR6857KzFD{MMJW>8-p%@H8JMuI-9t7Khe<7~csY@Q zG+8&yGW@Yw;<2n?;gb5~aCXa2kIgM|Y8GLf><>&peL5$b0zTN|+HIBatjnUNn@#;5 zBhVdra=1*cjdNf)O_WUm&AA^X&eud@n6;1Yo^oU-o!?%to9hJcLYnaykSM{5!~6K; zphIHOA9eRqoT~%<1|OL5uU6I<^saekAG>O%(^xRFPA{ZdJ9TVsPsD zpru94pk(tJ^_~EY--Vw5mgKJ{_P4SKwt!AbD0E)ZUmupy-W$}h*3WZKup1haW>gTy zhZ?>I>oFGF)rSw0sr+2?E1h22e66l3w>4MITJ4Ivk2ZBjC%R~(*C3RWObJfZ<=5k1 z(_j+Srb!;z9Mw-9%Vm_?t{E|}2haU%DpjW5`0N&zd&kDA&%}5K9c}OfQk9H#NMvp$ z@2*Ws&2uw#H^m3tH*QuxMq*p&!_rc1hR(q#jgeMhR&4@HC9j`1kbzLG$I& z=}7jxWeG`lk!WLF+BB}nqQ8*M4jU4;uzHo+c69RRmsOa&{=$QDt%2lac){CiXf;`4Cr(YGeG2f zK&=obk+@QbCFpE3u%Vm(o`-%FMV87~=k7k~A-!zibSAm?WG4(Jo8%r#` z6WG+J6jSnA%FpZzPV5cNvL`{dcSV4la!^c=2|sIjtEu34e=rk6QV9 zIp5ywl{6E(Ps#8LhnbZXK@nN?i>61g*sl?KI?ac$iQ+8LN2lerO>KISH#>52F$r8% z$+Ys&IW#--e*|UG) zN)UMxf=vL0g%}gYb`sa@b6oKth= zB2uH3ZXoe?&O?Z3Y+-AGn*NQh23qyD14L`Ci;GC8VNdiI+Jn3N&1_NEXttfk5G{_& z$2Da8;=WH>)E@JSVJz7A0|i7c`DR2Nk?vW*8J&e*|MdAwTMB zcNfojE8ezqYR!tYLYY|rw zzpihx^G!Pud$OLDeCM0pT;KwD!B7Dvi=KsD4c)TItnA(s|G5PD&!Q);S{GR>IsYLC zf6){|Rwl2|#mG6nEHqCob#Y;~MWV5Nd?FDgggmWkk(fRt9g-||_S2q;zO{qarfIo| zx3;YYPQFh+UTbmb<*dDSL9wNTvIV#eV#*c-S6G9p>54d%6V^NLTgNu8UXuhsUS1Ov#QA`O9Lg4D2>$2jASj zfLjL9rr2hu`fz>Sdigi|c1-HsoSBKzJvi$Pth@V;M9NVc`kgcQC5xu=e7uI6+t?yK zX^w#v(@;$R$kki?Q7rV4L79`b)-C|g0q!%bXc5LQ(kzsA2;VZC_VR&vN*3!X$GgWt z;TOJss|kGSv1X5uGvFFt?K^?cnn$!u7m%75HmCE8w##78w^)w5ccs3FkXUQs-oObI z+|gg#&h>bd1XIX@Ou4c9dGd{yk}`zsPSx7{V~RbZ!)pWu=DIs~`3UQdoT|Am@!R$$ zaH4%IsQx|p5a&F?Tm^=V0@BHBEy~{;m@Q!Sfo=QCA~JF^yezWfol6*6l;AlJkx||r zC_h_bli8Yx+VjuouaAEHK>#3CJLxBSu~}P)bbOe7M&8TVebeJ;_cYlf_pGjRDY1xn z7h9qA%&PLr^@m`o7TjMv^K$n9-sHbCAdw^3EM^p6KbtJDzU?{w#E3`oqXL_D2uDm( zOYij4^CDWC6D!jN28|2bXIO)t-{C+T)30P{4RfF}N17I;8}wCSLb1)&S81JD*(2Xv z_FStCsBt$RUATm^SE1Nv6ZJUyE`znGQL~kk)Kdw69Bn*TRa~#!aqHEJn6H-a!TE=d zKfTAj8Y*ro?d3Svo7i>LlCxO5tl`8g^YfPS4R*cdpzp82MxZh74fF>5a}EDxNIJcf zT!G;iheoA>vDY`UE@y6BaP>ES8=*S6jN!)`FN6KH;pD@)mXZJFJEEm!4QSrlw zeb7B(_fmQl&kVUbkD*0{GVSAkrYhEwzSrjp( z-IrlcoN;oF-*|bZYlZ>)bMF$t4u~?CU2cv<=`&4H8Mgjq26QS z#qt}b!6aF$rk~R1!gk@&8yfy!H7&{okZA{x_I6Q=IM|5S45DaN<0iN?N7cG>ns_%& z;BC|$Ftc}qv00c|%F-g4{cno&PtD>w5;f=T^HlyOeIteIh> zPA@}oL^0-xv9NjjZ3L&{%-GPppCPG9`ICW@DNjmXtTu6eO1$r2I!qTLZl2_)7#MzK z;BKJQGN+GX!RAomQShhh6}<@U;L08eJej`$S4r|PB1PUe`M}T5t<=oRnYv3#v!|7fvOqqSAks`cF2ic z4ZZc>$gzm~gR2+j(qPS?%?ltS?pOZh7uZPwdc=tI)ctIIS(jS2J~d)EA=-DG2bKAq+wpw$;pR zr2}JnKiL?+p2xLJj+aQbdNkzVCH45l!t>LhaF^t5g*wQ#j|^RqI-eB2)9f~JX`>2@k}`HeuR z9!c0GOc+{R@Il3nHf(=fo1uK^417pq;EYS-1YVLEZ{_Ap>OWkr z!hCr1XuCRzv%7n*rbw7Vf#FO%PQy`T3HhXf5N> z3#iD&Kk09D{1BuiM=IR@h}MMUD79}uR#4*CE%4^Y)yr>gr5^EGMg?-R2}Z2Dq~>0B z`ry$q?pC?5OWt-+KO*ta`^eY9>^wHuZDOP&sx+key1?L_klpODEM^|wfa#VoJLsdx zl9OGb^EdpK^3CVwdzbC&_l!mD8~VRlO?1HYH*t4I-l^r!Efwh|rn+^@4Tm}rt^rEyFs7`~CUL|#JWrqVM-JxLM znygcUuPN#;@n8QyGQ7B{f@gLQ`xq1snun_xR-lgUExZCP87{eJ|GrixF9Ht0(8 zl;fKP>x4Y8xX{E{0Y$61_j>#8;M(E`!tDs^#A|8FicHxQc%`AH&-YfsrubYZ%*-&Z z6PuBmJo8(xEyP_XD0~jNACSuJwdlOf=2S5zJzPNZyrJU`%T{?M62P)cp-%Gnxd7-O zOO%_3Dn%sqN8lR8VNfT}0>pt)(TU2Kw2Jwwkjf`kLFnjwcswL$##|7Y8?#ZJ?3IVI zs!< z;_)M}bnAI@zw8CdAxTn6J|Z4sC0`hC*rg65tFn20O`DZ~mpTi9XhJmgXFU{|D%Qx5 z2=OPEqn)Iomvha*2MT+ymsJtkz3X%0@I5%yf3O0pQ5HrZDUWwRPV^9;?XG|TJeWZM zcrg0^m6Nvcn@IAz`)PC=c%IpTm#~JOFV11^7{PJRyMq(I(IMy>T8_*-e6*IS;uQY% za1&RxSUfm-7-1`9_9iI~P#xriadYbr!Y=PHx|?UA!vkdDBFMri$Yk~@1HERhk(NgzT$?P&1vH`8@XtVSpM)d<}#0pL=D;Tqpo9}Ef^m31OwqDjD{t|0r2 z9+SK7+iy%w+sb7>LSYv}(jiO>U@bvehiclesMessagesTotal); + sbufWriteU32(dst, getAdsbStatus()->heartbeatMessagesTotal); for(uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++){ @@ -977,6 +979,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #else sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); + sbufWriteU32(dst, 0); + sbufWriteU32(dst, 0); #endif break; case MSP_DEBUG: diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c index 573112c2df7..260f89fa6af 100644 --- a/src/main/io/adsb.c +++ b/src/main/io/adsb.c @@ -131,6 +131,11 @@ void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t dest *bearing = wrap_36000(*bearing); }; +bool adsbHeartbeat(void){ + adsbVehiclesStatus.heartbeatMessagesTotal++; + return true; +} + void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { // no valid lat lon or altitude @@ -139,6 +144,7 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { } adsbVehiclesStatus.vehiclesMessagesTotal++; + adsbVehicle_t *vehicle = NULL; vehicle = findVehicleByIcao(vehicleValuesLocal->icao); diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h index 86eefd8aac1..77f2e31fa47 100644 --- a/src/main/io/adsb.h +++ b/src/main/io/adsb.h @@ -54,9 +54,11 @@ typedef struct { typedef struct { uint32_t vehiclesMessagesTotal; + uint32_t heartbeatMessagesTotal; } adsbVehicleStatus_t; void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal); +bool adsbHeartbeat(void); adsbVehicle_t * findVehicleClosest(void); adsbVehicle_t * findVehicle(uint8_t index); uint8_t getActiveVehiclesCount(void); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d682c0299ff..fff22479f06 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2136,7 +2136,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_ADSB_INFO: { buff[0] = SYM_ADSB; - if(getAdsbStatus()->vehiclesMessagesTotal > 0){ + if(getAdsbStatus()->vehiclesMessagesTotal > 0 || getAdsbStatus()->heartbeatMessagesTotal > 0){ tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount()); }else{ buff[1] = '-'; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c8f5600d6e6..4e8d54535a3 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1134,6 +1134,22 @@ static bool handleIncoming_RADIO_STATUS(void) { return true; } +static bool handleIncoming_HEARTBEAT(void) { + mavlink_heartbeat_t msg; + mavlink_msg_heartbeat_decode(&mavRecvMsg, &msg); + + switch (msg.type) { +#ifdef USE_ADSB + case MAV_TYPE_ADSB: + return adsbHeartbeat(); +#endif + default: + break; + } + + return false; +} + #ifdef USE_ADSB static bool handleIncoming_ADSB_VEHICLE(void) { mavlink_adsb_vehicle_t msg; @@ -1188,7 +1204,7 @@ static bool processMAVLinkIncomingTelemetry(void) if (result == MAVLINK_FRAMING_OK) { switch (mavRecvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: - break; + return handleIncoming_HEARTBEAT(); case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: return handleIncoming_PARAM_REQUEST_LIST(); case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: From b32d14d701c9a670039b2b27b20b7a73a344d330 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 10 Aug 2024 18:25:32 +0100 Subject: [PATCH 12/45] smooth xt error rate --- src/main/navigation/navigation_fixedwing.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0c49ad4fda7..3f814513edf 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -408,13 +408,14 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) { + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) { static float crossTrackErrorRate; static timeUs_t previousCrossTrackErrorUpdateTime; + static float previousCrossTrackError = 0.0f; - if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10)) { - static float previousCrossTrackError = 0.0f; - crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { + const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + crossTrackErrorRate = 0.5 * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec)); previousCrossTrackErrorUpdateTime = currentTimeUs; previousCrossTrackError = navCrossTrackError; } @@ -424,7 +425,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f)); /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */ - float limit = constrainf((crossTrackErrorRate > 0.0f ? crossTrackErrorRate : 0.0f) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); + float limit = constrainf(fabsf(crossTrackErrorRate) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); float rateFactor = limit; if (crossTrackErrorRate > 0.0f) { float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f); From 3efc75e825b443bd3810bb3dd62f04532b544d93 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 10 Aug 2024 21:52:54 +0100 Subject: [PATCH 13/45] Update navigation_fixedwing.c --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 3f814513edf..751c0698b38 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -415,7 +415,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - crossTrackErrorRate = 0.5 * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec)); + crossTrackErrorRate = 0.5f * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec)); previousCrossTrackErrorUpdateTime = currentTimeUs; previousCrossTrackError = navCrossTrackError; } From f97a1020cc4183902cb2c28470d92806d01e4d24 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Wed, 14 Aug 2024 13:00:18 +0100 Subject: [PATCH 14/45] not working --- src/main/blackbox/blackbox.c | 32 ++++++++++++++++---------------- src/main/blackbox/blackbox_io.h | 4 ++-- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 1222af19cd8..f719bfc6967 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -353,24 +353,24 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_16)}, {"servo", 16, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_17)}, {"servo", 17, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)}, - {"servo", 18, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_18)}, - {"servo", 19, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_19)}, - {"servo", 20, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_20)}, - {"servo", 21, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_21)}, - {"servo", 22, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_22)}, - {"servo", 23, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_23)}, - {"servo", 24, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_24)}, - {"servo", 25, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_25)}, + {"servo", 18, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_19)}, + {"servo", 19, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_20)}, + {"servo", 20, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_21)}, + {"servo", 21, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_22)}, + {"servo", 22, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_23)}, + {"servo", 23, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_24)}, + {"servo", 24, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_25)}, + {"servo", 25, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_26)}, /* - {"servo", 26, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_26)}, - {"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_27)}, + {"servo", 26, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_27)}, {"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_28)}, - {"servo", 28, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_29)}, - {"servo", 29, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_30)}, - {"servo", 30, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_31)}, - {"servo", 31, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_32)}, - {"servo", 32, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_33)}, - {"servo", 33, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_34)}, + {"servo", 27, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_29)}, + {"servo", 28, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_30)}, + {"servo", 29, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_31)}, + {"servo", 30, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_32)}, + {"servo", 31, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_33)}, + {"servo", 32, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_34)}, + {"servo", 33, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_SERVOS_35)}, */ {"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)}, diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 76b03b5cd6a..e4eda947d7c 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -48,13 +48,13 @@ typedef enum { * We want to limit how bursty our writes to the device are. Note that this will also restrict the maximum size of a * header write we can make: */ -#define BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET 256 +#define BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET 512 /* * Ideally, each iteration in which we are logging headers would write a similar amount of data to the device as a * regular logging iteration. This way we won't hog the CPU by making a gigantic write: */ -#define BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION 64 +#define BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION 128 extern int32_t blackboxHeaderBudget; From 376346d8f7ca6f761c5f566942bcf70a41043b4c Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Wed, 14 Aug 2024 13:19:30 +0100 Subject: [PATCH 15/45] fix typo and bug in servo logging --- src/main/blackbox/blackbox.c | 2 +- src/main/blackbox/blackbox_io.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f719bfc6967..4341ea0c34d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -712,7 +712,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition) case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_33: case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_34: */ - return ((FlightLogFieldCondition)MIN(getServoCount(), 26) >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS); + return ((FlightLogFieldCondition)getServoCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_SERVOS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_SERVOS); case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0: case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1: diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index e4eda947d7c..76b03b5cd6a 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -48,13 +48,13 @@ typedef enum { * We want to limit how bursty our writes to the device are. Note that this will also restrict the maximum size of a * header write we can make: */ -#define BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET 512 +#define BLACKBOX_MAX_ACCUMULATED_HEADER_BUDGET 256 /* * Ideally, each iteration in which we are logging headers would write a similar amount of data to the device as a * regular logging iteration. This way we won't hog the CPU by making a gigantic write: */ -#define BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION 128 +#define BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION 64 extern int32_t blackboxHeaderBudget; From 858210e14a2ba4dca1ff7aa64aeb39274948693c Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 15 Aug 2024 13:43:46 +0100 Subject: [PATCH 16/45] remove excessive `SD` debug from MSP processing --- src/main/fc/fc_msp.c | 11 ++++------- src/main/msp/msp_serial.c | 5 ----- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c8ae27f270d..b5715483f73 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1034,7 +1034,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_MIXER: sbufWriteU8(dst, 3); // mixerMode no longer supported, send 3 (QuadX) as fallback break; - + case MSP_RX_CONFIG: sbufWriteU8(dst, rxConfig()->serialrx_provider); @@ -1274,7 +1274,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff); sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_stage2_lowpass_hz - break; + break; case MSP_PID_ADVANCED: sbufWriteU16(dst, 0); // pidProfile()->rollPitchItermIgnoreRate @@ -1618,7 +1618,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } } break; - + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE @@ -2892,7 +2892,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; - + case MSP_SET_FAILSAFE_CONFIG: if (dataSize == 20) { failsafeConfigMutable()->failsafe_delay = sbufReadU8(src); @@ -3291,11 +3291,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #endif case MSP2_INAV_GPS_UBLOX_COMMAND: if(dataSize < 8 || !isGpsUblox()) { - SD(fprintf(stderr, "[GPS] Not ublox!\n")); return MSP_RESULT_ERROR; } - SD(fprintf(stderr, "[GPS] Sending ubx command: %i!\n", dataSize)); gpsUbloxSendCommand(src->ptr, dataSize, 0); break; @@ -4145,7 +4143,6 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro // initialize reply by default reply->cmd = cmd->cmd; - SD(fprintf(stderr, "[MSP] CommandId: 0x%04x bytes: %i!\n", cmdMSP, sbufBytesRemaining(src))); if (MSP2_IS_SENSOR_MESSAGE(cmdMSP)) { ret = mspProcessSensorCommand(cmdMSP, src); } else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) { diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 0dbc1768b8a..f72ea35da11 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -173,10 +173,8 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) case MSP_CHECKSUM_V1: if (mspPort->checksum1 == c) { mspPort->c_state = MSP_COMMAND_RECEIVED; - SD(fprintf(stderr, "[MSPV1] Command received\n")); } else { mspPort->c_state = MSP_IDLE; - SD(fprintf(stderr, "[MSPV1] Checksum error!\n")); } break; @@ -229,7 +227,6 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) // Check for potential buffer overflow if (hdrv2->size > MSP_PORT_INBUF_SIZE) { mspPort->c_state = MSP_IDLE; - SD(fprintf(stderr, "[MSPV2] Potential buffer overflow!\n")); } else { mspPort->dataSize = hdrv2->size; @@ -253,9 +250,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c) case MSP_CHECKSUM_V2_NATIVE: if (mspPort->checksum2 == c) { mspPort->c_state = MSP_COMMAND_RECEIVED; - SD(fprintf(stderr, "[MSPV2] command received!\n")); } else { - SD(fprintf(stderr, "[MSPV2] Checksum error!\n")); mspPort->c_state = MSP_IDLE; } break; From d7142fc4dab3a70ba442d11ae42fb69afee8e006 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 18 Aug 2024 23:01:12 +0100 Subject: [PATCH 17/45] improved method, setting updated --- docs/Settings.md | 4 +- src/main/fc/settings.yaml | 9 ++- src/main/navigation/navigation_fixedwing.c | 80 +++++++++++++--------- 3 files changed, 55 insertions(+), 38 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..c5965d13361 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3524,11 +3524,11 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_wp_tracking_accuracy -Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable. +Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 10 | +| OFF | OFF | ON | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c16ce93b21..d3aa1f481fc 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", "SBUS2"] - name: aux_operator values: ["OR", "AND"] @@ -2545,11 +2545,10 @@ groups: min: 1 max: 9 - name: nav_fw_wp_tracking_accuracy - description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Settings 1 to 10 adjust the course tracking response. Higher values dampen the response reducing possible overshoot. A value of 5 is a good starting point. Set to 0 to disable." - default_value: 0 + description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible." + default_value: OFF field: fw.wp_tracking_accuracy - min: 0 - max: 10 + type: bool - name: nav_fw_wp_tracking_max_angle description: "Sets the maximum allowed alignment convergence angle to the waypoint course line when nav_fw_wp_tracking_accuracy is active [degrees]. Lower values result in smoother alignment with the course line but will take more distance until this is achieved." default_value: 60 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 751c0698b38..4d8215c688d 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -399,42 +399,60 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - /* If waypoint tracking enabled quickly force craft toward waypoint course line and closely track along it */ + // Calculate cross track error + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); + + /* If waypoint tracking enabled force craft toward and ckosely track along waypoint course line */ if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - fpVector3_t virtualCoursePoint; - virtualCoursePoint.x = posControl.activeWaypoint.pos.x - - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - virtualCoursePoint.y = posControl.activeWaypoint.pos.y - - posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200.0f) { - static float crossTrackErrorRate; - static timeUs_t previousCrossTrackErrorUpdateTime; - static float previousCrossTrackError = 0.0f; - - if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(10) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { - const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - crossTrackErrorRate = 0.5f * (crossTrackErrorRate + ((previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec)); - previousCrossTrackErrorUpdateTime = currentTimeUs; - previousCrossTrackError = navCrossTrackError; - } + static float crossTrackErrorRate; + static timeUs_t previousCrossTrackErrorUpdateTime; + static float previousCrossTrackError = 0.0f; + static pt1Filter_t fwCrossTrackErrorRateFilterState; - /* Apply basic adjustment to factor up virtualTargetBearing error based on navCrossTrackError */ - float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); - adjustmentFactor *= 1.0f + sq(navCrossTrackError / (navConfig()->fw.wp_tracking_accuracy * 500.0f)); - - /* Apply additional fine adjustment based on speed of convergence to achieve a convergence speed of around 2 m/s */ - float limit = constrainf(fabsf(crossTrackErrorRate) + navCrossTrackError / 3.0f, 200.0f, posControl.actualState.velXY / 4.0f); - float rateFactor = limit; - if (crossTrackErrorRate > 0.0f) { - float timeFactor = constrainf(navCrossTrackError / 100.0f, 10.0f, 30.0f); - rateFactor = constrainf(scaleRangef(navCrossTrackError / crossTrackErrorRate, 0.0f, timeFactor, -limit, limit), -limit, limit); + if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { + const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + + if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { + crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; } + crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); + + previousCrossTrackErrorUpdateTime = currentTimeUs; + previousCrossTrackError = navCrossTrackError; + } + + /* Dynamic tracking deadband set at minimum 2m for baseline speed of 60 km/h */ + float trackingDeadband = 200.0f * MAX(posControl.actualState.velXY / 1700.0f, 1.0f); - /* Determine final adjusted virtualTargetBearing */ + /* cutTurnActive improves convergence with course line when WP turn smoothing CUT used */ + static bool cutTurnActive = false; + if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { + cutTurnActive = true; + } + + if (cutTurnActive && !posControl.flags.wpTurnSmoothingActive) { + virtualTargetBearing = wrap_36000(virtualTargetBearing - wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing)); + cutTurnActive = ABS(wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog)) > 500; + } else if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && + navCrossTrackError > trackingDeadband) { + float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); - adjustmentFactor = constrainf(adjustmentFactor + rateFactor * SIGN(adjustmentFactor), -angleLimit, angleLimit); + + /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile + * Adjustment limited to navCrossTrackError as course line approached to avoid instability */ + float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); + float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); + float limit = MIN(angleLimit, navCrossTrackError); + + adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); + + /* Calculate final adjusted virtualTargetBearing */ virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } From 601945e00dc6b756a6edb2e6d21391532e125c96 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 18 Aug 2024 17:54:04 -0500 Subject: [PATCH 18/45] MAMBAF722_2022B: use DMAR for motor 7 --- src/main/target/MAMBAF722_2022A/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h index 4ab5a75bbc8..cf834b11ea6 100644 --- a/src/main/target/MAMBAF722_2022A/target.h +++ b/src/main/target/MAMBAF722_2022A/target.h @@ -23,6 +23,7 @@ #define TARGET_BOARD_IDENTIFIER "M72B" #define USBD_PRODUCT_STRING "MAMBAF722_2022B" +#define USE_DSHOT_DMAR #else From 42a125994cf7f454a89151daa18447d0fc60472e Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 18 Aug 2024 20:11:59 -0500 Subject: [PATCH 19/45] GEPRC_F722_AIO add target with UART3 instead of i2c --- src/main/target/GEPRC_F722_AIO/CMakeLists.txt | 1 + src/main/target/GEPRC_F722_AIO/target.h | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/main/target/GEPRC_F722_AIO/CMakeLists.txt b/src/main/target/GEPRC_F722_AIO/CMakeLists.txt index 51e664a2637..9116889a26e 100644 --- a/src/main/target/GEPRC_F722_AIO/CMakeLists.txt +++ b/src/main/target/GEPRC_F722_AIO/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(GEPRC_F722_AIO) +target_stm32f722xe(GEPRC_F722_AIO_UART3) diff --git a/src/main/target/GEPRC_F722_AIO/target.h b/src/main/target/GEPRC_F722_AIO/target.h index 5dede599778..3b2ff13f0ac 100644 --- a/src/main/target/GEPRC_F722_AIO/target.h +++ b/src/main/target/GEPRC_F722_AIO/target.h @@ -17,7 +17,11 @@ #pragma once +#ifdef GEPRC_F722_AIO_UART3 +#define TARGET_BOARD_IDENTIFIER "GEP3" +#else #define TARGET_BOARD_IDENTIFIER "GEPR" +#endif #define USBD_PRODUCT_STRING "GEPRC_F722_AIO" @@ -53,6 +57,8 @@ // *************** I2C/Baro/Mag ********************* #define USE_I2C + +#ifndef GEPRC_F722_AIO_UART3 #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 #define I2C2_SDA PB11 @@ -74,6 +80,7 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 #define BNO055_I2C_BUS BUS_I2C2 +#endif // *************** FLASH ************************** #define M25P16_CS_PIN PB9 @@ -108,9 +115,11 @@ #define UART2_RX_PIN PA3 #define UART2_TX_PIN PA2 +#ifdef GEPRC_F722_AIO_UART3 #define USE_UART3 #define UART3_RX_PIN PB11 #define UART3_TX_PIN PB10 +#endif #define USE_UART4 #define UART4_RX_PIN PC11 @@ -120,7 +129,11 @@ #define UART5_RX_PIN PD2 #define UART5_TX_PIN PC12 +#ifdef GEPRC_F722_AIO_UART3 #define SERIAL_PORT_COUNT 6 +#else +#define SERIAL_PORT_COUNT 5 +#endif #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS From 3c88f0452e0d600f1ebdbe5a1b11e4e2c3e12ffc Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 8 Aug 2024 11:55:34 +0200 Subject: [PATCH 20/45] add TBS_LUCID_FC --- src/main/target/TBS_LUCID_FC/CMakeLists.txt | 1 + src/main/target/TBS_LUCID_FC/target.c | 52 +++++++ src/main/target/TBS_LUCID_FC/target.h | 151 ++++++++++++++++++++ 3 files changed, 204 insertions(+) create mode 100644 src/main/target/TBS_LUCID_FC/CMakeLists.txt create mode 100644 src/main/target/TBS_LUCID_FC/target.c create mode 100644 src/main/target/TBS_LUCID_FC/target.h diff --git a/src/main/target/TBS_LUCID_FC/CMakeLists.txt b/src/main/target/TBS_LUCID_FC/CMakeLists.txt new file mode 100644 index 00000000000..0f7f65f2625 --- /dev/null +++ b/src/main/target/TBS_LUCID_FC/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xMT7(TBS_LUCID_FC) \ No newline at end of file diff --git a/src/main/target/TBS_LUCID_FC/target.c b/src/main/target/TBS_LUCID_FC/target.c new file mode 100644 index 00000000000..213948bacce --- /dev/null +++ b/src/main/target/TBS_LUCID_FC/target.c @@ -0,0 +1,52 @@ +/* + * 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 "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "platform.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S1 + DEF_TIM(TMR3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 2), // S2 + DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 3), // S3 + DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 4), // S4 + + DEF_TIM(TMR2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 8), // S5 + DEF_TIM(TMR2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 9), // S6 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 10), // S7 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 11), // S8 + + DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 5), // LED Strip + + DEF_TIM(TMR4, CH2, PB7, TIM_USE_ANY, 0, 6), // Aux + DEF_TIM(TMR4, CH3, PB8, TIM_USE_ANY, 0, 7), // Aux + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h new file mode 100644 index 00000000000..b608622d1ed --- /dev/null +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -0,0 +1,151 @@ +/* + * 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 "LUFC" + +#define USBD_PRODUCT_STRING "TBS Lucid FC" + +#define LED0 PC14 +#define LED1 PC15 + +#define BEEPER_INVERTED +#define BEEPER PC13 + +#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 PB0 + +#define USE_UART3 +#define UART3_TX_PIN PB11 +#define UART3_RX_PIN PB10 + +#define USE_UART4 +#define UART4_TX_PIN PH3 +#define UART4_RX_PIN PH2 + +#define USE_UART5 +#define UART5_TX_PIN PB9 +#define UART5_RX_PIN PD2 + +#define USE_UART7 +#define UART7_TX_PIN PB4 +#define UART7_RX_PIN PB3 + +#define USE_UART8 +#define UART8_TX_PIN PC2 +#define UART8_RX_PIN PC3 + +#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 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 PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG_FLIP +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PA4 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG_FLIP +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PC7 + +#define USE_BARO +#define USE_BARO_BMP388 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C1 + +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC5 // Camera Control + +#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 VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#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)) +#define TARGET_IO_PORTH (BIT(2) | BIT(3)) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define MAX_PWM_OUTPUT_PORTS 10 + +#define DEFAULT_FEATURES \ + (FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_OSD | \ + FEATURE_LED_STRIP) + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 \ No newline at end of file From 61d569a635c810cf689fd8608438cdfbfdec6e2f Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 20 Aug 2024 10:53:49 +0200 Subject: [PATCH 21/45] Update Fixed Wing Landing.md clearer wording of the Final Approach phase description. --- docs/Fixed Wing Landing.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md index c89a5ee2075..3e658285d61 100644 --- a/docs/Fixed Wing Landing.md +++ b/docs/Fixed Wing Landing.md @@ -15,7 +15,7 @@ This enables up to 4 different approach directions, based on the landing site an 3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2. 4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held. 5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude". -6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is reduced to "Land Altitude". +6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is gradually reduced towards "Land Altitude" while approaching the Safe Home Coordinates. 7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held. 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`. From 899a2673a72616ad1d892da1765995935b732c36 Mon Sep 17 00:00:00 2001 From: b14ckyy <33039058+b14ckyy@users.noreply.github.com> Date: Tue, 20 Aug 2024 10:57:43 +0200 Subject: [PATCH 22/45] Update Fixed Wing Landing.md Fixed some typos --- docs/Fixed Wing Landing.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md index 3e658285d61..c50bef9c567 100644 --- a/docs/Fixed Wing Landing.md +++ b/docs/Fixed Wing Landing.md @@ -4,7 +4,7 @@ INAV supports advanced automatic landings for fixed wing aircraft from version 7.1. The procedure is based on landings for man-carrying aircraft, so that safe landings at a specific location are possible. -Supported are landings at safehome after "Return to Home" or at a defined LAND waypoint for missions. +Supported are landings at Safehome after "Return to Home" or at a defined LAND waypoint for missions. Every landing locations can be defined with a target point and 2 different approach headings (colinear to the landing strips) with exclusive direction or opposite directions allowed. This enables up to 4 different approach directions, based on the landing site and surrounding area. @@ -15,7 +15,7 @@ This enables up to 4 different approach directions, based on the landing site an 3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2. 4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held. 5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude". -6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is gradually reduced towards "Land Altitude" while approaching the Safe Home Coordinates. +6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is gradually reduced towards "Land Altitude" while approaching the Safehome coordinates. 7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held. 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`. From b2aef2524478104baab81235d2da43be7b2eca86 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 20 Aug 2024 17:38:18 +0100 Subject: [PATCH 23/45] bin cut turn and change setting use --- docs/Settings.md | 4 +-- src/main/fc/settings.yaml | 7 +++-- src/main/navigation/navigation_fixedwing.c | 33 ++++++---------------- 3 files changed, 15 insertions(+), 29 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c5965d13361..6ab03e46334 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3524,11 +3524,11 @@ Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within ### nav_fw_wp_tracking_accuracy -Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. +Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy. | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 0 | 0 | 10 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3aa1f481fc..36646efb2c0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2545,10 +2545,11 @@ groups: min: 1 max: 9 - name: nav_fw_wp_tracking_accuracy - description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible." - default_value: OFF + description: "Waypoint tracking accuracy forces the craft to quickly head toward and track along the waypoint course line as closely as possible. Setting adjusts tracking deadband distance fom waypoint courseline [m]. Tracking isn't actively controlled within the deadband providing smoother flight adjustments but less accurate tracking. A 2m deadband should work OK in most cases. Setting to 0 disables waypoint tracking accuracy." + default_value: 0 field: fw.wp_tracking_accuracy - type: bool + min: 0 + max: 10 - name: nav_fw_wp_tracking_max_angle description: "Sets the maximum allowed alignment convergence angle to the waypoint course line when nav_fw_wp_tracking_accuracy is active [degrees]. Lower values result in smoother alignment with the course line but will take more distance until this is achieved." default_value: 60 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4d8215c688d..e09fbea57a9 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -399,7 +399,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - // Calculate cross track error + /* Calculate cross track error */ fpVector3_t virtualCoursePoint; virtualCoursePoint.x = posControl.activeWaypoint.pos.x - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); @@ -407,8 +407,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - /* If waypoint tracking enabled force craft toward and ckosely track along waypoint course line */ - if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ + if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { static float crossTrackErrorRate; static timeUs_t previousCrossTrackErrorUpdateTime; static float previousCrossTrackError = 0.0f; @@ -416,43 +416,28 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; } crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); - previousCrossTrackErrorUpdateTime = currentTimeUs; previousCrossTrackError = navCrossTrackError; } - /* Dynamic tracking deadband set at minimum 2m for baseline speed of 60 km/h */ - float trackingDeadband = 200.0f * MAX(posControl.actualState.velXY / 1700.0f, 1.0f); - - /* cutTurnActive improves convergence with course line when WP turn smoothing CUT used */ - static bool cutTurnActive = false; - if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) { - cutTurnActive = true; - } + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); - if (cutTurnActive && !posControl.flags.wpTurnSmoothingActive) { - virtualTargetBearing = wrap_36000(virtualTargetBearing - wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing)); - cutTurnActive = ABS(wrap_18000(posControl.activeWaypoint.bearing - posControl.actualState.cog)) > 500; - } else if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && - navCrossTrackError > trackingDeadband) { + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); - /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile - * Adjustment limited to navCrossTrackError as course line approached to avoid instability */ + /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); - float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); - float limit = MIN(angleLimit, navCrossTrackError); - + float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed); adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); - adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); } } From 8c5c91c9b666b370e9c6940aa51b95260e4048f8 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 21 Aug 2024 11:12:16 +0100 Subject: [PATCH 24/45] reinstate fiddle factor --- src/main/navigation/navigation_fixedwing.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e09fbea57a9..fc8a5a051bc 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -432,7 +432,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); - float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed); + float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); /* Calculate final adjusted virtualTargetBearing */ From 5d38db02ea77c8eb7af47f8e62c5ebf603501efa Mon Sep 17 00:00:00 2001 From: P-I-Engineer Date: Wed, 21 Aug 2024 06:59:23 -0700 Subject: [PATCH 25/45] Update VTOL.md --- docs/VTOL.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/VTOL.md b/docs/VTOL.md index 64afb333d11..3873eeb4e3f 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -186,7 +186,8 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh ## Motor 'Transition Mixing': Dedicated forward motor configuration In motor mixer set: - -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. - +- Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin till throttle threshold is passed. + ![Alt text](Screenshots/mixerprofile_4puls1_mix.png) ## TailSitter 'Transition Mixing': @@ -233,4 +234,4 @@ If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default 2. In addition to 1. Add a little yaw mixing(about 0.2) in tilt motors. - There will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. Use the max speed or faster speed in tiling servo to reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. ## Dedicated forward motor -- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight \ No newline at end of file +- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight From e0eec2076e8b0a487b0d7c261cceb57dd5f81056 Mon Sep 17 00:00:00 2001 From: P-I-Engineer Date: Wed, 21 Aug 2024 07:01:19 -0700 Subject: [PATCH 26/45] Update MixerProfile.md --- docs/MixerProfile.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 55aedd3a30b..cd9702b5a32 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -41,6 +41,8 @@ The default `mmix` throttle value is 0.0, It will not show in `diff` command whe - -1.0 Date: Wed, 21 Aug 2024 09:12:55 -0500 Subject: [PATCH 27/45] GEPRC_F722_AIO_UART3 move use i2c --- src/main/target/GEPRC_F722_AIO/target.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/GEPRC_F722_AIO/target.h b/src/main/target/GEPRC_F722_AIO/target.h index 3b2ff13f0ac..ee2b9330552 100644 --- a/src/main/target/GEPRC_F722_AIO/target.h +++ b/src/main/target/GEPRC_F722_AIO/target.h @@ -56,9 +56,8 @@ #define ICM42605_SPI_BUS BUS_SPI1 // *************** I2C/Baro/Mag ********************* -#define USE_I2C - #ifndef GEPRC_F722_AIO_UART3 +#define USE_I2C #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 #define I2C2_SDA PB11 From 29244a4e8c4d28fd5c14b90bb9fca76fa46e3cbb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 25 Aug 2024 14:16:44 +0100 Subject: [PATCH 28/45] minor fix --- src/main/navigation/navigation_fixedwing.c | 73 +++++++++++----------- 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index fc8a5a051bc..68232ed7e0b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -399,49 +399,50 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition); } - /* Calculate cross track error */ - fpVector3_t virtualCoursePoint; - virtualCoursePoint.x = posControl.activeWaypoint.pos.x - - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - virtualCoursePoint.y = posControl.activeWaypoint.pos.y - - posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); - navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); - - /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ - if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) { - static float crossTrackErrorRate; - static timeUs_t previousCrossTrackErrorUpdateTime; - static float previousCrossTrackError = 0.0f; - static pt1Filter_t fwCrossTrackErrorRateFilterState; - - if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { - const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); - if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { - crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; + if (isWaypointNavTrackingActive()) { + /* Calculate cross track error */ + fpVector3_t virtualCoursePoint; + virtualCoursePoint.x = posControl.activeWaypoint.pos.x - + posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + virtualCoursePoint.y = posControl.activeWaypoint.pos.y - + posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); + navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint); + + /* If waypoint tracking enabled force craft toward and closely track along waypoint course line */ + if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) { + static float crossTrackErrorRate; + static timeUs_t previousCrossTrackErrorUpdateTime; + static float previousCrossTrackError = 0.0f; + static pt1Filter_t fwCrossTrackErrorRateFilterState; + + if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) { + const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime); + if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) { + crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec; + } + crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); + previousCrossTrackErrorUpdateTime = currentTimeUs; + previousCrossTrackError = navCrossTrackError; } - crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec); - previousCrossTrackErrorUpdateTime = currentTimeUs; - previousCrossTrackError = navCrossTrackError; - } - uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); + uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy); - if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { - float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); - uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); + if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) { + float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing); + uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle); - /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ - float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); - float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); - adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); + /* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */ + float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit)); + float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed); + adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed); - /* Calculate final adjusted virtualTargetBearing */ - uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); - adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); - virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); + /* Calculate final adjusted virtualTargetBearing */ + uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit); + adjustmentFactor = constrainf(adjustmentFactor, -limit, limit); + virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor); + } } } - /* * Calculate NAV heading error * Units are centidegrees From b5da18fe86fb392fc3001420a301a07aaae68f93 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 2 Sep 2024 12:41:11 +0200 Subject: [PATCH 29/45] at32: add uart pinswap --- src/main/drivers/serial_uart.h | 4 +- src/main/drivers/serial_uart_at32f43x.c | 81 +++++++++++++++++++-- src/main/drivers/serial_uart_hal_at32f43x.c | 1 + src/main/target/NEUTRONRCF435MINI/target.h | 5 +- 4 files changed, 80 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index b8e72026128..4c1e6d57508 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -50,7 +50,8 @@ typedef enum { UARTDEV_5 = 4, UARTDEV_6 = 5, UARTDEV_7 = 6, - UARTDEV_8 = 7 + UARTDEV_8 = 7, + UARTDEV_MAX } UARTDevice_e; typedef struct { @@ -69,6 +70,7 @@ typedef struct { void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins); void uartClearIdleFlag(uartPort_t *s); +void uartConfigurePinSwap(uartPort_t *uartPort); #if defined(AT32F43x) serialPort_t *uartOpen(usart_type *USARTx, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options); #else diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c index 354ea6c9343..90ebe05ef82 100644 --- a/src/main/drivers/serial_uart_at32f43x.c +++ b/src/main/drivers/serial_uart_at32f43x.c @@ -45,6 +45,7 @@ typedef struct uartDevice_s { uint8_t af; uint8_t irq; uint32_t irqPriority; + bool pinSwap; } uartDevice_t; #ifdef USE_UART1 @@ -59,7 +60,12 @@ static uartDevice_t uart1 = #endif .rcc_apb2 = RCC_APB2(USART1), .irq = USART1_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART1_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -75,7 +81,12 @@ static uartDevice_t uart2 = #endif .rcc_apb1 = RCC_APB1(USART2), .irq = USART2_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART2_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -91,7 +102,12 @@ static uartDevice_t uart3 = #endif .rcc_apb1 = RCC_APB1(USART3), .irq = USART3_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART3_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -107,7 +123,12 @@ static uartDevice_t uart4 = #endif .rcc_apb1 = RCC_APB1(UART4), .irq = UART4_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART4_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -123,7 +144,12 @@ static uartDevice_t uart5 = #endif .rcc_apb1 = RCC_APB1(UART5), .irq = UART5_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART5_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -139,7 +165,12 @@ static uartDevice_t uart6 = #endif .rcc_apb2 = RCC_APB2(USART6), .irq = USART6_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART6_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -152,7 +183,12 @@ static uartDevice_t uart7 = .af = GPIO_MUX_8, .rcc_apb1 = RCC_APB1(UART7), .irq = UART7_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART7_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -165,7 +201,12 @@ static uartDevice_t uart8 = .af = GPIO_MUX_8, .rcc_apb1 = RCC_APB1(UART8), .irq = UART8_IRQn, - .irqPriority = NVIC_PRIO_SERIALUART + .irqPriority = NVIC_PRIO_SERIALUART, +#ifdef USE_UART8_PIN_SWAP + .pinSwap = true, +#else + .pinSwap = false, +#endif }; #endif @@ -258,6 +299,30 @@ void uartClearIdleFlag(uartPort_t *s) (void) s->USARTx->dt; } +static uartDevice_t *uartFindDevice(uartPort_t *uartPort) +{ + for (uint32_t i = 0; i < UARTDEV_MAX; i++) { + uartDevice_t *pDevice = uartHardwareMap[i]; + + if (pDevice->dev == uartPort->USARTx) { + return pDevice; + } + } + return NULL; +} + +void uartConfigurePinSwap(uartPort_t *uartPort) +{ + uartDevice_t *uartDevice = uartFindDevice(uartPort); + if (!uartDevice) { + return; + } + + if (uartDevice->pinSwap) { + usart_transmit_receive_pin_swap(uartPort->USARTx, TRUE); + } +} + uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; diff --git a/src/main/drivers/serial_uart_hal_at32f43x.c b/src/main/drivers/serial_uart_hal_at32f43x.c index a06869ba10c..f30726332f4 100644 --- a/src/main/drivers/serial_uart_hal_at32f43x.c +++ b/src/main/drivers/serial_uart_hal_at32f43x.c @@ -85,6 +85,7 @@ static void uartReconfigure(uartPort_t *uartPort) usart_transmitter_enable(uartPort->USARTx, TRUE); usartConfigurePinInversion(uartPort); + uartConfigurePinSwap(uartPort); if (uartPort->port.options & SERIAL_BIDIR) usart_single_line_halfduplex_select(uartPort->USARTx, TRUE); diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 2e90f48a69d..6ef53abf64d 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -144,8 +144,9 @@ #define UART2_TX_PIN PA2 #define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 +#define USE_UART3_PIN_SWAP +#define UART3_RX_PIN PB10 +#define UART3_TX_PIN PB11 #define USE_UART5 #define UART5_RX_PIN PB8 From 570b301d440a59c15bcc7306303963d62b4e7b17 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 2 Sep 2024 21:16:48 +0100 Subject: [PATCH 30/45] Align update rate for WP distance with XT error --- src/main/navigation/navigation_fixedwing.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 68232ed7e0b..ff38b2e17e5 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -401,6 +401,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta if (isWaypointNavTrackingActive()) { /* Calculate cross track error */ + posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + fpVector3_t virtualCoursePoint; virtualCoursePoint.x = posControl.activeWaypoint.pos.x - posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing)); From 3f9f70f8e8e1da0f2912e5acc7a9d06487557f48 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Mon, 2 Sep 2024 11:17:35 +0200 Subject: [PATCH 31/45] lucid: remove in-accessible servo pins --- src/main/target/TBS_LUCID_FC/target.c | 10 ++-------- src/main/target/TBS_LUCID_FC/target.h | 2 +- 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/target/TBS_LUCID_FC/target.c b/src/main/target/TBS_LUCID_FC/target.c index 213948bacce..e6c7a3830ab 100644 --- a/src/main/target/TBS_LUCID_FC/target.c +++ b/src/main/target/TBS_LUCID_FC/target.c @@ -37,16 +37,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 3), // S3 DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 4), // S4 - DEF_TIM(TMR2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 8), // S5 - DEF_TIM(TMR2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 9), // S6 - DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 10), // S7 - DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 11), // S8 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 8), // S5 + DEF_TIM(TMR4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 9), // S6 DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 5), // LED Strip - - DEF_TIM(TMR4, CH2, PB7, TIM_USE_ANY, 0, 6), // Aux - DEF_TIM(TMR4, CH3, PB8, TIM_USE_ANY, 0, 7), // Aux - }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h index b608622d1ed..7a9ed60fb52 100644 --- a/src/main/target/TBS_LUCID_FC/target.h +++ b/src/main/target/TBS_LUCID_FC/target.h @@ -140,7 +140,7 @@ #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define MAX_PWM_OUTPUT_PORTS 10 +#define MAX_PWM_OUTPUT_PORTS 6 #define DEFAULT_FEATURES \ (FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_OSD | \ From a2187ce16616bf21359202fe6f52921aa6d76d60 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 3 Sep 2024 12:37:33 +0100 Subject: [PATCH 32/45] change WP mode linear climb method --- src/main/navigation/navigation.c | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e43eb9d8534..4ae4cf228db 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1859,6 +1859,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav case NAV_WP_ACTION_LAND: calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]); posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos); + posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS @@ -1922,16 +1923,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - - // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP - // Update climb rate until within 100cm of total climb xy distance to WP - float climbRate = 0.0f; - if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) { - climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / - (posControl.wpDistance - 0.1f * posControl.wpInitialDistance); - } - updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET); + /* Use linear climb/descent between WPs arriving at WP altitude when within 10% of total distance to WP */ + tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance), + posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance, + posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); + + setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); if(STATE(MULTIROTOR)) { switch (wpHeadingControl.mode) { @@ -3703,7 +3700,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags); } // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission - else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !FLIGHT_MODE(NAV_WP_MODE)) { + else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !FLIGHT_MODE(NAV_WP_MODE)) { // WP upload is not allowed why WP mode is active if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) { // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission) From a0c1717670757b4da10412fccd2eaf323c70972a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 3 Sep 2024 18:16:08 +0200 Subject: [PATCH 33/45] Update ci.yml --- .github/workflows/ci.yml | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4663febe50e..76b85db3ba0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,12 +40,13 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - uses: actions/cache@v4 with: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe -G Ninja .. && ninja -j4 ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe -G Ninja .. && ninja -j${{ env.NUM_CORES }} ci - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -74,6 +75,7 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - name: Download artifacts uses: actions/download-artifact@v4 with: @@ -116,8 +118,9 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j${{ env.NUM_CORES }} - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -148,11 +151,12 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - name: Build SITL run: | mkdir -p build_SITL && cd build_SITL cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja .. - ninja -j3 + ninja -j4 - name: Upload artifacts uses: actions/upload-artifact@v4 @@ -187,6 +191,7 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 - name: Upload artifacts From 8c19013b81fcfb8aaae13bc865585a54fceb8a1b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 3 Sep 2024 18:35:28 +0200 Subject: [PATCH 34/45] Update dev-builds.yml --- .github/workflows/dev-builds.yml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml index 94492966f79..c107e24521d 100644 --- a/.github/workflows/dev-builds.yml +++ b/.github/workflows/dev-builds.yml @@ -42,12 +42,13 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - uses: actions/cache@v4 with: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DVERSION_TYPE=dev -G Ninja .. && ninja ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DVERSION_TYPE=dev -G Ninja .. && ninja -j${{ env.NUM_CORES }} ci - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -76,8 +77,9 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "NUM_CORES=$(grep processor /proc/cpuinfo | wc -l)" >> $GITHUB_ENV - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja -DVERSION_TYPE=dev .. && ninja + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja -DVERSION_TYPE=dev .. && ninja -j${{ env.NUM_CORES }} - name: Upload artifacts uses: actions/upload-artifact@v4 with: @@ -112,7 +114,7 @@ jobs: run: | mkdir -p build_SITL && cd build_SITL cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -DVERSION_TYPE=dev -G Ninja .. - ninja + ninja -j4 - name: Upload artifacts uses: actions/upload-artifact@v4 @@ -150,7 +152,7 @@ jobs: #echo "VERSION_TAG=-$(date '+%Y%m%d')" >> $GITHUB_ENV echo "version=${VERSION}" >> $GITHUB_OUTPUT - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE=dev -G Ninja .. && ninja + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE=dev -G Ninja .. && ninja -j4 - name: Upload artifacts uses: actions/upload-artifact@v4 with: From dd82cd86ef65f5f06016e35718a15f2a8721d2fb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 4 Sep 2024 15:47:35 +0200 Subject: [PATCH 35/45] iFlight IFLIGHT_BLITZ_H7_WING changes --- src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt | 3 ++- src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h | 11 ++++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt b/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt index b8fc79235d4..c4bd2c43840 100644 --- a/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt +++ b/src/main/target/IFLIGHT_BLITZ_H7_PRO/CMakeLists.txt @@ -1 +1,2 @@ -target_stm32h743xi(IFLIGHT_BLITZ_H7_PRO) \ No newline at end of file +target_stm32h743xi(IFLIGHT_BLITZ_H7_PRO) +target_stm32h743xi(IFLIGHT_BLITZ_H7_WING) \ No newline at end of file diff --git a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h index 23fde63466c..7d93ec91ce6 100644 --- a/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h +++ b/src/main/target/IFLIGHT_BLITZ_H7_PRO/target.h @@ -20,7 +20,11 @@ #define TARGET_BOARD_IDENTIFIER "IB7P" +#ifdef IFLIGHT_BLITZ_H7_PRO #define USBD_PRODUCT_STRING "IFLIGHT_BLITZ_H7_PRO" +#else +#define USBD_PRODUCT_STRING "IFLIGHT_BLITZ_H7_WING" +#endif #define USE_TARGET_CONFIG @@ -161,7 +165,12 @@ #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE 212 -#define VBAT_SCALE_DEFAULT 1135 + +#ifdef IFLIGHT_BLITZ_H7_PRO +#define VBAT_SCALE_DEFAULT 2100 +#else +#define VBAT_SCALE_DEFAULT 1100 +#endif #define USE_SERIAL_4WAY_BLHELI_INTERFACE From d9596a1ef43e3142504dc33d1aa4e72631c9644f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 5 Sep 2024 18:13:20 +0200 Subject: [PATCH 36/45] Add some debugging for msp headtracker --- src/main/io/headtracker_msp.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index 8a800b3f46e..6c03f89130f 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -56,6 +56,7 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) { if(dataSize != sizeof(headtrackerMspMessage_t)) { SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize)); + DEBUG_SET(DEBUG_HEADTRACKING, 7, 1); return; } @@ -66,7 +67,10 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX); headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US; - UNUSED(status); + DEBUG_SET(DEBUG_HEADTRACKING, 0, headTrackerMspDevice.pan); + DEBUG_SET(DEBUG_HEADTRACKING, 1, headTrackerMspDevice.tilt); + DEBUG_SET(DEBUG_HEADTRACKING, 2, headTrackerMspDevice.roll); + DEBUG_SET(DEBUG_HEADTRACKING, 3, headTrackerMspDevice.expires); } headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice) { From db9723e4bed9c5c36ee52b0756cc15c41e1faa0c Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 5 Sep 2024 19:19:59 +0200 Subject: [PATCH 37/45] More permissive parsing. --- src/main/fc/fc_msp.c | 2 +- src/main/io/headtracker_msp.c | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e7a7a4aa3a6..2b544ee449a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4127,7 +4127,7 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP)) case MSP2_SENSOR_HEADTRACKER: - mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize); + mspHeadTrackerReceiverNewData(sbufPtr(src), sbufBytesRemaining(src)); break; #endif } diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index 6c03f89130f..b90064caa81 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -54,9 +54,12 @@ void mspHeadTrackerInit(void) void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) { - if(dataSize != sizeof(headtrackerMspMessage_t)) { + if(dataSize >= sizeof(headtrackerMspMessage_t)) { SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize)); - DEBUG_SET(DEBUG_HEADTRACKING, 7, 1); + static int errorCount = 0; + DEBUG_SET(DEBUG_HEADTRACKING, 7, errorCount++); + DEBUG_SET(DEBUG_HEADTRACKING, 5, (sizeof(headtrackerMspMessage_t))); + DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize); return; } From 9dcdb543f48822142f8437ab62df74aac4af412f Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 5 Sep 2024 19:25:36 +0200 Subject: [PATCH 38/45] Warning fix --- src/main/io/headtracker_msp.c | 2 +- src/main/io/headtracker_msp.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index b90064caa81..24c409469ed 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -52,7 +52,7 @@ void mspHeadTrackerInit(void) } } -void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize) +void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize) { if(dataSize >= sizeof(headtrackerMspMessage_t)) { SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize)); diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h index edcfb465874..f767b1c6286 100644 --- a/src/main/io/headtracker_msp.h +++ b/src/main/io/headtracker_msp.h @@ -37,7 +37,7 @@ typedef struct headtrackerMspMessage_s { void mspHeadTrackerInit(void); -void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize); +void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize); headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice); From 9849d69ee28889b20216709e06b0c676e89140a5 Mon Sep 17 00:00:00 2001 From: Sensei Date: Fri, 6 Sep 2024 01:31:32 -0500 Subject: [PATCH 39/45] Update USB Flashing.md - mention using a hub with USB-C --- docs/USB Flashing.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/USB Flashing.md b/docs/USB Flashing.md index 9d79d5cd8d1..ade5ae2ba4a 100644 --- a/docs/USB Flashing.md +++ b/docs/USB Flashing.md @@ -60,7 +60,7 @@ With the board connected and in bootloader mode (reset it by sending the charact * If you are using a device with only USB-C ports such as a Mac-OS device, you will need a dongle. * A USB-C to USB-C cable is identical on both ends and thus requires extra hardware to let them be auto detected as devices instead of hosts. - * Using a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works. + * Using either a hub woth USB-A ports, or a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works. ## Using `dfu-util` From 842cbfb5da6ec76af132b36cf3a46860c581f209 Mon Sep 17 00:00:00 2001 From: Sensei Date: Fri, 6 Sep 2024 01:33:05 -0500 Subject: [PATCH 40/45] Update USB Flashing.md - typo --- docs/USB Flashing.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/USB Flashing.md b/docs/USB Flashing.md index ade5ae2ba4a..97787c7fb48 100644 --- a/docs/USB Flashing.md +++ b/docs/USB Flashing.md @@ -60,7 +60,7 @@ With the board connected and in bootloader mode (reset it by sending the charact * If you are using a device with only USB-C ports such as a Mac-OS device, you will need a dongle. * A USB-C to USB-C cable is identical on both ends and thus requires extra hardware to let them be auto detected as devices instead of hosts. - * Using either a hub woth USB-A ports, or a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works. + * Using either a hub with USB-A ports, or a USB-A to C cable or dongle is usually the easiest way to get a working connection but an USB-OTG adapter also works. ## Using `dfu-util` From 73f4ace3a2c5f15159b6146a9833615f0f016a09 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 6 Sep 2024 11:58:19 +0200 Subject: [PATCH 41/45] Update ci.yml --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4663febe50e..28873b19fff 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -18,7 +18,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] steps: - uses: actions/checkout@v4 From a3c503fb4c8a2cfd006df9102e9e1537da497fef Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 6 Sep 2024 12:05:23 +0200 Subject: [PATCH 42/45] Update dev-builds.yml --- .github/workflows/dev-builds.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml index 94492966f79..c633ec827b8 100644 --- a/.github/workflows/dev-builds.yml +++ b/.github/workflows/dev-builds.yml @@ -20,7 +20,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] steps: - uses: actions/checkout@v4 From 3ef2c56c2e2b23c14a6593d1f6ec045e19ac9de5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:00:58 +0200 Subject: [PATCH 43/45] Small fixes for serial gimbal --- src/main/fc/fc_msp.c | 2 +- src/main/io/gimbal_serial.c | 6 ++++-- src/main/io/headtracker_msp.c | 17 +++++++++++++++-- src/main/io/headtracker_msp.h | 1 + 4 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2b544ee449a..e7a7a4aa3a6 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -4127,7 +4127,7 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src) #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP)) case MSP2_SENSOR_HEADTRACKER: - mspHeadTrackerReceiverNewData(sbufPtr(src), sbufBytesRemaining(src)); + mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize); break; #endif } diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c index 4eec2903947..085590f0880 100644 --- a/src/main/io/gimbal_serial.c +++ b/src/main/io/gimbal_serial.c @@ -116,7 +116,9 @@ bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice) bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice) { UNUSED(gimbalDevice); - return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort); + + headTrackerDevice_t *htrk = headTrackerCommonDevice(); + return htrk != NULL && headTrackerCommonIsReady(htrk); } bool gimbalSerialInit(void) @@ -251,7 +253,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime) #else { #endif - DEBUG_SET(DEBUG_HEADTRACKING, 4, 0); + DEBUG_SET(DEBUG_HEADTRACKING, 4, 2); // Radio endpoints may need to be adjusted, as it seems ot go a bit // bananas at the extremes attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM); diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index 24c409469ed..ad2a8842f7c 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -30,10 +30,11 @@ #include "io/headtracker_msp.h" +static bool isReady = false; static headTrackerVTable_t headTrackerMspVTable = { .process = NULL, .getDeviceType = heatTrackerMspGetDeviceType, - .isReady = NULL, + .isReady = heatTrackerMspIsReady, .isValid = NULL, }; @@ -47,6 +48,9 @@ static headTrackerDevice_t headTrackerMspDevice = { void mspHeadTrackerInit(void) { + for(int i = 0; i < 8;++i) { + DEBUG_SET(DEBUG_HEADTRACKING, i, 0); + } if(headTrackerConfig()->devType == HEADTRACKER_MSP) { headTrackerCommonSetDevice(&headTrackerMspDevice); } @@ -54,7 +58,7 @@ void mspHeadTrackerInit(void) void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize) { - if(dataSize >= sizeof(headtrackerMspMessage_t)) { + if(dataSize != sizeof(headtrackerMspMessage_t)) { SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize)); static int errorCount = 0; DEBUG_SET(DEBUG_HEADTRACKING, 7, errorCount++); @@ -62,6 +66,8 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize) DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize); return; } + isReady = true; + DEBUG_SET(DEBUG_HEADTRACKING, 6, dataSize); headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data; @@ -81,4 +87,11 @@ headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *head return HEADTRACKER_MSP; } + +bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice) +{ + UNUSED(headTrackerDevice); + return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady; +} + #endif \ No newline at end of file diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h index f767b1c6286..e082d0db8ae 100644 --- a/src/main/io/headtracker_msp.h +++ b/src/main/io/headtracker_msp.h @@ -40,5 +40,6 @@ void mspHeadTrackerInit(void); void mspHeadTrackerReceiverNewData(uint8_t *data, unsigned int dataSize); headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice); +bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice); #endif From 2ac360336c8d4bd01c5dbe1a46810ac78dfba8b7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:03:06 +0200 Subject: [PATCH 44/45] Update headtracker_msp.c --- src/main/io/headtracker_msp.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c index ad2a8842f7c..e53fd3ab207 100644 --- a/src/main/io/headtracker_msp.c +++ b/src/main/io/headtracker_msp.c @@ -48,9 +48,6 @@ static headTrackerDevice_t headTrackerMspDevice = { void mspHeadTrackerInit(void) { - for(int i = 0; i < 8;++i) { - DEBUG_SET(DEBUG_HEADTRACKING, i, 0); - } if(headTrackerConfig()->devType == HEADTRACKER_MSP) { headTrackerCommonSetDevice(&headTrackerMspDevice); } @@ -94,4 +91,4 @@ bool heatTrackerMspIsReady(const headTrackerDevice_t *headTrackerDevice) return headTrackerConfig()->devType == HEADTRACKER_MSP && isReady; } -#endif \ No newline at end of file +#endif From 2511cc31cb0edfa8bb229e11c911e2180c35af3e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 7 Sep 2024 00:15:58 +0200 Subject: [PATCH 45/45] Unit test fixes --- src/main/drivers/headtracker_common.c | 4 ---- src/main/drivers/headtracker_common.h | 3 ++- src/main/drivers/time.h | 7 +++++++ src/main/io/gimbal_serial.c | 2 +- src/test/unit/CMakeLists.txt | 4 ++-- src/test/unit/gimbal_serial_unittest.cc | 6 ++++++ 6 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c index feec581da8a..8ea71cd2f66 100644 --- a/src/main/drivers/headtracker_common.c +++ b/src/main/drivers/headtracker_common.c @@ -157,10 +157,6 @@ void taskUpdateHeadTracker(timeUs_t currentTimeUs) #else void taskUpdateHeadTracker(timeUs_t currentTimeUs) { - if (cliMode) { - return; - } - headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice(); if(headTrackerDevice) { diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h index 8c177e3e0f0..92dac7e61e6 100644 --- a/src/main/drivers/headtracker_common.h +++ b/src/main/drivers/headtracker_common.h @@ -23,7 +23,6 @@ #define HEADTRACKER_RANGE_MIN -2048 #define HEADTRACKER_RANGE_MAX 2047 -#ifdef USE_HEADTRACKER #include @@ -81,6 +80,8 @@ typedef struct headTrackerConfig_s { float roll_ratio; } headTrackerConfig_t; +#ifdef USE_HEADTRACKER + PG_DECLARE(headTrackerConfig_t, headTrackerConfig); void headTrackerCommonInit(void); diff --git a/src/main/drivers/time.h b/src/main/drivers/time.h index 6901d57c19b..ea30592515e 100644 --- a/src/main/drivers/time.h +++ b/src/main/drivers/time.h @@ -19,6 +19,9 @@ #include +#ifdef __cplusplus +extern "C" { +#endif #include "common/time.h" extern uint32_t usTicks; @@ -32,3 +35,7 @@ timeUs_t microsISR(void); timeMs_t millis(void); uint32_t ticks(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c index 085590f0880..fd6294c9959 100644 --- a/src/main/io/gimbal_serial.c +++ b/src/main/io/gimbal_serial.c @@ -63,11 +63,11 @@ static gimbalSerialHtrkState_t headTrackerState = { .attitude = {}, .state = WAITING_HDR1, }; +static serialPort_t *headTrackerPort = NULL; #endif #endif -static serialPort_t *headTrackerPort = NULL; static serialPort_t *gimbalPort = NULL; gimbalVTable_t gimbalSerialVTable = { diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 300721b8f53..2cb53b64ecb 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -40,8 +40,8 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c") set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST) -set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c") -set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST) +set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c" "drivers/headtracker_common.c") +set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST USE_HEADTRACKER) function(unit_test src) get_filename_component(basename ${src} NAME) diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc index 1f0c47231c6..4ca55e17ee2 100644 --- a/src/test/unit/gimbal_serial_unittest.cc +++ b/src/test/unit/gimbal_serial_unittest.cc @@ -34,6 +34,12 @@ void dumpMemory(uint8_t *mem, int size) printf("\n"); } +extern "C" { +timeUs_t micros(void) { + return 10; +} +} + TEST(GimbalSerialTest, TestGimbalSerialScale) { int16_t res16 = gimbal_scale12(1000, 2000, 2000);