From 4904c718a553479d22f7da6d25992b61c31097ed Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 25 Jul 2024 05:42:46 +0930 Subject: [PATCH 01/77] AP_NavEKF3: Fix yaw alignment bug When the yaw is aligned to the GPS yaw, the recordYawResetsCompleted() function should be called the same as for any other yaw reset. --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 390dfceb8705ae..67ac2a57943a4c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -294,6 +294,7 @@ void NavEKF3_core::SelectMagFusion() have_fused_gps_yaw = true; lastSynthYawTime_ms = imuSampleTime_ms; last_gps_yaw_fuse_ms = imuSampleTime_ms; + recordYawResetsCompleted(); } else if (tiltAlignComplete && yawAlignComplete) { have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS); if (have_fused_gps_yaw) { From 25a406e63370d59998c4efdfa55e3dd86755a7f7 Mon Sep 17 00:00:00 2001 From: mduclehcm Date: Mon, 22 Jul 2024 11:48:07 +0700 Subject: [PATCH 02/77] AP_Mount: Conditionally define `serial_instance` to fix unused variable compile error - Wrapped the definition of `serial_instance` with preprocessor directives to ensure it is only defined when necessary. - This resolves the compile error caused by the unused variable when no features requiring `serial_instance` are enabled. --- libraries/AP_Mount/AP_Mount.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 777256cb050a97..f3225bbd63880c 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -46,7 +46,7 @@ AP_Mount::AP_Mount() } _singleton = this; - AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info); } // init - detect and initialise all mounts @@ -184,6 +184,8 @@ void AP_Mount::init() set_mode_to_default(instance); } } + + (void)serial_instance; } // update - give mount opportunity to update servos. should be called at 10hz or higher From 025077b6f49e246c1a421b792e9b2e57338a881c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Jul 2024 16:39:52 +1000 Subject: [PATCH 03/77] AP_HAL_ChibiOS: add support for recording UART Rx errors --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 40 ++++++++++++++++++++++++- libraries/AP_HAL_ChibiOS/UARTDriver.h | 8 +++++ 2 files changed, 47 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index da4ae589df661e..fcf29a854578ff 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -70,6 +70,9 @@ static const eventmask_t EVT_PARITY = EVENT_MASK(11); // event for transmit end for half-duplex static const eventmask_t EVT_TRANSMIT_END = EVENT_MASK(12); +// event for framing error +static const eventmask_t EVT_ERROR = EVENT_MASK(13); + // events for dma tx, thread per UART so can be from 0 static const eventmask_t EVT_TRANSMIT_DMA_START = EVENT_MASK(0); static const eventmask_t EVT_TRANSMIT_DMA_COMPLETE = EVENT_MASK(1); @@ -495,6 +498,16 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) vprintf_console_hook = hal_console_vprintf; #endif } + +#if HAL_UART_STATS_ENABLED && CH_CFG_USE_EVENTS == TRUE + if (!err_listener_initialised) { + chEvtRegisterMaskWithFlags(chnGetEventSource((SerialDriver*)sdef.serial), + &err_listener, + EVT_ERROR, + SD_FRAMING_ERROR | SD_OVERRUN_ERROR | SD_NOISE_ERROR); + err_listener_initialised = true; + } +#endif } #ifndef HAL_UART_NODMA @@ -1091,6 +1104,22 @@ void UARTDriver::_rx_timer_tick(void) _in_rx_timer = true; +#if HAL_UART_STATS_ENABLED && CH_CFG_USE_EVENTS == TRUE + if (!sdef.is_usb) { + const auto err_flags = chEvtGetAndClearFlags(&err_listener); + // count the number of errors + if (err_flags & SD_FRAMING_ERROR) { + _rx_stats_framing_errors++; + } + if (err_flags & SD_OVERRUN_ERROR) { + _rx_stats_overrun_errors++; + } + if (err_flags & SD_NOISE_ERROR) { + _rx_stats_noise_errors++; + } + } +#endif + #ifndef HAL_UART_NODMA if (rx_dma_enabled && rxdma) { chSysLock(); @@ -1748,13 +1777,22 @@ void UARTDriver::uart_info(ExpandingString &str, StatsTracker &stats, const uint } else { str.printf("UART%u ", unsigned(sdef.instance)); } - str.printf("TX%c=%8u RX%c=%8u TXBD=%6u RXBD=%6u FlowCtrl=%u\n", + str.printf("TX%c=%8u RX%c=%8u TXBD=%6u RXBD=%6u" +#if CH_CFG_USE_EVENTS == TRUE + " FE=%lu OE=%lu NE=%lu" +#endif + " FlowCtrl=%u\n", tx_dma_enabled ? '*' : ' ', unsigned(tx_bytes), rx_dma_enabled ? '*' : ' ', unsigned(rx_bytes), unsigned((tx_bytes * 10000) / dt_ms), unsigned((rx_bytes * 10000) / dt_ms), +#if CH_CFG_USE_EVENTS == TRUE + _rx_stats_framing_errors, + _rx_stats_overrun_errors, + _rx_stats_noise_errors, +#endif _flow_control); } #endif diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index f21c1d7aa8be4f..2315192f9e90b3 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -79,6 +79,7 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint8_t get_index(void) const { return uint8_t(this - &_serial_tab[0]); } + bool low_noise_line; }; bool wait_timeout(uint16_t n, uint32_t timeout_ms) override; @@ -282,6 +283,13 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { // Getters for cumulative tx and rx counts uint32_t get_total_tx_bytes() const override { return _tx_stats_bytes; } uint32_t get_total_rx_bytes() const override { return _rx_stats_bytes; } +#if CH_CFG_USE_EVENTS == TRUE + uint32_t _rx_stats_framing_errors; + uint32_t _rx_stats_overrun_errors; + uint32_t _rx_stats_noise_errors; + event_listener_t err_listener; + bool err_listener_initialised; +#endif #endif }; From 993d694fbbe6d35d057a8171d07ccaa8e536466c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Jul 2024 16:40:30 +1000 Subject: [PATCH 04/77] AP_HAL_ChibiOS: allow setup for low noise clock mismatch tolerant UART line --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 7 ++++ libraries/AP_HAL_ChibiOS/UARTDriver.h | 7 ++++ .../hwdef/scripts/chibios_hwdef.py | 33 ++++++++++++++++--- 3 files changed, 43 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index fcf29a854578ff..86689cfdba30b8 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -455,6 +455,13 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr3 |= USART_CR3_DMAT; } sercfg.irq_cb = rx_irq_cb; +#if HAL_HAVE_LOW_NOISE_UART + if (sdef.low_noise_line) { + // we can mark UART to sample on one bit instead of default 3 bits + // this allows us to be slightly less sensitive to clock differences + sercfg.cr3 |= USART_CR3_ONEBIT; + } +#endif #endif // HAL_UART_NODMA if (!(sercfg.cr2 & USART_CR2_STOP2_BITS)) { sercfg.cr2 |= USART_CR2_STOP1_BITS; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 2315192f9e90b3..c549b8eebd5424 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -28,6 +28,10 @@ // enough for serial0 to serial9, plus IOMCU #define UART_MAX_DRIVERS 11 +#ifndef HAL_HAVE_LOW_NOISE_UART +#define HAL_HAVE_LOW_NOISE_UART 0 +#endif + class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { public: UARTDriver(uint8_t serial_num); @@ -79,7 +83,10 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { uint8_t get_index(void) const { return uint8_t(this - &_serial_tab[0]); } + +#if HAL_HAVE_LOW_NOISE_UART bool low_noise_line; +#endif }; bool wait_timeout(uint16_t n, uint32_t timeout_ms) override; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e01e4283a45d30..e59b850f75eac9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1865,6 +1865,7 @@ def write_UART_config(self, f): OTG2_index = None devlist = [] have_rts_cts = False + have_low_noise = False crash_uart = None # write config for CrashCatcher UART @@ -1878,6 +1879,14 @@ def write_UART_config(self, f): f.write('#define IRQ_DISABLE_HAL_CRASH_SERIAL_PORT() nvicDisableVector(STM32_%s_NUMBER)\n' % crash_uart) f.write('#define RCC_RESET_HAL_CRASH_SERIAL_PORT() rccReset%s(); rccEnable%s(true)\n' % (crash_uart, crash_uart)) f.write('#define HAL_CRASH_SERIAL_PORT_CLOCK STM32_%sCLK\n' % crash_uart) + # check if we have a UART with a low noise RX pin + for num, dev in enumerate(serial_list): + if not dev.startswith('UART') and not dev.startswith('USART'): + continue + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + have_low_noise = True + break for num, dev in enumerate(serial_list): if dev.startswith('UART'): n = int(dev[4:]) @@ -1904,12 +1913,20 @@ def write_UART_config(self, f): if dev.startswith('OTG2'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') OTG2_index = serial_list.index(dev) self.dual_USB_enabled = True elif dev.startswith('OTG'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') else: need_uart_driver = True f.write( @@ -1948,9 +1965,17 @@ def get_RTS_alt_function(): if s not in lib.AltFunction_map: return "UINT8_MAX" return lib.AltFunction_map[s] + if have_low_noise: + low_noise = 'false' + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + low_noise = 'true' + f.write("%s, %s}\n" % (get_RTS_alt_function(), low_noise)) + else: + f.write("%s}\n" % get_RTS_alt_function()) - f.write("%s}\n" % get_RTS_alt_function()) - + if have_low_noise: + f.write('#define HAL_HAVE_LOW_NOISE_UART 1\n') if have_rts_cts: f.write('#define AP_FEATURE_RTSCTS 1\n') if OTG2_index is not None: From 4909ad4e4f887dfd7e9eef9155da2293a44d607a Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Jul 2024 16:41:16 +1000 Subject: [PATCH 05/77] AP_Networking: add debug code for PPP --- libraries/AP_Networking/AP_Networking_PPP.cpp | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/libraries/AP_Networking/AP_Networking_PPP.cpp b/libraries/AP_Networking/AP_Networking_PPP.cpp index 6bcf3651bba098..5d1128a781f2a7 100644 --- a/libraries/AP_Networking/AP_Networking_PPP.cpp +++ b/libraries/AP_Networking/AP_Networking_PPP.cpp @@ -25,6 +25,9 @@ extern const AP_HAL::HAL& hal; #define LWIP_TCPIP_UNLOCK() #endif +#define PPP_DEBUG_TX 0 +#define PPP_DEBUG_RX 0 + /* output some data to the uart */ @@ -34,6 +37,27 @@ uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32 LWIP_UNUSED_ARG(pcb); uint32_t remaining = len; const uint8_t *ptr = (const uint8_t *)data; +#if PPP_DEBUG_TX + bool flag_end = false; + if (ptr[len-1] == 0x7E) { + flag_end = true; + remaining--; + } + if (ptr[0] == 0x7E) { + // send byte size + if (pkt_size > 0) { + printf("PPP: tx[%lu] %u\n", tx_index++, pkt_size); + } + // dump the packet + if (!(tx_index % 10)) { + for (uint32_t i = 0; i < pkt_size; i++) { + printf(" %02X", tx_bytes[i]); + } + printf("\n"); + } + pkt_size = 0; + } +#endif while (remaining > 0) { const auto n = driver.uart->write(ptr, remaining); if (n > 0) { @@ -43,6 +67,22 @@ uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32 hal.scheduler->delay_microseconds(100); } } +#if PPP_DEBUG_TX + memcpy(&tx_bytes[pkt_size], data, len); + pkt_size += len; + if (flag_end) { + driver.uart->write(0x7E); + printf("PPP: tx[%lu] %u\n", tx_index++, pkt_size); + // dump the packet + if (!(tx_index % 10)) { + for (uint32_t i = 0; i < pkt_size; i++) { + printf(" %02X", tx_bytes[i]); + } + printf("\n"); + } + pkt_size = 0; + } +#endif return len; } @@ -224,6 +264,25 @@ void AP_Networking_PPP::ppp_loop(void) } else { hal.scheduler->delay_microseconds(200); } +#if PPP_DEBUG_RX + auto pppos = (pppos_pcb *)ppp->link_ctx_cb; + for (uint32_t i = 0; i < n; i++) { + if (buf[i] == 0x7E && last_ppp_frame_size != 1) { + // dump the packet + if (pppos->bad_pkt) { + printf("PPP: rx[%lu] %u\n", rx_index, last_ppp_frame_size); + for (uint32_t j = 0; j < last_ppp_frame_size; j++) { + printf("0x%02X,", rx_bytes[j]); + } + printf("\n"); + hal.scheduler->delay(1); + } + rx_index++; + last_ppp_frame_size = 0; + } + rx_bytes[last_ppp_frame_size++] = buf[i]; + } +#endif } } } From 8a4b925c71cda0ce1a0c1cf6188f49f172d72cc0 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Jul 2024 16:42:52 +1000 Subject: [PATCH 06/77] AP_HAL_ChibiOS: add support for PPP between CubeRed Primary and Secondary --- .../hwdef/CubeRedPrimary/defaults.parm | 19 +++++++++++++++++++ .../hwdef/CubeRedPrimary/hwdef.dat | 19 +++++++++++-------- .../hwdef/CubeRedSecondary/defaults.parm | 10 ++++++++++ .../hwdef/CubeRedSecondary/hwdef.dat | 16 +++++++++++----- 4 files changed, 51 insertions(+), 13 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm index cdd83a642efe87..22f785c1a3b188 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/defaults.parm @@ -6,3 +6,22 @@ ADSB_TYPE 1 SERIAL5_BAUD 57 SERIAL5_PROTOCOL 1 EK3_PRIMARY 1 + +NET_ENABLE 1 +NET_OPTIONS 1 +NET_DHCP 0 +NET_P1_TYPE 1 +NET_P1_PROTOCOL 2 +NET_P1_IP0 192 +NET_P1_IP1 168 +NET_P1_IP2 144 +NET_P1_IP3 10 +NET_P1_PORT 14553 + +NET_P2_TYPE 4 +NET_P2_PROTOCOL 2 +NET_P2_IP0 192 +NET_P2_IP1 168 +NET_P2_IP2 144 +NET_P2_IP3 100 +NET_P2_PORT 14554 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat index d6e7dfa8e38aad..5bd8d3c71c6394 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat @@ -274,17 +274,20 @@ PG15 USART6_CTS USART6 PG8 USART6_RTS USART6 # primary <-> secondary -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 +PE7 UART7_RX UART7 SPEED_VERYLOW LOW_NOISE +PE8 UART7_TX UART7 SPEED_VERYLOW # order of UARTs (and USB) -SERIAL_ORDER OTG1 USART2 USART6 USART3 UART4 UART8 EMPTY UART7 +SERIAL_ORDER OTG1 USART2 USART6 USART3 UART4 UART8 OTG2 UART7 EXT_FLASH_SIZE_MB 32 INT_FLASH_PRIMARY 1 -# forward Serial traffic from USB OTG2 to Serial7(UART7) -define HAL_FORWARD_OTG2_SERIAL 7 -define HAL_HAVE_DUAL_USB_CDC 1 -define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2 -define DEFAULT_SERIAL7_BAUD 2000000 +# set protocol for UART7 to SerialProtocol_PPP +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_PPP +define DEFAULT_SERIAL7_BAUD 8000000 + +define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.144.100" +define AP_NETWORKING_DEFAULT_STATIC_NETMASK "255.255.255.0" +define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.144.11" +define AP_NETWORKING_BACKEND_PPP 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm index 3be7684ba40cad..342b61b6d584b8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/defaults.parm @@ -1,2 +1,12 @@ SERIAL3_OPTIONS 8 SERIAL4_OPTIONS 8 +NET_ENABLE 1 +NET_P1_TYPE 3 +NET_P1_PROTOCOL 2 +NET_P1_IP0 192 +NET_P1_IP1 168 +NET_P1_IP2 144 +NET_P1_IP3 100 +NET_P1_PORT 14554 + +SYSID_THISMAV 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat index bd77ac79c934f3..e24d8c5299effb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat @@ -7,6 +7,8 @@ MCU STM32H7xx STM32H757xx define CORE_CM7 define SMPS_PWR +env OPTIMIZE -Os + # crystal frequency OSCILLATOR_HZ 24000000 @@ -39,7 +41,8 @@ PA6 RSSI_IN ADC1 SCALE(2) # CAN config PB14 GPIOCAN2_TERM OUTPUT HIGH - +PC12 GPIOCAN1_SHUTDOWN OUTPUT LOW +PF1 GPIOCAN2_SHUTDOWN OUTPUT LOW PA12 CAN1_TX CAN1 PB8 CAN1_RX CAN1 @@ -118,8 +121,8 @@ PA7 HP_UNIDIR_ENABLED OUTPUT HIGH GPIO(5) # UART connected to FMU, uses DMA -PE7 UART7_RX UART7 SPEED_HIGH -PE8 UART7_TX UART7 SPEED_HIGH +PE7 UART7_RX UART7 SPEED_VERYLOW LOW_NOISE +PE8 UART7_TX UART7 SPEED_VERYLOW # UART for SBUS out PC7 USART6_RX USART6 SPEED_HIGH LOW @@ -144,13 +147,16 @@ PD4 USART2_RTS USART2 SPEED_HIGH PD3 USART2_CTS USART2 SPEED_HIGH # order of UARTs -SERIAL_ORDER UART7 UART8 USART3 USART6 UART4 USART2 +SERIAL_ORDER UART8 UART7 USART3 USART6 UART4 USART2 # use 2 MBaud when talking to primary controller -define DEFAULT_SERIAL0_BAUD 2000000 +define DEFAULT_SERIAL1_BAUD 8000000 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_PPP define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_Sbus1 define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_RCIN +define AP_NETWORKING_BACKEND_PPP 1 + # only use pulse input for PPM, other protocols # are on serial define HAL_RCIN_PULSE_PPM_ONLY From 028017e38ef609e3def1548599f3d1fd840797d4 Mon Sep 17 00:00:00 2001 From: Luca Pescante Date: Sat, 7 Oct 2023 18:43:21 +0800 Subject: [PATCH 07/77] ArduCopter/RC_Channel: add option 219 (Transmitter Tuning) --- ArduCopter/RC_Channel.cpp | 4 ++++ libraries/RC_Channel/RC_Channel.cpp | 1 + libraries/RC_Channel/RC_Channel.h | 1 + 3 files changed, 6 insertions(+) diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 3cb3b99ea26080..c62f8b6d2bcd07 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -128,6 +128,7 @@ void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwi case AUX_FUNC::FORCEFLYING: case AUX_FUNC::CUSTOM_CONTROLLER: case AUX_FUNC::WEATHER_VANE_ENABLE: + case AUX_FUNC::TRANSMITTER_TUNING: run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -648,6 +649,9 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc break; } #endif + case AUX_FUNC::TRANSMITTER_TUNING: + // do nothing, used in tuning.cpp for transmitter based tuning + break; default: return RC_Channel::do_aux_function(ch_option, ch_flag); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 8d27aeb426eb10..e0bdfed8e43b64 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -247,6 +247,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 210:Airbrakes // @Values{Rover}: 211:Walking Height // @Values{Copter, Rover, Plane}: 212:Mount1 Roll, 213:Mount1 Pitch, 214:Mount1 Yaw, 215:Mount2 Roll, 216:Mount2 Pitch, 217:Mount2 Yaw + // @Values{Copter}: 219:Transmitter Tuning // @Values{Copter, Rover, Plane}: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 // @User: Standard AP_GROUPINFO_FRAME("OPTION", 6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE|AP_PARAM_FRAME_BLIMP), diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 99959480704dd2..97dddce75b0d87 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -271,6 +271,7 @@ class RC_Channel { MOUNT2_PITCH = 216, // mount3 pitch input MOUNT2_YAW = 217, // mount4 yaw input LOWEHEISER_THROTTLE= 218, // allows for throttle on slider + TRANSMITTER_TUNING = 219, // use a transmitter knob or slider for in-flight tuning // inputs 248-249 are reserved for the Skybrush fork at // https://github.com/skybrush-io/ardupilot From a519597249397e31fc492b3907ed1949a1a1920c Mon Sep 17 00:00:00 2001 From: Luca Pescante Date: Sat, 7 Oct 2023 18:51:05 +0800 Subject: [PATCH 08/77] Copter: find channel option "Transmitter Tuning" instead of CH6 --- ArduCopter/tuning.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index c2cea3c72f2c38..a0bb8f7b829e90 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -1,33 +1,34 @@ #include "Copter.h" /* - * Function to update various parameters in flight using the ch6 tuning knob + * Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob * This should not be confused with the AutoTune feature which can be found in control_autotune.cpp */ -// tuning - updates parameters based on the ch6 tuning knob's position +// tuning - updates parameters based on the ch6 TRANSMITTER_TUNING channel knob's position // should be called at 3.3hz void Copter::tuning() { - const RC_Channel *rc6 = rc().channel(CH_6); + const RC_Channel *rc_tuning = rc().find_channel_for_option(RC_Channel::AUX_FUNC::TRANSMITTER_TUNING); + // exit immediately if tuning channel is not set + if (rc_tuning == nullptr) { + return; + } + // exit immediately if the tuning function is not set or min and max are both zero if ((g.radio_tuning <= 0) || (is_zero(g2.tuning_min.get()) && is_zero(g2.tuning_max.get()))) { return; } // exit immediately when radio failsafe is invoked or transmitter has not been turned on - if (failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) { - return; - } - - // exit immediately if a function is assigned to channel 6 - if ((RC_Channel::AUX_FUNC)rc6->option.get() != RC_Channel::AUX_FUNC::DO_NOTHING) { + if (failsafe.radio || failsafe.radio_counter != 0 || rc_tuning->get_radio_in() == 0) { return; } - const uint16_t radio_in = rc6->get_radio_in(); - float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max()); + const uint16_t radio_in = rc_tuning->get_radio_in(); + float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc_tuning->get_radio_min(), rc_tuning->get_radio_max()); + #if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max); #endif From 0583e851457adabdcdd8ee3767569ebfbe47843d Mon Sep 17 00:00:00 2001 From: Luca Pescante Date: Sat, 7 Oct 2023 18:52:58 +0800 Subject: [PATCH 09/77] Copter: update "CH6 tuning" comments --- ArduCopter/Copter.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 7c9a0e91b3402f..bedc575a1f6b86 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -659,7 +659,7 @@ void Copter::three_hz_loop() // check for deadreckoning failsafe failsafe_deadreckon_check(); - // update ch6 in flight tuning + //update transmitter based in flight tuning tuning(); // check if avoidance should be enabled based on alt diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 8b2578d49b5683..5c1d243ed129e4 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -68,7 +68,7 @@ void ModeCircle::run() } // update the orbicular rate target based on pilot roll stick inputs - // skip if using CH6 tuning knob for circle rate + // skip if using transmitter based tuning knob for circle rate if (g.radio_tuning != TUNING_CIRCLE_RATE) { const float roll_stick = channel_roll->norm_input_dz(); // roll stick normalized -1 to 1 From 9acd23d196b63446195b6f51ff67868ee351c30a Mon Sep 17 00:00:00 2001 From: Luca Pescante <72148330+be-ska@users.noreply.github.com> Date: Wed, 24 Jul 2024 12:43:36 +0200 Subject: [PATCH 10/77] autotest: use RC6 for tuning --- Tools/autotest/arducopter.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 17b500367509fc..47f4fcd5cf0e22 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -11270,6 +11270,7 @@ def MAV_CMD_NAV_TAKEOFF_command_int(self): def Ch6TuningWPSpeed(self): '''test waypoint speed can be changed via Ch6 tuning knob''' self.set_parameters({ + "RC6_OPTION": 219, # RC6 used for tuning "TUNE": 10, # 10 is waypoint speed "TUNE_MIN": 0.02, # 20cm/s "TUNE_MAX": 1000, # 10m/s From fe2e75b2a0be533b5ed5a13d915d6b3573e368a1 Mon Sep 17 00:00:00 2001 From: Ep Pravitra Date: Wed, 24 Jul 2024 15:18:42 +0700 Subject: [PATCH 11/77] Plane: option to immediately climb in AUTO mode (not doing glide slope) add comment in Parameters.cpp Update ArduPlane/altitude.cpp Co-authored-by: Peter Hall <33176108+IamPete1@users.noreply.github.com> clarification in FLIGHT_OPTIONS description change the comment param comment change Update ArduPlane/Parameters.cpp Co-authored-by: Peter Barker --- ArduPlane/Parameters.cpp | 1 + ArduPlane/altitude.cpp | 7 +++++++ ArduPlane/defines.h | 1 + 3 files changed, 9 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 65198d9cb53240..7ff30ad4277175 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1096,6 +1096,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces + // @Bitmask: 14: In AUTO - climb to next waypoint altitude immediately instead of linear climb // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 8b7970e6315d1a..6e32f619439d5b 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -81,6 +81,13 @@ void Plane::setup_glide_slope(void) break; case Mode::Number::AUTO: + + //climb without doing glide slope if option is enabled + if (!above_location_current(next_WP_loc) && plane.flight_option_enabled(FlightOptions::IMMEDIATE_CLIMB_IN_AUTO)) { + reset_offset_altitude(); + break; + } + // we only do glide slide handling in AUTO when above 20m or // when descending. The 20 meter threshold is arbitrary, and // is basically to prevent situations where we try to slowly diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index f4a8f9fc45111e..b2939557adfdf5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -168,6 +168,7 @@ enum FlightOptions { DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), + IMMEDIATE_CLIMB_IN_AUTO = (1<<14), }; enum CrowFlapOptions { From 66817e207fa5f48ac11748d3fe2a29388d627065 Mon Sep 17 00:00:00 2001 From: James O'Shannessy <12959316+joshanne@users.noreply.github.com> Date: Wed, 17 Jul 2024 17:33:14 +1000 Subject: [PATCH 12/77] AP_InertialSensor: Check the gyro/accel id has not been previously registered If the Gyro/Accel ID is already in the registered list, do not try to add it again. This stops an issue seen on a CubeOrangePlus BG3 where, during the very first boot after a parameter wipe, software incorrectly registers a fourth IMU. The Fourth IMU is registered because the AUX IMU is the same DevID as the third ICM45686. --- .../AP_InertialSensor/AP_InertialSensor.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index ffe9c7f38ed7d2..6d254dccec9071 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -736,6 +736,14 @@ bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rat return false; } + // Loop over the existing instances and check if the instance already exists + for (uint8_t instance_to_check = 0; instance_to_check < _gyro_count; instance_to_check++) { + if ((uint32_t)_gyro_id(instance_to_check) == id) { + // if it does, then bail + return false; + } + } + _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz; _gyro_over_sampling[_gyro_count] = 1; _gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000); @@ -796,6 +804,14 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra return false; } + // Loop over the existing instances and check if the instance already exists + for (uint8_t instance_to_check = 0; instance_to_check < _accel_count; instance_to_check++) { + if ((uint32_t)_accel_id(instance_to_check) == id) { + // if it does, then bail + return false; + } + } + _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz; _accel_over_sampling[_accel_count] = 1; _accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS); From 84ff78f5c6ace6d2e3b332dae166e84b196f4dcf Mon Sep 17 00:00:00 2001 From: muramura Date: Wed, 26 Jun 2024 21:27:17 +0900 Subject: [PATCH 13/77] AP_Notify: Perform common checks first --- libraries/AP_Notify/RGBLed.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Notify/RGBLed.cpp b/libraries/AP_Notify/RGBLed.cpp index 944fb1d0e82ac5..caa549fd94cae9 100644 --- a/libraries/AP_Notify/RGBLed.cpp +++ b/libraries/AP_Notify/RGBLed.cpp @@ -165,16 +165,13 @@ uint32_t RGBLed::get_colour_sequence(void) const if (!AP_Notify::flags.pre_arm_check) { return sequence_prearm_failing; } - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && - AP_Notify::flags.pre_arm_gps_check && - good_ahrs_location) { - return sequence_disarmed_good_dgps_and_location; - } - - if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && - AP_Notify::flags.pre_arm_gps_check && - good_ahrs_location) { - return sequence_disarmed_good_gps_and_location; + if (AP_Notify::flags.pre_arm_gps_check && good_ahrs_location) { + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) { + return sequence_disarmed_good_dgps_and_location; + } + if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) { + return sequence_disarmed_good_gps_and_location; + } } #endif From 2ab3d0b3b0a337b444e9cce24c567a9fe28598f3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 23 Jul 2024 17:01:53 +0930 Subject: [PATCH 14/77] AC_AttitudeControl: Add Landed Gain Backoff --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 40 +++++++++++++++++++ .../AC_AttitudeControl/AC_AttitudeControl.h | 11 +++++ 2 files changed, 51 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 0465ece670ec68..14d3a1fc7f32d3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -150,6 +150,27 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = { // @User: Standard AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT), + // @Param: LAND_R_MULT + // @DisplayName: Landed roll gain multiplier + // @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0), + + // @Param: LAND_P_MULT + // @DisplayName: Landed pitch gain multiplier + // @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0), + + // @Param: LAND_Y_MULT + // @DisplayName: Landed yaw gain multiplier + // @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis. + // @Range: 0.25 1.0 + // @User: Advanced + AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0), + AP_GROUPEND }; @@ -204,6 +225,25 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly() get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC); } +// Reduce attitude control gains while landed to stop ground resonance +void AC_AttitudeControl::landed_gain_reduction(bool landed) +{ + if (is_positive(_input_tc)) { + // use 2.0 x tc to match the response time to 86% commanded + const float spool_step = _dt / (2.0 * _input_tc); + if (landed) { + _landed_gain_ratio = MIN(1.0, _landed_gain_ratio + spool_step); + } else { + _landed_gain_ratio = MAX(0.0, _landed_gain_ratio - spool_step); + } + } else { + _landed_gain_ratio = landed ? 1.0 : 0.0; + } + Vector3f scale_mult = VECTORF_111 * (1.0 - _landed_gain_ratio) + Vector3f(_land_roll_mult, _land_pitch_mult, _land_yaw_mult) * _landed_gain_ratio; + set_PD_scale_mult(scale_mult); + set_angle_P_scale_mult(scale_mult); +} + // The attitude controller works around the concept of the desired attitude, target attitude // and measured attitude. The desired attitude is the attitude input into the attitude controller // that expresses where the higher level code would like the aircraft to move to. The target attitude is moved diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 1883546c5632d1..b3968282580d51 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -163,6 +163,9 @@ class AC_AttitudeControl { // reset rate controller I terms smoothly to zero in 0.5 seconds void reset_rate_controller_I_terms_smoothly(); + // Reduce attitude control gains while landed to stop ground resonance + void landed_gain_reduction(bool landed); + // Sets attitude target to vehicle attitude and sets all rates to zero // If reset_rate is false rates are not reset to allow the rate controllers to run void reset_target_and_rate(bool reset_rate = true); @@ -479,6 +482,11 @@ class AC_AttitudeControl { // rate controller input smoothing time constant AP_Float _input_tc; + // Controller gain multiplyer to be used when landed + AP_Float _land_roll_mult; + AP_Float _land_pitch_mult; + AP_Float _land_yaw_mult; + // Intersampling period in seconds float _dt; @@ -561,6 +569,9 @@ class AC_AttitudeControl { // PD scale used for last loop, used for logging Vector3f _pd_scale_used; + // ratio of normal gain to landed gain + float _landed_gain_ratio; + // References to external libraries const AP_AHRS_View& _ahrs; const AP_MultiCopter &_aparm; From 1012be95d86930f030b4fefd632be648142c2f62 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 23 Jul 2024 17:02:04 +0930 Subject: [PATCH 15/77] Copter: Ground oscillation gain reduction --- ArduCopter/mode.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 19de3725dbbeba..3a851eee911595 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -439,6 +439,7 @@ void Copter::update_flight_mode() #if AP_RANGEFINDER_ENABLED surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used #endif + attitude_control->landed_gain_reduction(copter.ap.land_complete); // Adjust gains when landed to attenuate ground oscillation flightmode->run(); } From 297fcfd77787bfb532bbace3b3f746d2a93af831 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Tue, 21 May 2024 06:52:45 -0700 Subject: [PATCH 16/77] Sub: handle MAV_CMD_DO_REPOSITION --- ArduSub/GCS_Mavlink.cpp | 43 +++++++++++++++++++++++++++++++++++++++++ ArduSub/GCS_Mavlink.h | 1 + ArduSub/mode.h | 1 + 3 files changed, 45 insertions(+) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c89771e18c9a3d..a26dbd24ad37d9 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -483,6 +483,46 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) return MAV_RESULT_ACCEPTED; } +MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_do_reposition(const mavlink_command_int_t &packet) +{ + const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; + if (!sub.flightmode->in_guided_mode() && !change_modes) { + return MAV_RESULT_DENIED; + } + + // sanity check location + if (!check_latlng(packet.x, packet.y)) { + return MAV_RESULT_DENIED; + } + + Location request_location; + if (!location_from_command_t(packet, request_location)) { + return MAV_RESULT_DENIED; + } + + if (request_location.sanitize(sub.current_loc)) { + // if the location wasn't already sane don't load it + return MAV_RESULT_DENIED; // failed as the location is not valid + } + + // we need to do this first, as we don't want to change the flight mode unless we can also set the target + if (!sub.mode_guided.guided_set_destination(request_location)) { + return MAV_RESULT_FAILED; + } + + if (!sub.flightmode->in_guided_mode()) { + if (!sub.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) { + return MAV_RESULT_FAILED; + } + // the position won't have been loaded if we had to change the flight mode, so load it again + if (!sub.mode_guided.guided_set_destination(request_location)) { + return MAV_RESULT_FAILED; + } + } + + return MAV_RESULT_ACCEPTED; +} + MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { switch(packet.command) { @@ -496,6 +536,9 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_ case MAV_CMD_DO_MOTOR_TEST: return handle_MAV_CMD_DO_MOTOR_TEST(packet); + case MAV_CMD_DO_REPOSITION: + return handle_command_int_do_reposition(packet); + case MAV_CMD_MISSION_START: return handle_MAV_CMD_MISSION_START(packet); diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 4ca41e1d8c9e84..c38ec3f4abb6ef 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -22,6 +22,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; + MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); // override sending of scaled_pressure3 to send on-board temperature: void send_scaled_pressure3() override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 4228d0327343c7..11a64471675114 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -320,6 +320,7 @@ class ModeGuided : public Mode bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return true; } bool is_autopilot() const override { return true; } + bool in_guided_mode() const override { return true; } bool guided_limit_check(); void guided_limit_init_time_and_pos(); void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); From 872cbc72ffcb3b6425685f6b4dc004c791bce315 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Wed, 24 Jul 2024 13:09:44 -0700 Subject: [PATCH 17/77] autotest: MAV_CMD_DO_REPOSITION in sub --- Tools/autotest/ardusub.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 635785a22897f0..ffe3ef37a36e05 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -739,6 +739,35 @@ def MAV_CMD_CONDITION_YAW(self): self._MAV_CMD_CONDITION_YAW(self.run_cmd) self._MAV_CMD_CONDITION_YAW(self.run_cmd_int) + def MAV_CMD_DO_REPOSITION(self): + """Move vehicle using MAV_CMD_DO_REPOSITION""" + self.wait_ready_to_arm() + self.arm_vehicle() + + # Dive so that rangefinder is in range, required for MAV_FRAME_GLOBAL_TERRAIN_ALT + start_altitude = -25 + pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700 + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120) + self.set_rc(Joystick.Throttle, 1500) + self.change_mode('GUIDED') + + loc = self.mav.location() + + # Reposition, alt relative to surface + loc = self.offset_location_ne(loc, 10, 10) + loc.alt = start_altitude + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT) + self.wait_location(loc, timeout=120) + + # Reposition, alt relative to seafloor + loc = self.offset_location_ne(loc, 10, 10) + loc.alt = -start_altitude + self.send_do_reposition(loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) + self.wait_location(loc, timeout=120) + + self.disarm_vehicle() + def TerrainMission(self): """Mission using surface tracking""" @@ -877,6 +906,7 @@ def tests(self): self.MAV_CMD_MISSION_START, self.MAV_CMD_DO_CHANGE_SPEED, self.MAV_CMD_CONDITION_YAW, + self.MAV_CMD_DO_REPOSITION, self.TerrainMission, self.SetGlobalOrigin, self.backup_home, From cbef0944353d256967ed0967e8b74fcac265e8b3 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Wed, 24 Jul 2024 10:14:32 -0700 Subject: [PATCH 18/77] autotest: test mag fusion --- Tools/autotest/ardusub.py | 70 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 68 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index ffe3ef37a36e05..e33b4a588b181e 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -36,6 +36,22 @@ class Joystick(): Lateral = 6 +# Values for EK3_MAG_CAL +class MagCal(): + WHEN_FLYING = 0 + WHEN_MANOEUVRING = 1 + NEVER = 2 + AFTER_FIRST_CLIMB = 3 + ALWAYS = 4 + + +# Values for XKFS.MAG_FUSION +class MagFuseSel(): + NOT_FUSING = 0 + FUSE_YAW = 1 + FUSE_MAG = 2 + + class AutoTestSub(vehicle_test_suite.TestSuite): @staticmethod def get_not_armable_mode_list(): @@ -850,7 +866,7 @@ def SetGlobalOrigin(self): # restart GPS driver self.reboot_sitl() - def backup_home(self): + def backup_origin(self): """Test ORIGIN_LAT and ORIGIN_LON parameters""" self.context_push() @@ -881,6 +897,55 @@ def backup_home(self): self.disarm_vehicle() self.context_pop() + def assert_mag_fusion_selection(self, expect_sel): + """Get the most recent XKFS message and check the MAG_FUSION value""" + self.progress("Expect mag fusion selection %d" % expect_sel) + mlog = self.dfreader_for_current_onboard_log() + found_sel = MagFuseSel.NOT_FUSING + while True: + m = mlog.recv_match(type='XKFS') + if m is None: + break + found_sel = m.MAG_FUSION + if found_sel != expect_sel: + raise NotAchievedException("Expected mag fusion selection %d, found %d" % (expect_sel, found_sel)) + + def fuse_mag(self): + """Test EK3_MAG_CAL values""" + + # WHEN_FLYING: switch to FUSE_MAG after sub is armed for 5 seconds; switch to FUSE_YAW on disarm + self.set_parameters({'EK3_MAG_CAL': MagCal.WHEN_FLYING}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + self.arm_vehicle() + self.delay_sim_time(10) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + self.disarm_vehicle() + self.delay_sim_time(1) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + + # AFTER_FIRST_CLIMB: switch to FUSE_MAG after sub is armed and descends 0.5m; switch to FUSE_YAW on disarm + self.set_parameters({'EK3_MAG_CAL': MagCal.AFTER_FIRST_CLIMB}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + altitude = self.get_altitude(relative=True) + self.arm_vehicle() + self.set_rc(Joystick.Throttle, 1300) + self.wait_altitude(altitude_min=altitude-4, altitude_max=altitude-3, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + self.disarm_vehicle() + self.delay_sim_time(1) + self.assert_mag_fusion_selection(MagFuseSel.FUSE_YAW) + + # ALWAYS + self.set_parameters({'EK3_MAG_CAL': MagCal.ALWAYS}) + self.reboot_sitl() + self.wait_ready_to_arm() + self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG) + def tests(self): '''return list of all tests''' ret = super(AutoTestSub, self).tests() @@ -909,7 +974,8 @@ def tests(self): self.MAV_CMD_DO_REPOSITION, self.TerrainMission, self.SetGlobalOrigin, - self.backup_home, + self.backup_origin, + self.fuse_mag, ]) return ret From 6a13432a21c63463a8ff15dd7386c729041828f1 Mon Sep 17 00:00:00 2001 From: rishabsingh3003 Date: Tue, 23 Jul 2024 10:15:45 -0400 Subject: [PATCH 19/77] AP_Periph: Fix compiling issues with AP_PERIPH_PROBE_CONTIONUS enabled --- Tools/AP_Periph/compass.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/AP_Periph/compass.cpp b/Tools/AP_Periph/compass.cpp index 2c70ae1cc04ffb..5a4cd5eb842af0 100644 --- a/Tools/AP_Periph/compass.cpp +++ b/Tools/AP_Periph/compass.cpp @@ -20,6 +20,8 @@ #define AP_PERIPH_MAG_HIRES 0 #endif +extern const AP_HAL::HAL &hal; + /* update CAN magnetometer */ From ace57047faff545bbac88fac62224c965d3c2f28 Mon Sep 17 00:00:00 2001 From: muramura Date: Sun, 14 Jul 2024 23:10:24 +0900 Subject: [PATCH 20/77] Copter: Distinguish preprocessor processing --- ArduCopter/AP_Arming.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index c711b06f5197ee..a003f23756b8bf 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -341,10 +341,10 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) { // check if fence requires GPS bool fence_requires_gps = false; - #if AP_FENCE_ENABLED +#if AP_FENCE_ENABLED // if circular or polygon fence is enabled we need GPS fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif +#endif // check if flight mode requires GPS bool mode_requires_gps = copter.flightmode->requires_GPS() || fence_requires_gps || (copter.simple_mode == Copter::SimpleMode::SUPERSIMPLE); @@ -443,10 +443,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) // check if fence requires GPS bool fence_requires_gps = false; - #if AP_FENCE_ENABLED +#if AP_FENCE_ENABLED // if circular or polygon fence is enabled we need GPS fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0; - #endif +#endif if (mode_requires_gps || copter.option_is_enabled(Copter::FlightOption::REQUIRE_POSITION_FOR_ARMING)) { if (!copter.position_ok()) { From 14d94552eb8c2c8ef424a66e17105cba18a5a0ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 10 Jul 2024 16:41:21 +1000 Subject: [PATCH 21/77] Tools: update BotBloxSwitch bootloader --- Tools/bootloaders/BotBloxSwitch_bl.bin | Bin 92588 -> 92700 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Tools/bootloaders/BotBloxSwitch_bl.bin b/Tools/bootloaders/BotBloxSwitch_bl.bin index 77f7357d05cdb3dc132c292de5b754d199ee70e0..f183d8d66dddadd117e462291a13d29fa0265303 100755 GIT binary patch delta 21314 zcmb`vd3aPs_BUSDOL9AFhY+%pUJ}v}0v*Bh7J`GqTeIJrwv574&dRBfSbVo5p>^f=+kS|1rrr?W-1H- z%ml0lth_{oJ%G~y`fnoS0`3Q_1aJ;}4s?c1zl9qvrEaJ;C{x}i{D7gd_Dcan%O z{0(+h^_ccWU!3#=o2?$LeG{=W>|J$s?-!5qjuSJ05J9+3E)t43aMmg+aQx2z3Gk|& zsWb)QJput&tn@1zsc|Mu3#g%!W%B?rTr{iJAgK^;o8I;vu}Vit>X0} zbt}o#OUt}HL}JiOUA-|*BE73nsVYb$S5S}gXIQn?JXhl-mVdR5tSWv+^$gjdDh)5A z(hcu%7ir-Qpd`E=ZLGHqqC$@$y86cu>KlbInht?PNM{0?j{XQw1q`C5TvOJ3MPC>v zHMMk>{rOMC(5^uT8>zI7>B3U8-|?&Krz;w7L8H~Y>So;9#8S0A(itmt_=!28yzr{=pUq#mS{Ub*;LN zk-e1s9sNkV@aO7j!xF2X;_r%#QPEjM`f5ZYur5d#Iy# z?2R^RsFa9~c*5n;S0f{5cwX>QDTM`fgR(DrRfeFPAIR|hMJW`ibj7O?>z(Q?h!5P+ zQK*_wG+`u_ZnNnT$;qp|dRLZ6K_3^$tf;f^D+)hrL7Dg2OA!O;2=*!dXEJZZ{NY!; zbZgL9jY&-KRbJh#{^I6ys;A39%c#`V7bX(Rh+B`mOY5}PR%!EHyafaN9QVV1Dm}<{ zMjD5g=Fcygs2GN6S)r&gY4>ObQR)6*jp%X}pvEY#w)TeWxYD%-Jw-o1^Qc_J{Gd^@ zTZ0-J8I4Mun*o}i{(#-#A(rY}ukPoyxqQ5~SN3Z*Qz_beHPnI&A>4~-L7Mkc2y-fg zS+!rgrz=K05ldNP)YNKF-x|+gvB}jB*jcPPYG}oF&u=3>-O~}8k)XUaRJsiOzC_I> ztr2xJKeAOYMeO0rVFs4xw9M;l{zxwUBDQLiWt zL!TdI4@A#U-SDtO(S5p|^HknCwwM?V$T;PR?&vQ>NZq{57u}l_AU=L!bWjkV$sl<-p|7>@k%yX$ zVm4Z(X2#eomDR&aWABNG@Pu{f2$ceERvViX`MG)}dc#X3CpN z@B80VdTXilxU$UI8LLsKSlZMXf0h-;<Qto{0v(LDWf3MN`W6 zHjx6ULe>_S&~2DPEM;xxXH+??IZ168CpYuO-Ggd}c1fqZ*yJu|+Rj#V zc`0wSqUrdB_7W$Fc_mbeQJ%8LH&Fw@k@Sjkf)MHPphmPdSr-#&YtSGPX*wGoKY}K* z74aE#INKI)j=VpFPYP0LG5achazeXDvGG+G*iTIZO4cY>3_3|>gS)mzMg%XKdWxLO zAeNhuk>K+~P8!diOEA*u zY)?Xd_1dwy*Htg9eCwNbGIv9L>(^Jl5R}z^-P^?cz0&o7z}``MTXk%< zjhHJX8WG)*pwjM{B(7Hytm~Do+t*(wFX*Jp{%uNEe(Lusly=ov#7L=2K!crH-1fb4 z%Bhon4V+hqb#u|slRBwApso$df35ab-uhyRdTYJ+W(W1gt5vtVE!2BfNna_1MD6}w z+3u`Uh@SN9+F}+bis}vP6@xV>ulK6AMD2_2Fp`ft^s3~N69>Y2Sz5wO>X(T$Dxlsf z?6ZnkT>S{Ux8lSB`trSEP%iVoppZhH^iE)0`M5Fh_D@n8Za-OFoqJMm(@DDmDlt;z zE#4g9{}1|w2J@5A9oES2mCPL^r)!JatTI2ZXstSFeE{d>5^9<(qS4ad(a`ajgFC?? z6*UJ6Z`|LdOg!FB>I+e&-fw<#ANu`R0B2{!DzWm`^NTksXUX^yBB_bWONv>waz($W zA)N^M^lVkG7>KS^?`Z{725Tz&J;su!^NUP>1Arl{RS(gmVBx~aqV3K)Ey z)J=)rxABUatO9Jq1V!z!U`mErg}Nwqp*_qTC25sx*Gc7~zRYR1@4H5+C#RsrW7g(7 zuNB9Baxy4ae}V~kelf8$w|(T|`@1lGhoQ&HUXdN2z4cM6U5Lf~9r}A38{#p*(ecEv zwt^TwM94SEGQi^iejdcWXKnnX{%I32if@~D-=}D%63OW&#-%3hZp{WF?em+(Ug8vE zbjN<8-l&rr{ZqPGG;d=;sVps13`Vs59b!f=m$R8k33MJ?nq;&J(mmLn`euEju2eTh zC-qfy<;0*X(@Xc^PS#0tkgF*V3*F3msr&=>MNe}l;P{PTaYq+l~ORD znxgik?ar%=kx~?uIc`ti(iHJ~=Eyzrn8D>p#f>V^k;toaBb{D>Y(ZuaBOMC2hG$J z-4RZ$STyx+YKdywri5Fy3LTo0s?PcK!eG&oK_rH2xV_CFs_UiZ_`y`jZ4Zvrhz5fo zP4a3*y^|Or!nntqvLU==SAMVTuA$OIPgf%sdoH*;!ynxn6{|H&e9B48s}(C_X*tzB zyaAc`NuP7A0(fP3hnYr-L&Xo8U{BjdBy@RI0|Fc{FI4XK;Qp-#jZu zdd97|I&nuJ`c(h%S78--HBS=VmqXz9L^+ZcgDE(2-HBUr^!8XjK z%PZrRkc}}?X&}5}m?^x>X8R2@BA~)9AT1M8iC_S;p6Nue6tG9rhgIr>IbuH?bTx}0 zS8UQrK95ny<&F;cEii}yf` z)YE54*+40`(nv8Lxr3G#5`nRGHth#&h@TH2jyY)hXq^7a0 zbnB*~s1qM`XjEE^Y|#zZHy%Th%}u1kewDbaiJBwP z$LEwh+-=#xhfG3QA$oQ$7>$wPr@uBd0x7{en9cqe@Nt0YfMb9{fK-k1YfZft!&DLo z6GxbamT|e@a4_67q>K;JX?JT=0b+XT74L3moK-Jf#Kfp@;|1k-46J|!PI{NM@={Mh z0?}fcq(VX3)s$+=32}JOy}POKhP#Wlqkm|*-SBHW#!x||IM+XOSJN%3kSMY;$k*L3 z{?o2VQ}#uerFe?0(apJvx!eKMU!9f=ggzKZ_e&Bi%2i|RrL$8zH6tk&kWOO zJu@3u^?cD|sIl73YgZLTogrCa4ybMjCe*!sX~=xIrT(5QC~B)t&@w*SsEF4Y9?|{BQ{&-@Hb7b!pO~qEWSYk&iz(ontgw5s#`lTB z4bAsh9A4*<<08jW?8Ra3{Ixrav}Y}LLkaC4Bh@Qd$-rCP>}Xa&wL>wdt4szQi68ce zlxmgP>1vY+sYvO0WhdfUq4;XZ)`D7ZvV>AAkkYFKQ$Z;81X6KG6^2p^km`a|X((k! zs;k;y8Xro{L@G&LXj&LbO+m^q9HjN3hU(L_o{h5frn@9IcNr<`zUZsf<043!f)1cCOCkOILUT%D(_ zAmYtsGK;j9+CEg&`Upu1n$ziKJ^&HY)!=NzdG{lvAA&nW@hrr@3)0QMbw5J-CKztu zM{sVtQXGSuiWn0`tN4^D$kt~^M;{B$Pt%&zI2|LT57=ATx%4e|Iy*PHHdrj4a%nqE zYTh<%dfv7H5z=awYRT@=Kd>Z?n!=5QSwv!vka`DBH8p^bfUzLVEJkNWNTvX@TS{n< zy<^EuxZ+=ucB)A*@l1VY9L^Fnll^Kbrbk(R&%SXN1D`d;_L!X(9%2Di3R}{1Fg?QF z>1mAL8~C8f+Jkop!-xtZ@KU+dvoBr8;(BFw8|?oURqIUpyHzKn>b`z9u9v8a^s`rb zjmdb%&)*gKZ4p`d5-c}+pCJxXls~M%j7=k4=(ppYf=j} zbP9{a;K4i=fL z$XM~k`eacf*5af-60|nON_RYwX0;9S)_|gmyQ=hN&3?T}#pb8sl7pxsw9Kgr%0IbP z`-(6lgZW!zwt$8%@MybdUNEH#@8*j_KlN8fii#8=sz}kKbr)~9?X04e`KhgXvwH7f z(N?8qVNyGk4_`wsWtM^}p$`AmTZ)>6F3`Gv7k zXyhZK#n{Don8j)^+>g^p23yr$W4f(M1-7RY5Qx`X4Tf;3#Lev1%&a<}LF}; zS3E8<@-nR@(iMf&nMAsW)~XC)QWbMr6X=udb89*s&3?4@jcf~Wao$!H21QmzUTSq$ zcbr8f8ryn`ba$-ux@S&zV%GML88=UiE~C5oLzS>t=|@kEQ;!Eqtn_bh&=PS5(p>C3 zF%J?$tn`iN?V?|y=pq-1Aq*FQ6;}r|WFi$Q1lAtz-dIpdBl68OzmS|i5R>C+i$WR(cJ@4 zO~5p7Ps8l88TpSV^U@muSOLY))o&4FF#lT6x3TN_4T&X{JVz;843j)8e&BNL7bvrr zZ5U`yAFdQa4Arh2wX%4LW(fq&fzme8*zE1Sic|nzH^1@8A^q200I~^l$i>~Meu0SFe;21#{My6lIoV1S%$tCF$ox+vtSte zVCWS36iXg9G&DXhvdP0H>gq@GB@`wNV2#7_!mZ8QLH;qjJ}i-rV%p(-bv%6vaFGok zo(S#P{r`nqKYVWdZxl)+Kj^z+o{zEcdln_|x#ipo(eVOo8ooMKvi0{Q$33Qy+%c^= zD@e{*Eaq@&7&~~+9$n88B5+;`s~RyifyXSvsW1jm(5MwrUOLatj+kiVId)+A`Yd%8 z+68A&e#r}2(ZE$>;nE5=wy-Ci#+DXlg=lL-95iAyI4lZkoLMsqnhQ{tr>j_VVQL)b z`1T*DCbIUz)ZrX^>JQlA!TX%%6*NcNrYSGH(~YX}XvkNNDYfGZLZjyU18p`NIWn21 zu&E={!#JPJd)PB0O`&MXJ?w8IhlHZGdm4Tk8Ai>VV(mRtcoXm~;5OhG01vg%eD7$b zLKa)x7Ag}pg8fw7S-fxr>rv9DlhX(`rR2r%`}_pMX=K++vW?MhjhJ*q2SxD5kX6Ga zEz)P21f&m#(tb8zRCa8;q7j9LS8?7LM8B+I^GEHV#~Y%>c&Z7<%b8`4?rr7G+c*M? z5>SB290qtAz$cbZa0$ZW$m0{Dl>}cy&FFEI+8X{@dRmpo3y%lE6%fP~Qeg<-IN%|` zX26GFFc$fou?g{I4W}y((k|N(RWd12K*H;rFp|AC_B-ttKwoDo$5n^%Sl1#JRyi&d zwHC1{m4_oZwx);*lZsgP@mF;`xgE)-hPVk{RSHj@K=R*!4!|@prp4Id#SJ~~JD^J7 z`HPTV2>fFJp8rzA^*c`Fl_+K3A`i{>F+YO`xwgY?@R_L+yHxwNK8_eGQ^- z0asYulz45Q60D(yo>N{|C4?i&xlBS>4d5N#+i+o8j=D>T`6w!s0%b%vXjDUr?PXOD zjxR_0Vc=f|5GWBQA^dF;__*3fu@kdC(5?pAUbboWb2=Vt1^mjg9?Yln+2jWuF&u|i zw2%oHC9=SS18F52Feg8ZXG{~>qjR1Jaj0eOb4H|d{0}pT93*d3c zr`gu5476@5vb3a&%|*;=Ps*OT@<|Y=VMp=FulY1}+lp$W2Ue24L>ja5kRsGJb&yyh zjb;n%IjPnDRgkHOv}0v>T|cqVJ~`Pd=lLgL;eN;(?OF7DcHW+sIsu#teNOSTi<62FlJIl>r*;PYlr24sSW>{V;Cjc&Q1cQVLL4KlV{oLUnd9vMx-t9!U~? za!X+L3&m26diJWr=!>d#JaCPHBcV8Nd$#vIPe@}oT5ywzwgIXRk%}cEHnE_I#^c zU|W{_NUVZS^kcGfh84SD=6ImnIJV8PwqQ!2aN_x`wUPn!(_GCCFrDt4NsaIa) zS4rFa{rGFYglmn0@@p-e-zk95sFG%~<4@$%Tt=7j{{xohBt(tH7=bGnoK=$E7d=iT z4P}onHABz1Wof$ZXRp~l)TxrXv6iKkkRW20rH5sr%($_v-?IJmM|NphVl+og1Iz=| zHE5TsX);HB3%Cj3XH*o1um){D*kF88M>At!5g@ZQ9X(W&xe|m;MArN6NupPd4*cli zg@=t}26+;ZRGj2 z$ls39tG$;PUPD~M7_X`4W2}-sW!h(!({8N#naSEpyTyKpU3exZftTVP_!y#2+TGH- zS$g$In#5*Qk5wgj+5YN7^dD?)O%{%@^)=b_5!P7ajX4D3?*Z2UC6x^yto$3*B|*+G z+iX%^Gn!` zRedO9#?`6XQt)_;ja)quwQX9xAc3cA0j~l!qFY}f+|QENICNhfA}u_3FKb$p9V#cW zAJ?p-Hn!j|332!1Iwh8eThsW3n(metG;I1y9i=B(-r8U{UMyuiE+~Kx@d*h3Jf4Z` z7KixMvF3HxX%XA^{5Cuw%3jEi<^_&|=v%;#Y~2ehv)@;A_10!CSv9@@b!p?e`s3}C zR*C9T^~+Rx3%T61e~IP2n2>nJbDtt?=Vwm$HsP8^R|-Wkl}@wiFAnP2ew7#td47v{FKd2r(AZTMM?2@mX{8MH1B%ID@~Cf~4Np9E z2%Lr1PW+_1_;cZrr}&9I`!0U+UHtj*DW|D4-7_M0G8?%*q1Pl2t^3AVd38>AaE*FE z>4R5ml=NY^{`7@*l~LD3dPQD{*c;)A^08;ucdL%?WJgeq{-eS#!y`{mbVlrpfS!`} zhZ0G3uc9B8R9vL!qj=?AJE95POf(kwWlsqOOjfjqirq18SrUdaj8dsla2UmS@r9I z_h9uc$3sFra+2tgm%5WA8e86`u1j49z0wJuqZ^qG`gv~kX7#$%RFX6Bf)SLj`j^=T zreeYL`Se?oo8Q_~EU>Gjgn;g%dJm87hK;7)Il#&JNv)Pn#pg?E>4Sh3hP!-d@S)kq zMrXR0Qp@t(>bj?FTj3G#l+7b|@qO+hx#!wQ)*0Lxe4eeC=R&}9fW6p!vk;E32#wk^ zVHGN>zDD$Qlgv#z$p_%mJ@92pTXEeBZ%~uV6Tb@kJg)J|F#L0y#zKx z%`0IrcFkT#O%0w^H)pM!s>9%zXRm~~VBWd%gewEWeY&wbW@@fN&3|{zuDaR8m%8P` zPJ4dCM5Tw}YM~1Dz<6R5GKroF{Q za=fk3>ycxyMN?^IHvm#(tB3ca=p;coEYt@t% z-gzibUG9<76=D$Z?%NF-PAfHr;joPr({SAOVq-T%rt6g6O|e*>eMjoi36;X+wtcn@Co1Isbf-nU$LW!qJR_V1 ze7?~2!AI3nrT>mgX(mK!#MGenZr~pV)4(p)u_2+#!}gxX#D zj#!L{4odQF<~uI6)YVTc-ywgkyK`+t`7O48xm0yIO@q*f1I9t6Zu$8jHL5lTsVErrNBMe(~zyEwbR{grQ5|OtmYhs@qoe>P&*|qN+=sKh$}>IQIaN?hlY$B?OF({*C(uz8Ivv8*>F;l?g8CZ&>aA8 z-n@bASC%;U0PK-g(TFB|1$Tgxcbh2O%OA^8z&;O4guaQJJPVpv-xMgZe|bP96^ClS z-TL2C^R&XJ<_abw=p~jPP}h>XZIam|8t^_7h3BhXN)&k0g;P-njCDwKM!|Bs8RDP@AimZXadg)%yq=in(3p(R43HGyWCO zq4@(Z@&iO@T@)9F3wp9*)EjX@p9@d8dqH>NC*8%L57(VaZ0N*2a2IzbJoz+{Zu+>g z>HLA6OI9^$Sl;h>OcCj4A53_!L7}O_)RD-vi$uEYqXYH*_qy)1@llx8 zir(wF-w8Zu4D_~V(FQoosJ*H<+VfBg#TdXa=F_~kN^C4DZ6k1sjIN7x#%!Hp6URpH zjj8Kt-%zx?Eyp*bGK%lII!Ccn zocT>`Is5x4a(s1$`ag1x?o{x9$VBJEyI~k7__iuB2DLb(Ned(R92!?1kG^#AY3n$? zi<1s{9w9gGm-t}X=+H+jb>%b(==H!wI#Bw(7uuuEDYjLv-eMhACBGB+iG$PHDvZKT z=?$0;pyr@_3C6olHLIZ}94fMIVmj8xF=!$OU>khF4sPkL*)Re+#8=pp9bGBWW{ zhc@=gxP1qX$h}WHTu~Wn`XuYu>!&HTZ2h_!7^NGSAvuUSf75++IrI$srY=j@=yAF9O@4VB)4n=Lx594}1p{1I*{D}@vfo5qp97S} zxQcMxj0s{Y++X>vO5~GUL(SaY9i<>7B;5Z!=Qo8X^Mds!MTX@?C%K&PYRjK=A>pSsuzCPPUmP# z;e+S4IE^CD4RBuzdt-r5buSa81TjsX{~ z$Ks3tZj&(zxQpzqZN_fAwF+Q6TAqrhpYBy644cTl+cxHbzxWa_hMD5a1nDK5#8TVr zr-~&(ea*EA9ZLznemVvkL$_Rk<>P`y!ypDAFTrTs%=1&jFWk)^iu`Xv`F{DJ{}6;fq=Z)3qRNKu>iRG^t6EU*fi3&U)=k7Dp-KW}#nshv)+FGPN$1&4b z`1ahqQEnbDg0%dj>kQf%cV9>~VUHG3@(J$F)0>y<2kjL0A$J0uvpp}oD;!H8qp@w< zGwA)Sd3$2t>q;k8d>iNYi!$0dQY=L*s8~anKi`KueHQXHmHx%lJ92YB55zkLe32$T%+!v%pr@{_OK$a z@?=;d&KgW~3s1rSIO;6}oV|13x<9rDvicwkmONzTH*9z-o`x+1kNc)H?A>jmaK1Rc zXMxH(h267vweCK*+L&O_z=8Fvy({&7mBuaVR+-QT}Sa1KqHU+-CysikqUJ} zvvAjo{}VgOQGLtve(`Ihm*LCLefv2-4R+=oX#DlfN~1PCN?ha|cgL1}w5SL6pQYs< zSQa_I@zu^ZQ*S{s(q6Ox@$uaXlxAx1x&Hr^9R0llO$D!_P|*z4NF82Y4QcUVbpD8c zAmkoNTFwBkk{7t{X6^};fCLZ9FIfBg87jjx z7IE;Nq)G_ryBmr_s)ZDs>5s&vpoquEYzGa|>)hv*D64k2migp&*!qK6@mGBo3|V6^ zj3128-8YT9&U|{Xn6@$9q14>!7Hx-~8pI6G8b!N7mMenVEyOaT)x1u9O`z)zyij!I zOyw=ZV!c6@zhV;(C5-%Y@Eg|zILa2<@5okv&(MVQ4qBX98!kav{>*#XuIY#h zGdD$)?=eOL)9kfkG*5W>n0^n(GLjhvOW0rGpDTnMsf?fDTAmK&Ip9x#`G6N$^x+Js z&T|ish70}Uhx=-|;%yt-bJ$EjWycTa&@Ie;*pYn6bH;@iism%_Yc(fa*Z>5MuP$}=tN>Q` z&7%Wo1MAs*5|#Lyv#L#Qx>;AI$UP7W2n!IZOks%4cZZe1A%|=Jt|@X+)AdjA*%d$s za314tnoeDerD+3J%F>y&7>-5m*~?(bY<&^S?VmH(17mkzo|n_K78J{8K997`%|G~A zjr5ZxchLg>`_P zfF^(fFwEduZKZM@E?Y}e6}bE@&yKnVvr8W)&pZPN8pi z0Ge4;NPr0-7oapkh{7rvfwmrr8v*(!1Ah7Kz(&PlB|YXYD~6sfYw<(2)kr_I&@aEm zRva4{TjS^F2lt!nO8oM2cKldRIE~21%++ZDVz!8vXL041U+($A0H>~e1I$au!O_AkmjFD(# zyFapGTami$rkNVd3k#YBq-fg>v&^=98ry|)szoV+ehL81EOz+QG)mYvpN^v2So&wF z@#}GO;mAYdpYSK)Wrr@aiqEn_)A%Zi>O-cj9H$=5EM25Wl@I#ORXNTm3{!SqG}6;B z1UXLr677}8hxtB#CqFyxdZQR-xNPJ1Ur^3G#G7*rJZ}JcqjPejx=JI?b`l)P6d!o_ z<-hr;H32fIUnNgw9iPQVI)F%SD?nB`i~ZcJxg*O#Ht_Qvv@e_fd0(2$)_-oSKH)RY zH!KUv-Qj9uZ!*HA*e?gXaNBJ%&yROfJP4@G+?iPk<+up=a+|g4X7f@>X%0#l>=+t)McwMsD(G)z^>&s zK-`Gz<`)BULUV?}c@xkA&^M}KTbyJc@BH10(Duf<9db1$U2d3c{`PDC4*Z+@Do3w@DxM^3>Bpdp4u{Ns}de0)M$_z3Xc zf5At94gacAcNq@E9Q!TX^wn4>Brbo|Gk&MXFK==GR>NyZJw{ANZQ~ww%m%w9bJQGQ4e< z>Qja7Z~K{!V|6EvXl}{B1{p;8i;(ut*-z~T14>+E`qoY!#>-EVQx_!K2l1R({EO)oA7~#JPHcbC6zq3sV|4W!sP;w4(m?WGWO#S*>FOO{&64s_Qz#Iv;CK=sF9mc_z8Y_CY6Rk zvf{6t`C<5+Cln(%mtnR5r3$BNv7`#oS=dWIB@F)(Ymd~b3i+2h+X4KG9J#TuvcOI( z%Uef`Bbipj9%~&w7T(0ti{7F3Vda(5$U=2TA@tdN>ILl@7%(0)xwVA z|45U5I!x*xme~h_jA4ypAq$(eA*#Q#cdyQ%U0Lk4T$SI;?zxs&T|x>~&{G#nAFAVR zc$Afr!a<;;YMBHH*8I!6b{%c54&NLfusJt!acQy}6MIK_bQPN0#>G7$T9pvhM(*v6xn81@PI@$%*_e29#=t8=e)Thva5HJlM59pX(}6ht3p@o!DDBSO3GvFZ_VwQ54}=QJ_H$! z_ptz8YFsIG&rxbFgw}=M7euiRy&-E^`t6MH+j#HcOG8$CJ8S6of$ddEAy4ZY^4UNh zHU_`Fp@79bxysIUHn8Z%a}KL)dg~4ObCl%*5S5;0J8v8DEc^7fnZ~pB+v$j@9#*j%&`syT{pi9oy8-=n(=r|^GBUIxk5B6=2U|j;4d$D=P~M?Uf1;0Refa$JhfV*asnoXV8qFR2Dbmfp z;?yv4XHnS6r_(IS+2Q~U;+MV~u86*xv(&(ad@2oqkk4ySV1tiF{!YgPN=JmoWBWA6 zE0q2zdd^X1UulmlVK#gh>u^(=n#@bU&t~-Z%F!C3Lm%FI+!;lWr8B^htD?EIk7KHe zrpD`i^QyMPYv)<9d{p!a{=*v^9P3oHvd4B5=PU{uq1N?k%TKfmqtvNM$)h?5{kUDA zP|IS39RU@cWjga91X+9(^&-NBja%{god5@Ix`#*~D|B~<{8iBLteTEfji2E-p{DyI zdV_8}hZ&CNHFRKx{y{jHqI?u^-2g_wLhJ|u-R+Rm9N%kbLSjdNSZ>0P8y`&Z??Fa$ zU+oS#%n_-jqx6p)%IednEgWBlT7WznaM-leLf>+%)6)EC_$0hLnWgjiq_fT=;sHi3&OF`?t@WRNCp-E{2QF%2DF zdG(a$m$rgt-iTI!1klgzY>j=T@;-YlQs|jad!UuI2;@?(R88tgG11?;+79U;ijp8Z@g8F z{~N9P<^s-~XbK;5?1-jI=oWw1@6G^t-o!U+Kt8MS;_f$*Htwf=YoB58rEl*+F7VVy68;sG~C-j#RpoUU9^x z(OlK7xsH4AksD=>nQ2(?FF8I+ql03z+|&%ilvtx=Sp#wpM|e6KIKq*ePKVNyjw+-w zZz=qPAbuvU#pQi3N@KRBatrQ z-F4n^y*nMGJL%*4^MJh1k()tR(l;INW>7ixFK&MM5$PXIT>D(`)AyFA*r;i%Z9@4u8$RHfxpn54img>IBPYdP1TingHrj#Tf635} zz(O1E$TQN^ilOD_W(_S*nHi$!mtxCD)&}+668EOqQtZ5Z|MLE2TW1l|l-bF%^UKc7 z-ZFd2gZ;`=0Ow|*q_i89S&nsJ$UmsQ*YP(aeKBmc>e-qa;wUiDVPQv(l9sM^$CD=7 zBMjyg9{(GUztkB&!Q;z2;{hI@-5Kv?rZ$cBD5(ExrjJ(3M|eqo>!{(G-7Pd;Uis%E zTqf+qMcu{y`v{j0xef9yKOa@Ed=_$2m{DjH;00do3p0kpXZ$II0>Y&TbqJRrj6k>m zVIso$2rUTb`gF!84Zb3IwlCb+4~QAQC}R(VQxRt4QsQQh`$JZupu8qz8u7`Kq5U!{ zK3NU~<-wsw1m*7?J2Gi%BJTtLc#L}^XkjghfY3vv#g5c0{0Kpr{==@))C!)p zEiRCU?NnF}Fbr}0qbI&A-RhXpizd@t$MRnIsv_Fa+>0)!uQ~?h;K!5hb$pcr`*)$^ z*BtDN0>|9m)R-=zu)YdkHXz&$H~@e?gZO(w!+CI5Z<;~>bUPOY>^|_XM)*hg-*5?i z@f!`GmH$9L2smDj<9{n3>~k=s5514l8pndZbaFDks*a*={;^Y6eA1NUTvZj>M5MEh zOMOF^cdV86q{)uKR@zthd!rqTtTd6Xajdh_O6qc)wbFcaIy#RIO8cD#Oz|C-fk@*$ z)Zmx%;i2IepNFxU>bR6gU4LNK){mO2XDHf=*|W42I{}vgnX}BZx$R9i5Nc4s9KbpN z#f$7+7-mQH<5#!cG`9Qw{Da(%$EaW42VV(%NoT|tblsw?+iltoyXsfDAB?mMn<+d7 z8c}W|)(DR~{}A7Eh{U#Rx|^{J8Jp~mXY=ViZ7UXs*3pqqC)4*F75y>wn1)zndQyqL}z!6rKG4n}_&c6rFX1kL5MQ4ae`N z^s=R3$6Ef+M8DH?`5!a~e;G&*B*c9=mI&PWDDE$v;jV+uG1ME?h1;xsT0*>l=8wjo zHr(gz;3pu=!*BRaM|l5R zj(euiUW13@H~c8z%L7D+L-;tL4e>mLeE0CbOGvu`BVWg__aWR9jrtDmm_lP||E{pW zB>~d7B@Q6L4tWcp0HToA1AazaKrDDkmywr< zkPP?|Dxn_%^wPnwS+obO-qS&ZO@KB)GXVFa&ua^WeA2xLiW1ifq$R;T<&<$`IxPib1o{ z0nxdFUUWS2AYI!1^c^(Ujo<(Abk=qXB{^_TKR|$(LyM{{D{Fe`QT<;00i78P5>g*Y z>Ji6ZO$7gcH;?eV&|m!#qC*bn^T)h;f=dT~5tipejSb~>9i`&swvdJ4OBaT+L;voU ztNstu|K$A_(?6j9Sn&T*xzGUru@?vLeTdGZiTwszXY{l7%d-yZH)uxQziE!3lLpsgU%iH3a5;kT7~o65DF7cU{v#3mV&)D17PwU!LR<*P hBfRa1tAhM+3vUz0rH5#e!#0;{Le2T(QO&c)|9?jKxkUf~ delta 21217 zcmb8X33OD&);3(#b7$-jLT2brlcYmu=n#f5XX20n2#CmhNf@*PatUx%6fr?iK~TYh z+ekz~8~~>@F+#u)QE=v(h+YLj;W{*k$_djsJ^xR2(AWE|_xslRv(`>k?b@}gYM&Z* z?b_APeyM)#lscDaiLqNjbo*L}t`@N2R=5V?HQ@gWFMYcucfh#GX;ilwun+JRfSRc; z^#ak21uO&X1bhtW2BiL{hr`$dJ>k?_VUC%y20BWs?oV}F%ydg$Vr0cFQeI)=mJ9KH z?g3&a;A6lIfawRK%lUarZcMk&c~Ls5HA1i0-%D>=(s9 z)DwL%Vj-Ka9H+h$u^sG9WoGuocX*eH89<0m_ZvA+DB{4qYo|Jn{}mttitc0zRiXA* zf4~(jwz6WCGw$|)LQIftA8pMBlqMSs0Q>Nm5K_a?>29*DTHK@&Z_3{$SzdMWTN*_`@M7Ai(wG~wS)qB!K zI(P#p(Ke%v&EgQM%NVY2Rt=>(L#uAQs#{0G#FqoA?m-A&2p9!brK+O+qM;~8T-ec5 z_SZXz@sbK1Y^CBhrq?8AzT{Un&yiK$!d9Dk?X{To@#RW;xHDQj=qKj5%IfP6EQ>9V z7GGvn8Y}ua&^*7!x>9M?cFnh2+Z0ABRRQ7k??xAzRZqn)*+#vUo@EF1rcAw0Wm0ce@1^1g-b3Bl(O0{uv05a0 z;?YXuehCks>)Gg~qJ;(ZLo>heDvUv?AkfeAs9Ypa@w`_hG&_~gAUd@x z{>J8nB_=-THMr6R3i_Bp|EebYt0mgg7L<8|Jry>X4rM3sKZSY27LU5=r8|PA27E-b zulkq0%8#x+rg*Rdw2X>rK8-*uW3DfGlQyX@uT>Ygcnb#mIqt_ds5qPL2{(=EwLi9O zifkOIW<`Q3O}!5vR-7Mf6kM)C)Tr{RH(zy~lzTU#r|9QVkHSUF3rwoLD%9{gqY?3U z#e?SOzhIy75KF`LXW!tpxqQ5~f4-sGPDQo%mrx7Ng>akDg5KT>A>kMn61>v2# zw6J}AI4r@i9^Pq83!1{f?{D6q^tdktU;PQDjx^CEmKkY^UhGz*eV=#*VI=zeAe$CB zS3$h&P-JeOi=OK1$Cne65gGsSM0O9-g^53Um@hJ$R4d#vVi*okVx8!U! zgd?wX_QY1oR48xji9f(fW304mb%8f%+TgNBrUzumhLD?m6+kL-?A zDEG}03WX=+h)GY#k#Ah3gLb&)JKcA(=U4O6Ei|+mRLX_Fikas_@1t!*2AcH+a^Hg*9P&=T|#q()?++A?!#uiSFG| zAXtUDp2s~;$O*QfwAmZ>>fvrpbYyoTQC<$GmX2ys6LuSfFf9<)O%<9~zc@e`eR_a! zTh;j8U6%+AO5^>b>-Azopt`bpLahD$q?Q{GG-RDJ)ab?g0u2fw95M_S7h!=97dQAX zI;{_cy(vs2*4`aTv%Y1zzVs7rVSI&V$oJ(!<31TM^`l-D7reeSA}r$5~(jj zuM+&$2bfbYRt89hR;a%I_ygPI)5KOrL?sFHk|_*d@97pgM3sBB>my^7xWH#^cz{?N zt_yEQH@E+)*obNN)ET+AZH8b-3){O^G%7zqiijzJ$nGemYQJV*p`a5l1;bzc=S3x1 z3)qRz6SmKYPcg`*X$oWKE~(7n;y1wou2b@PV~NvhKb+*T7It{d>VgB8OQYXC6_h^4 z!tqYmQ5T=i4c#BSa;(#?i^e`3`YTG^ZOm9RApAj006qy;0`39)1MtoyVq9NEjC}6D zi#RbTrboYJYkj}@!8BqLUNi5#T~^H^;t4-7-Iu1`tJ*@u4!>C#Aj~vHcE3TCTlJ9e zOpEHZYLO}|9W#wseV&kCC+59Gd?FYQftKD3E{8aaG`GH8JzJY#vKhn#d>%u)p;ce5 zAFmgcvc8fS^%VwD1H4`whWV?i)P#0tgXn8#UnUHi=3Wg|CXwhgVM|u@YgO74>{N96 z3~6=vdL3RWD9VG)@77=LxrK)NriJW{Oj*xLp<=WU>SYweNZDY?I0 za?!n!Vt+O_v0v6~&)&A`>&KS2%Q^!UCu3GyYW|1%#GllY*ei+s%D-_hz#7ozGuf-l zGxDi;z+1DZ{K$lIDlWq=I>D#iT!r{z#K-w`n=24shE!DBzlLn`|0 z&6}tb{oX!%b)v#oEd~o{TExDTz4_IV;?F^aIc8s8d6MwGe7rGI{4S_8qRJqsY*C_F zeow0wV!pBokw)=?+hpG&=h)uA)~MWGqV2S`n9FryirXZlAxvQJCz%>P_BP)jOKIoc z5_Ol!9=`ok`}T3f;_3{?ZZtUX19o7YctbV`MZ(vvGRXc8zyd%$VDmI$JozlqU4V-D z5YjOPL{~AG=!&YwlamPdBL4}5dVJe+pwYDkrTicvIhUmBMuA|=S#&;fqp>DRJirzw z52cT@J;|nY$vbH*ZHno-O&BI5V7>UFfE`G)WQNY zO^fW-Qa;(=c2i44*G^e$Q_FOOHCbuBa&DMl=|{v@FqbZM83kpt*uJNb>T)gxi&cUV z`si?@&x5_q~Mp)$}=5?}-J)dH2 z814;78b8TB)2Rb5>KmC>EQ}D|y+j1Ex#*^Bh!RT!MOQ1+prukJX3xVx<07N;{l(^a zQDUWAcJ<(nK*xO+B7DL~bOcJcig?s7BF#nSfXHH7RcOa>Po!8P4?;aByFNx6Dta_gti22 z0?8oaw{C>uMfYY%kjK1kyORzH!!-Jb$0kIHZm$|zf0TH`Yf0Kwa`9AuQK{kzYPGl^ z{CnBA>#ZA&{U!?79j-U_ez{~c+t@d8#A;dHH}71WsH9PybjP-m@Gp*as}zcTC07kA zii9j|*mq;E|EMD;!6xLEoq)or*ca~A+#uou>_T5t!-OC?KS9_h9Ch^(`>7&3tFM#g zL6ZW~zuj}Z+X!l#@E%I>G!dV5V}TUsx-ruH3Sng%HHV>xkIDJi#Vo-)({vR@=+~8C zI&N>8?vbD4xD7{pQ!UkFCJ)tEiXXRsXm@m0l_ z-3p;cv8fGOKxOk>z&N%~g(&s{ngQie(NH-_uBm2c|?54uV}8v7`Dx1|2EF153vH%+N_x#W24P(Uca^^;v14~ zTu%$5#PJ^UdUY8c(R!mX&)j(ZRDFAtSi-(HB`Z1)GoLA8)RM!5sH>9f>Z;`RO-XA@ z6sHYkQDVwrVyqu`9D8KgWMWiQCEF8#jXz8}IPQ&KA}zOyPUiY{7ZObcN`X;qh^a!MUn3#M=?;OfzD=qC4$cNS--W4ITmYrJZ4L2{&M zX9ecdafv}<`MOJY-nvM6`C;YSlFvJXQZzI5&(~)JxriggiELJX6CKA^_cy2B6|mcP zm8d%TMp{@DAjE%B7}5p9b;sJMu*IM{M2QyzFgarbg1= zJRwm?f+lr3$kNg$<%VN~^SKy13ocGkrzx>6hKUccjhQ+0F4mTrlQ=0@Dtzfucc&?NZ8jlP+u$&Bl}{`h(u8>DoF1Rg^8LpI3Z_Ytu$h-{1yvSVLKrP%&elik`&sh5oPkbg+3efgR9ed1xvAN=AHf_1-MYH<&BQz}<>VzrfugIdZeEJ9Q@4MI~*ALB&c_RIlnfYU(I6rxKeNgleg-4L*RU^{)5V}d(_|J=Pszou7wp=m9VI_@ed2=gtF;LIQ=_Lo zH=ATL8eu`RNZ_K_Z%VDHQ-I5RG6;mDppiz z1%#NjtyG}PK5+|8awu}IIq2Qhllg8J4+vA0D) z{zKd`7KLEy$`a__Xz@d~EI)oqq_@#&zzHK-4D$soVc$Rri(Vw;Lji~ugPzw)uE074 z3!_nkHT_-e1~6+0XalrSo%jswU$lIiPpRfJWQxPse+E`& z@@Vhj&_F}qR3&IcOMq6T)S!hOHmrAiJZNxoo>Gt*85W3W1EzgfO0;-}Jyvj!nwRd+ z=%85I&f*5O#D6)FtAcXrIl2Gl?fAe^LOe=OXWtK66VGW!qNe)?5*?@O7vdbj?i*Z6 ze_=-kkD;G1@8H4VKlpW3BrU04BmT@r47p!D9c6~IwjpMm+|Lb(52YVu?jiTx%DHFg zC{vn$&#H$7Yuj+4=;}2SKa;wBFRz*;(9t&!yu3mqCb3gP#}sZ3UU5;YqRS+N8zonA zn^Dk;8Nr`iyi`nnIZ7S#edh|*H-hS3M!lN>9}Z`gh2wKdf*PT-tzf)HeAP$InJdlp z!2=rcbzgN`zK~b05wkr;r$*fA%Rs|lVDA?;V0SGYW=i7h3}811!c-3-x`$yAZUmei z&K`mxXaqYkY)T9#$_CMFz)CF0-0+EEJh~Tz zUyWclhfi1Z@v%uGHicaThUWxFu%AcFq&r#h$PpnarLZ+4r|64{`54iNCU$OQe!pJr zwvkkK8-RafOItMTr(2grL2Vu9(TMphXH=evQ!D_~|AngP>X;Z#)els9wt7^a{!c2_ zI_j?2KT>jQ6pjDA!^Hem{Djo|X|E=f9q988a)6#|{C`0t!(QJTY@<(G(5Okw>zb zMM-onyQe6Nj%LpnrBC2!<0ww75+Xr7C#Z6!*A`-N>`6}z-tIK7rdD;A3c~Q$D$&ik z@^N|Hq{5J}M*gMj7#35U9K$&-_zO0RjVex_$g!LLf{hKPg!t+@UP5hr+ViOqhZbmp zv;VBI=w>-pp+*C0+*GshzWPv|Z;fIv7pH}K^yMh_uj1jMs5q)6szgK0yrzWF7+rw7 z0gnJ~2jDz5j@O|Uv)Rznu27k0N3*E1p5mRO*~GHk9!{g#!)2SaMScP)D_PvwOw$!v zB_tft!`k~Eb_K2ILi(FF9nyzG>F?Q`v6<1I0;_9z7VDEy@JkttjonROY8foVQk6!$ z9%SRkWrs#a|1o?#uNfEHn}7W>eEm7V@G++ruX|hej+;p7_!g=BD@8u<%_I=41i@~= zCxAIcRCg1h1>qzxn81e*-~iIP7Jt=2+RFyC+@F&5K#fG!g(CLBgzwbTfnLq_O>EHc z*o(z1w|ZhI`ff3MxcYDy#~Mng?qV?$CjFx4NgI+CEyE^z6-hif56R_#HGtV*yanNh zB`p(gKcI-?`Lq;M1NeUccz(Z@xM`&{mS=I&oJ(T0T6nBkB|gm-PoGTJw6ssJrlEF~ zv6z|tOtVq`Zoo=_cn2|V29NcCm4KtbEohlM^La&_15wWNBEsl03}0jqZ&A;&Dtm>h z0;(J+C&CZQT1sl3R)o5Fri`7L*HfU4dGTJ`>F+Gpf02Knml-h-z?a+rG0}pS^ zM({Y~YnO-ER6BN*SUQq~!zFQ0?MDf-k{gnn4s;xN9eGtwRnw~_M66l!07@xM524gf zpQ>qRNuzkj8j?rEsx^mX-R3sfP>Cq8ZFXz&L;kfe$P#ha8g0`+p~yZX(JS5NpN>J> z!M?Sp)9)Cq%TJyJPGfvd;VTzS<6EpxdN|lq?r&^D-H3*ykXd(RXX%E7-wgv;iTaB? zw4ly31m$!-Z4<}V+fRe{4?Y`sPmdxmGosd)g+Xp67kDI@7SSq%O-sso%Ub z4W*I_QCEI&rBl0un8G&?FDt33zHVI3u^GYeCXHY_k|6jRq@#iP8%xDT<@~jW(K$uu zB*7=W72w8VMJMk}hW9P`PS~NxTz~To^HOeJef5nW{?5Z!-cX~d1M(wLX^Z#ypA$iC zmS1!6kE9w$b-x2f1L^^f0bT)|0Hl^v-7J6$5ck6p0=XE45oH8g+u6`n)Z1+5A}bD2 zA1|5{{^QSW!YJX8a9H@B#75v2LJ~`JX;3b{Q!;O zcwGh64FHS>%wVlcf{7e!N2(og77$m7*#wxzPX2vbXdpJStfi|$JO+Wp^8shn473*`4&{I0;yoD#PkDw6m^u5*HSgk&##W%`|(Ug0oWqt+h#RIu9 zpo|-ig3@yxoZkX~u2msUU^nh9plX(NAOAn+K5N{w$l+Yk&0>Y<^F~foh?#8XeP-Im zj@_54|Hf;!k8mnP1#{h34aH>W%2dr;DD&NTHgn}0>0KU+;FU(gQla4Hk;l08(+%4L zC;1iN0a#md?pM-8j=FmS)vX0^p+-Qcjp)UmmWl`Tw7=-bVSc`MW=Bh6gu3em}O9=e|@*eegsP?y{D?2j>ZgEfwq;vKjR ziqUJ}&Px}vu?@vk#U5*zppZQ5-wlUQ-BXR}SP5GjGwBR=uF)G+4)#j{s{mh4Z25W3 z%T&J?@s_y|G~2c|-5A-e3xCTSxEAG&tvGvNgmAc9AFb`CT79=o-&7Vnz|O24u2H-> zqHL(ZlGf$ox~yVdvickFP_epoQ&3y`x}|YEZLEg!2iS^k%|bYu6+hz8zkY~x@Z90- z!XufXa*ZtV(G8g;*mkG4RfG&&lY(o;E<44RB&a0_F93e8W`EzXJjB<^e%kOG?axjZ(?atg>EFxt#%xNVye{1o zM~|^bHVy6dlBX!=R2M(xU3WbQ?z<!ep)9LD?DRz?)YZKhGq7*-8!7dS~e)R5T-yw z%RIE{b7%E0Q~RQph-q>z3G+nwQhp)PQJv=+E-6gM$*BCAxmA=VpMPt6|cK-AIB%*Q_S{cM&euU5%%PjN>NlAtm@7Z zEDI8eQ3?Y)ak$OVbxTM_-%?`oFuC z+m#!VlZkc6ITI*%`B&BqNycc&J#m)g6m(_@g?5GL_3O_o_wm@1xY}uU4t8>WVyC6Y zZ2FW^d@*3dMPD+k(6Ie0#GP!|Q^^&B-O8p_H9O!ju&Tx*_3?e^A~|QeNcuNeLik61 z7tjfapN!#*nRpfPF&15``WsD^f+{Z)L(_C~n;zE?JD)1Znd-*?cPhmR0b;q*sTbe( zgenuq1}rr$cnGLgw98@Sb=hF^NY%{$RKJE6n4CYyX_^ncXq+pErL=Qh9m$S9L&YED z*Xoo4$=05UZlfb#t4$Cf+i_I<{ty+<$=ves&!AeE-a^HH$r?eGH~gFLFG3H_%q|#0 ztQ%-S@&RhKuKBD3=V&oy2;3r%h;eV+ohT8t>G%Hpmg1Vf0W@u~o(eUlbW!uGYr0(Vl|?sG zP4W0vi)3p4*tNLM+eY%#=U%lZJEzDQwv>{X&{R-&Uj7rK3rzxRc&D|l>K2i@Q{A0F zPRPA?QBja{>_=TT;ZfJKSX%g(u&~-K$&q<7G3s!>FU0qFkZstKoSH0WlnaH|Wda$mV#l_a8oracv6}Okgi><=a_A5P?+%%m7uqxJ^J~mCR5Zw`PKz+w z>5)G5jByr1+H#RX%y~yCmilkHWE_6PR$|`J9F4v2by2d0h&Uxca!hCW)jP?1sgvaH=_KaJZVkKx zIMWX>IX-fxlv5MAtTF_pyJ5^%iU)nf@&uZg3NW2lS3tw^tPS1TXa`lj00+*bPTlEdOs;>Df z6;|4{M;q0-RE#D{@$abhchveLwuc_fLZ7kbC)a#?&3ek*Hfec~?!#pX1ik|9OT==f zQz_n#LSLfLCvuN8$NMZbpSkLBch<$|R#QU?ItWK6aUke^0o`6O=Uo~A*8g#p*-7^0 zPOi)h&)1Y^3vTH+Edg#Sb^Rm}cbGBy{sNRV2wMFPFoV(O}Z&0%H z>lGXTo@-S8F`ld`T?IlVu0Vs*o2+|lBK?4cZ<`#Sh*3u@)m;QbCkI_vSDb^vFo><# zHkRJc+P0Mri}v>OJm&2{{bgR8Qzst5>Vc(D9EIBX7+!s=d(G&ct@W|y6nIG}OhM@& z8~jgGYzo?ZrBeqlm_NP;jzj;J)kT(V&5uJ!+l9W*sW$6R>u|+C%F(akU$C$$&*(%$P;dUlRU~Gw8SVV{ zC1Q>(Rd&6FZ%f3ZjghYrQ%agqkfl)pVv5~+QZ8#ec`14y?kjx>=)%=Ng!wZU5pQ}Q ztBkcrGNp5N&G&M`fjpsi4Z(+-El`uH>G_ggUE!}*3X|bdqhR`1Dbv168dxd5$tI7g z?^rbAUjaRuUU;6LcyygB^;+l#Gi4*8&<&K(4fsa;2Y$w%`0&s|nRbgFoAoXF#7-i1 z`?yW*+XH)6tZGxUU|LO;^0Kz~3oH1yBjPW<2A#MS23ti9$w)d+#Q*rHi4HXk+UvU6 zMRI3$I_=ygW}uzk@GRTnz1FStDq?8Xp$-Zu;DXVodR`;6my~xA@qTY)Q@Ar~$IO~> z6C(FVHD%eil&tEqI>%yI6)YQzVYSH@*<^)0?)LJs8+;@?cymz0n{-H2XOCD^+y4|x zNGb|QC3v&oO)d>(=ebK-`Z!!+xD2p3zRD2a0N{$snCrdX3=d(}0~78t32(Z3WEj$; zC^gT7#sLK8`~%;>RVkj)BgB3^3Kj}*)r;f%K!}rkO>&e`DGYB@<5qqljj8N~ z{-pcVO&s6LNr!Jl$h{3peRObmw;}u0H%$s)rzX7N@D0SnWvB#op+^mI#uaA(5@$n^$t%ETs-euYyeN}75(CI8|M?V~P zDs~jdw{@$dA3VLyO<)qSVEt*tI<=~EI@`S?c1g4+;e3xiWwd_VWdo~8-nMRK^qkYv z;pITAGdgG4iS>@oYp(|JdVl8QT1klTCegsD6vzBtRV&=d*3kS~CZs zu4w?dHKvN|>v2Ldd~vxFKw;i>OAQ`Qo6B6!_G=j6=bH>R?~d1CrQ#)50=P3wte5~_ zHASVOGANydckqRsL^rj8=nPf$t;u!`tgP0yC|Eq4Ff6;Ja*q}~B`mTvPIz_;%KCRN z(S_=ABF=GJoF;+iUgsVdC1eB%RK)xHf48CPJ1YR z7$57~ztjz72__mW8rV^d&iuX!ShcBmjlI5mLYhw80<6wS#Ut`to^@h&O_9E+gxl)j zri-J(%|k5Zg@pXEIFx?`Asbr5#>3S|Y{CgbH(p<<#~7Lmu~G49VAPfBkV_smZMN`* z^!P%b8E$$;;X<8S+#4hz2YEw#7Tf(o=8$KC^qHPqwYcF{9=}n{dV%Ov6}nbgdO#)? zWqCqJkIv3LKVF-`0xu-BWbZMm`oB8^?+Qpz8`}!kuiNRCn!9D`eNR72*lA-LeTJt7 zdRhG|M%uu(yfQWVPGsdG3m1~eTG4Xjl~}4FGpX*!87)zGbb%Y_{QXN6@6KQ^>|duJ z?^c@Pj4H95<-NMbuvF$Nxe*oyyC6xqzLs}i%~k3n_Jw!T@Lmsuw@7c86!c_EqVpL2 z+aVvt5yQ}G!nomMX%+^z%BB(Z0 z5+BDK0{*6e^MmFt`eatv(l_Qk|0L&&$=9LgsLxwqcRS4PYUwxOtv`uXRh+y?R*g`J zm%MzLBpsX3)4$@YaXZfOcHm)&J4#xcny*~bud%1ug`3hNgnTV@oNqQ_GOVi-gIL6z4 zQCH-X9EWiGgR9n_>sB>p55IZ~NMHE_(zoo1x8oaL@n3blh~|6%;2kJ$(^vj%(w9rp z4m6g_QZ24-dq2f2D&dBBqmx#;Op5X=p)$Xsj2p04%S)=Eg;!sHdzltx9|Zp$fL*tE zK$pjA-Fp_*4FxD|g{?+7>jtIQ20$|nNRL2cE_3eD;xq{so0h4-VwT^6oJKGxZ07~1 zvk3?LDVk@ozaJc(umno@-j>pk)gcKh@RFD$6iM*2;|GnAuei^^Lt4F8&3w|QtovYk ztPZ>+=~Pe?GB^GRZV@c&P$|8_mK;is+1a7)Hc+F`&+~|^-XckL?3qK!(=OoxmRQbr zmTZ7$6YlbrUvQB>82s8b8J@O9_M1|@KP&VZDuWhhdga@ubTna|?=rL>8&p+?n^=Cr zXc>f3wIMH{JMEHWW8rO+vljZEbQyuy28rdV=B+E7jd272%e&bl zZ6@krue9aUXV~es{JXAu&2{j>QJdR(RlaD7MXAf)`E_P&O)q1!lB6HKKiE~>5gKz_ zSmi!bBrxB4ZII(xFPG*e_^Ofq8r-Zs35Q-?$TzF~1kvq6eJ21v1H6C~w(M{}I+<-b zJdPH#(}(la+!QyAMIJHJT9$vr3NQLOM;wVk&o?fd^Hg8qzgqQ$Yr`i!yb4+5JLV`T zC+4kg-6Zq!fTZ-Y(eKb;>NXe69(GnwNF4mkGg+YWNllIQFMRp z<48Z`=C6d-A^kw@V@ThN{8N|I1F_$nY5;as?Zdz>2DWK~wT<5kt*&hVW**Dy*hjx- zr#sAeQ{?YRq`$NHck}5Jtm0ht95~TI)$}1u{2SF304f0M0nLA7xySlc zz%T1h1KfNteqn| zu@SY;2d%hZj_4xr0g|Nu1T&A6-~eR7_>@O zrXb1tp_9P%Dh510yG8Rd>OE^Nj9h+uE)1DSogtPdJuW9F z6d_v3YRg1p^I|eVBL|io>5mfIm-R=gJ23JHAKN&5wigQd8ze~=;OoFi_YK!`&Y>If zakoX`$lM6E*Be;8XhhlKd44It#vRY3@3E!FZw#4s4YzHj;7I%2JUk)0f{_vkvJy>$#$R z;dpWM<9@yZa0j{G#4oL7`6sgIcsA>VIdyn|m@UE&^SD*aFBKde?9^AT#;QOvZc5p# z=|o~`zoT;dAM#j-mT)ouik6%Os9D#EScNvgem;@WixU}Z;GzPe?SQSdtnUXYctSVg zgR%4#w&sK6*jKTYVC_L08vO}43Zdof%@5KWc+qSWHH7Y^tWE=@TfWSI-aYI$*IAtr z=xru$3vWsfpeI(R4(S2;VMs6c_wZYeUDhOMj6c-y4Jjz~Kg4<74xSGK-T_Fh$~u)W z-$}4G(tW({W*@c1L5cGlrTKWSXRe)JH{S^nX(?WJ_@&**PVC~b+u4c_%_>QXli7|B zGjLII?87{2X5Al}`iMT$V&lr7)E9e!z0CyIVLvWkndPItV;}Y8!;avWu5%CRmU<}E zq(9v(Nw@p3j+rEB`mJ!0kEGGefUW~`ZwnKWK7JR*ru#)iTLzC)h3}Ye`7;GHC0UT`lUS2AH5ia zl+k0h&%z2F*2T4npKJA@w_-P?LQF#yF)lN-^9x=NHcH+{-oNRgKJNasT2GJkVDDz` zPbbiSvvHqg)gJTsr9JLHYS@d^RyVJKSoy|VboCP`8+=gRlwLdh2ghn~#6VJhd%$4b z*-=!YsKNtBqK2z6{QQx}*PR+Cu^XK?rTbaeXL*r70(%~yYxPS{wM3r0i_%jq_x!6E z^JT2En<_P@*wN1uRGY_kf6;R0^S(54i#u%-^m9CZy&=tD>aK+_DJ<@KNuhSLUSIr5 zpJc%=j;L-(d2-9qQ)kq)xaFDCkxII^<(0Ef(1zOYi0&0Y+qwUS{|(1!hJH_U>F584 z_P$$XO@G1h{N3Hv3jKihzoEM($!Q|#_&>|tsxM0SUj55!8n|;Bk5Nlf=y6}sF z>R8>6UxgRJfkJMlwmmK#tN1CCj$+GxdR3vVVrN__8hrCI#dMbP^U4vC{vYaKNW^^y zUx$CF#gWp}b5vNY!K0nfp@Cp^rK0Tnp-y26FE14pA-X8``Ok5s2jQ1aHYdJ0FRDzb znO*a2&Doli+S#?w)}F0h(0_`3M0Kf1R(vm?ZJbgw#csY{qb$8nlv8SkuqCd+4X0$1 zPtq4t1^gAS%^Q2YasG!g8Tg^B8W@yD2ep>4PW6q(!L2t)zhArPNdDL+C`AM_&MQOy zjX6OhEd1lI+G2@{L~Ms=YfL$=-b-+}2VEF+0#2V4G+Zaea#vV6-4c{yan>xlPUdOg zWaiEc@aM=s;!z7XM39aYT#Z}!t`lSV}mGhD2UYxHp21kH{8M->wc( z?PqWPGMA>Y=*u~Za333eIliHs6vehi;b=|7a4+9#h#4%d zX;s@xqVWF|9wwGtZ%lh1;VIc*Qt<@K{`L0$>dph*jdN9pXkvtYv#@Ey5oAtmHj4a(l$-H{L-*_D9f z%{5$gW?eBWMtRu8D=A629{S91;k-Q`WR?)wB^M`3^R%qHa;Kud_>NLgR+h>tYiSKN zE0@=gv!LMy9IsUZKT6~7$j_p#+UGC#ekpG%QwX%lbU)T)@r;{GGQy@1Z@};hOI!hu zUqQSK;fF_ha6Entah~^n4_!Wwx8I^;J#+&(-4pEe)f~9fXs^Kl#AaPf?De@v6*}ll zLX(_ViSBead-U4YaF}Jf%Sie?3S51sUtb+=@J@Gf%|RhfW3OMY(vR`rAh8d3iHzR3 zD=QDy77Q=wQ@lTga6aRvn&IzIa}f*_d_y3zZNyb+0^4$DZ*Xc`C)ZDc&jEYn=3Vfa`|W0Y7;Z!{`=}TUwnRxq6nba1yefW>k6Kin3cP9V z{id~Qhs#>E-9^P@c0ul^%?iGbcoNgP`)LD#9f)6HIqoU?Mu=!70JpSmreBN87S4AC zOZN=b&%c$L!WMd#_O;C<#-0^VRrResX|bU+VSbNn>c`==&sERk3Iq>%ALfC=SHc6q z_Ye<0@O`9u@CYJL`-f;apKU(&FaMyF65#L%29JEcwQsgJ;UV)@7@;V3UC{OoWb`#* zd;cGjr$Mr#gq{vc7dk>D^8!OS`yGLaJoqJ0R;CG1F*~5H&|pcn;N_*c{*lnfBOMp< zq-H%j%Gv2!m%pv5`sOYoE`TLP2q>zQa0d^#c;3a?YZ`#@I%3->Iu6XYJxY0p zwtYhXVUqDW^t?}%{8EYXn|pB6n=4#%h2^SFQ)AC(p6!@RX^#3B$l4rm1dBDD@LhF< z<@Hd=_{{lG!FgSBE4N=$@p;DyO2>pGQ#8{Nrl5C3*1p5+YwY1=tcHqJjwK43oXAUX z2ZZnCcN%rw25t7lZ)7;!i207)3YtR)I6eneY=m!7-LA_j{lh}#*vK#VFJC<2@GEGw zr3l42i$eUQ##df-jwWCoG(7|o+sJy94Q51 zed`W9B+$X#n&KAjJ(=#kD#glnW3Y1k9iK3GGZClWgJ3xH0ECGs=$}o8p7pu(H#{+ zKa8nONY@;>^~mojlJ#*HMN_!rPlKaNNAm~W34(K%6lP*+?W`0+v*|n!yQRncCtV7G zhk8P|(w;-m$z@&arN97FWf*szE@whgQn4U^+gk;z?s{lOA!Mzyg>B_Z|h?R@vG z7K&G#H;hiCwT{(cG(B~;uV?!7tmspH>q3)>a}P=r4|asn zTAKSMxG1ZVS0+1^Rh2jQrzO?YkK;IOjnkEw+DUNX{X*{S&+(lL91|mGh63Te5p)-Q z(h(m?7e>8S2e-UC34XGMgX8f?T5o>ukfBwLeL5f=^Qx+j%bRUV{NHLbw12~y6IGGQ zks3u;&^?YVQ8XvAMlPuPR_pt+7j%x9j!&cLD8)Am9AVLPB@H;%M$^fP^9v6CI~wn2 zfIs+fFN_9G5Y?OZ4NrCR_p@TPPs(--?M<^{^4&w#>aVszi+S3|$^%loV_9!Vhq@2G z*PD7m1&{VYJRMAzJFdjjbXx6*OQ3FAQE5Vd z`vRD9Esod}dLRAOu`z|_D7G$i9LD=I>TsM(!JuZ2QK@ujbe5Z%xu(`Ar`rP30LR)? zG_cCCFO`mqm#`zxHu+b3l62v9BLpL-#tYMk*yg?dJO!5#MR!>I-oPgVU-glpmex z4#z0`Fh)*w)tQ=^_NP_8uO?O|)zGvZH4`e&)Zj5r?T*^=svUKkk&|SHlC(UjrqrXcj`ii#!NFbZ9VaUJiezV{x=?fq9^`1kKfl5f7?uJRAuj=WKw_n_lEEgmhU)9 zxJIXigyxmVJ6yx#w`y&EE=I2;L7&l>bgeqLTyOJfOj`J6Z$YR-_yj^d!VL(+5I%x1 z9^o2<7KE#PdeZ|4@Aqj<0}RP8KD z?z+dm`0uB+>Mvj2utTl*a_RVHBaULP0J72AfZNno~akZn?Myu&Ihto#k@$LA@Mu$fHwUDDQ zpKb=XkMpTZ|7V7yZ6M@R>o_-%#^*dU7#2;y4L~A(@{3mBN5gQNIcFf>^zCUA`c5sy zbD{2kP`^~`Fcr|aL4$CWS>H}^uigs6k@&(q?)*!9AL65K;a>{H9g7O+B6SVEY^vjQ z0i8kLb`%Ukhie?QgXk)H&~a`MdNRwAgI~Q$!C}z3X9K^;;^+50Yj$l=m%g;-q${<6 z>Kr=k^9^_Z*D&LQtguxxf#4LQ6Rb5pspfyl*WSvH2~kY`FN&Ul`rkat{)?ig4($Y9 zL(C|Fqz$M^!mQ8uzZ3mQWB5O44*qQ@JrEbOdjiqH35Q&Iq$gZ`&@_R1BYN?hT%Vc{ zFQEO{q_3K%IC5vwlF`NciS8tBf1hv?-KahI4G=)>D@1qfMWRz9t_NH|{>Ok+;D1AS z50Q6@wS68 zvuHFO^dj!O_5xl5Yy~s|)&bT7o(1dzJP6nh*aX-NaO@gMQx4Y8rt8#U+=kAL(wl1? zfm(WJuXE>#t`)Elum-U6{K0$Y(G1!!xSQxqfH{B)z*GQ!^GcTkNCeaYcw+}&1sz}x zpaL)zPz=ZcBm(LHoR0E;_?r&{!8&N0Jt;z(p<463w0QY~dnVQ0z4RX>y`;MMHb?RT znmA|jbx0Rb1ZzPfl;X_*#jTR~aG^i-jh+e&NbN#CNB&V6S9vH^kSrDMXrs_rk9I`v-=w*tzgGbLx&i^LFiD zxL=t}$TPs-S0Caz)rfcx$NdfadE$>hRQP8>PVfhwhf}9+`%7a6ya1C~&tG#hZ|MJ{ z-2W@j@PB3=eD6-Wi1ry&ICr3Jpe;Xt;GntrLu^Cx3vELNL!ZNem8JyRgeP0NeCZttf`~Ps&!bq9h2{( U364&% Date: Wed, 10 Jul 2024 16:41:05 +1000 Subject: [PATCH 22/77] hwdef: fixes for BotBloxSwitch --- .../hwdef/BotBloxSwitch/defaults.parm | 15 ++++++ .../hwdef/BotBloxSwitch/hwdef-bl.dat | 16 ++++--- .../hwdef/BotBloxSwitch/hwdef.dat | 48 ++++++++++++------- 3 files changed, 56 insertions(+), 23 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm new file mode 100644 index 00000000000000..f508c948650b0c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/defaults.parm @@ -0,0 +1,15 @@ +NET_ENABLE 1 +NET_OPTIONS 1 + +# enable hw flow control +UART1_RTSCTS 1 + +# swap TX and RX +UART1_OPTIONS 8 + +SCR_ENABLE 1 +SCR_VM_I_COUNT 1000000 +SCR_HEAP_SIZE 150000 + +WEB_ENABLE 1 +WEB_PORT 80 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat index 5984ec4712d62c..03960b22470703 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat @@ -7,7 +7,7 @@ MCU STM32H7xx STM32H723xx # crystal frequency -OSCILLATOR_HZ 0 +OSCILLATOR_HZ 16000000 # setup build for a peripheral bootloader env AP_PERIPH 1 @@ -39,13 +39,18 @@ PA14 JTCK-SWCLK SWD PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 +#PD4 USART2_DE USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + PD8 USART3_TX USART3 PD9 USART3_RX USART3 PD12 USART3_RTS USART3 PD11 USART3_CTS USART3 # LEDs -PE5 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # blue +PC0 LED_STT1 OUTPUT OPENDRAIN HIGH +PA4 LED_STT2 OUTPUT OPENDRAIN HIGH define HAL_LED_ON 0 define HAL_USE_EMPTY_STORAGE 1 @@ -55,15 +60,14 @@ PC1 ETH_MDC ETH1 PA2 ETH_MDIO ETH1 PC4 ETH_RMII_RXD0 ETH1 PC5 ETH_RMII_RXD1 ETH1 -#PB12 ETH_RMII_TXD0 ETH1 -PG13 ETH_RMII_TXD0 ETH1 +PB12 ETH_RMII_TXD0 ETH1 PB13 ETH_RMII_TXD1 ETH1 -#PB11 ETH_RMII_TX_EN ETH1 -PG11 ETH_RMII_TX_EN ETH1 +PB11 ETH_RMII_TX_EN ETH1 PA7 ETH_RMII_CRS_DV ETH1 PA1 ETH_RMII_REF_CLK ETH1 define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_ADDRESS 0x0005 define BOARD_PHY_RMII include ../include/network_bootloader.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat index 24f6c6e702b2b1..f277e0068d3bb9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat @@ -8,7 +8,7 @@ DEFAULTGPIO OUTPUT LOW PULLDOWN MCU STM32H7xx STM32H723xx # crystal frequency -OSCILLATOR_HZ 0 +OSCILLATOR_HZ 16000000 # setup build for a peripheral bootloader env AP_PERIPH 1 @@ -42,46 +42,60 @@ PA9 VBUS INPUT OPENDRAIN PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 +#PD4 USART2_DE USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + PD8 USART3_TX USART3 PD9 USART3_RX USART3 PD12 USART3_RTS USART3 PD11 USART3_CTS USART3 # LEDs -PE3 LED_RED OUTPUT OPENDRAIN HIGH -PE4 LED_GREEN OUTPUT OPENDRAIN HIGH -PE5 LED_BLUE OUTPUT OPENDRAIN HIGH +PC0 LED_STT1 OUTPUT OPENDRAIN HIGH +PA4 LED_STT2 OUTPUT OPENDRAIN HIGH define HAL_LED_ON 0 -# use blue LED -define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_BLUE +# use first LED +define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_STT1 + +# Ethernet switch chip reset pin +PD13 GPIO_ETH_RST OUTPUT HIGH PULLUP +# CAN1 standby GPIO +PA0 GPIO_CAN_S OUTPUT LOW PULLUP + +# GPIO/PWMs +#PC6 TIM3_CH1 TIM3 PWM(1) GPIO(100) +#PB14 TIM12_CH1 TIM12 PWM(2) GPIO(101) +#PB15 TIM12_CH2 TIM12 PWM(3) GPIO(102) PC1 ETH_MDC ETH1 PA2 ETH_MDIO ETH1 PC4 ETH_RMII_RXD0 ETH1 PC5 ETH_RMII_RXD1 ETH1 -#PB12 ETH_RMII_TXD0 ETH1 -PG13 ETH_RMII_TXD0 ETH1 +PB12 ETH_RMII_TXD0 ETH1 PB13 ETH_RMII_TXD1 ETH1 -#PB11 ETH_RMII_TX_EN ETH1 -PG11 ETH_RMII_TX_EN ETH1 +PB11 ETH_RMII_TX_EN ETH1 PA7 ETH_RMII_CRS_DV ETH1 PA1 ETH_RMII_REF_CLK ETH1 define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_ADDRESS 0x0005 define BOARD_PHY_RMII -include ../include/network_PPPGW.inc +define HAL_PERIPH_ENABLE_NETWORKING +define AP_NETWORKING_MAX_INSTANCES 4 # allow load from USB SERIAL_ORDER OTG1 USART3 -# no ADC pins +define HAL_RCIN_THREAD_ENABLED 1 +define HAL_SCHEDULER_LOOP_DELAY_ENABLED 1 + define HAL_USE_ADC FALSE -define STM32_ADC_USE_ADC1 FALSE -define STM32_ADC_USE_ADC2 FALSE -define STM32_ADC_USE_ADC3 FALSE +define AP_SERIALMANAGER_REGISTER_ENABLED 1 +define AP_SCRIPTING_ENABLED 1 +define AP_FILESYSTEM_ROMFS_ENABLED 1 -define HAL_RCIN_THREAD_ENABLED 1 -define HAL_SCHEDULER_LOOP_DELAY_ENABLED 1 +include ../include/network_PPPGW.inc From a0ac6f22df5cffee82d343135ad24905834d8cfd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Apr 2023 22:59:30 +1000 Subject: [PATCH 23/77] AP_CANManager: use a switch statement to tidy driver allocation --- libraries/AP_CANManager/AP_CANManager.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 56f6b59ece6fb7..5f6d2cfefd5bef 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -199,8 +199,9 @@ void AP_CANManager::init() } // Allocate the set type of Driver + switch (drv_type[drv_num]) { #if HAL_ENABLE_DRONECAN_DRIVERS - if (drv_type[drv_num] == AP_CAN::Protocol::DroneCAN) { + case AP_CAN::Protocol::DroneCAN: _drivers[drv_num] = _drv_param[drv_num]._uavcan = NEW_NOTHROW AP_DroneCAN(drv_num); if (_drivers[drv_num] == nullptr) { @@ -209,10 +210,10 @@ void AP_CANManager::init() } AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[drv_num], AP_DroneCAN::var_info); - } else + break; #endif #if HAL_PICCOLO_CAN_ENABLE - if (drv_type[drv_num] == AP_CAN::Protocol::PiccoloCAN) { + case AP_CAN::Protocol::PiccoloCAN: _drivers[drv_num] = _drv_param[drv_num]._piccolocan = NEW_NOTHROW AP_PiccoloCAN; if (_drivers[drv_num] == nullptr) { @@ -221,9 +222,9 @@ void AP_CANManager::init() } AP_Param::load_object_from_eeprom((AP_PiccoloCAN*)_drivers[drv_num], AP_PiccoloCAN::var_info); - } else + break; #endif - { + default: continue; } From 9f2da4a763773795e2fc5f075c5336bb57d32403 Mon Sep 17 00:00:00 2001 From: muramura Date: Thu, 2 May 2024 05:12:50 +0900 Subject: [PATCH 24/77] AP_Mission: Change division to multiplication --- libraries/AP_Mission/AP_Mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 3e138e7cacb403..2e5fb77d996332 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1821,7 +1821,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED case MAV_CMD_NAV_PAYLOAD_PLACE: - packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (cm->m) + packet.param1 = cmd.p1*0.01f; // copy max-descend parameter (cm->m) break; #endif From f1e67ba7470314d2eb00f278f062d664124a5274 Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 16 Sep 2023 03:06:35 +0900 Subject: [PATCH 25/77] AP_Mount: SWITCH statement to table reference --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 43 ++++++++-------------------- 1 file changed, 12 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index bb2ee5ee025fc8..2202446b929982 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -916,39 +916,20 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens) } // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful - CameraImageType cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; - switch (lens) { - case 0: - cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL; // 3 - break; - case 1: - cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL; // 5 - break; - case 2: - cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM; // 7 - break; - case 3: - cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE; // 0 - break; - case 4: - cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM; // 1 - break; - case 5: - cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL; // 2 - break; - case 6: - cam_image_type = CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE; // 4 - break; - case 7: - cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM; // 6 - break; - case 8: - cam_image_type = CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE; // 8 - break; - } + static CameraImageType cam_image_type_table[9] = { + CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 + CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5 + CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 + CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0 + CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1 + CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2 + CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE, // 4 + CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM, // 6 + CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8 + }; // send desired image type to camera - return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type); + return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type_table[lens]); } // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type From 7f97e897e26310bc9e07aec2db8c44ae08dbc2d5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 10 Jul 2024 22:41:02 +1000 Subject: [PATCH 26/77] AP_Camera: add sanity check for Siyi lens change --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 2202446b929982..2158eb6dad707f 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -916,7 +916,7 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens) } // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful - static CameraImageType cam_image_type_table[9] = { + static const CameraImageType cam_image_type_table[] { CameraImageType::MAIN_ZOOM_SUB_THERMAL, // 3 CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL, // 5 CameraImageType::MAIN_THERMAL_SUB_ZOOM, // 7 @@ -928,6 +928,10 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens) CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE, // 8 }; + if (lens >= ARRAY_SIZE(cam_image_type_table)) { + return false; + } + // send desired image type to camera return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type_table[lens]); } From 1e001fa0174bd7937d01b7e24f87cf3e7a5e51ad Mon Sep 17 00:00:00 2001 From: muramura Date: Thu, 2 May 2024 15:47:00 +0900 Subject: [PATCH 27/77] Rover: Change the IF statement to a SWITCH statement --- Rover/GCS_Mavlink.cpp | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index e718273703ce62..54bd15f4319d53 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -856,10 +856,14 @@ void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_messa } // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && - packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && - packet.coordinate_frame != MAV_FRAME_BODY_NED && - packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { + switch (packet.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_OFFSET_NED: + case MAV_FRAME_BODY_NED: + case MAV_FRAME_BODY_OFFSET_NED: + break; + + default: return; } @@ -975,14 +979,19 @@ void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_mess return; } // check for supported coordinate frames - if (packet.coordinate_frame != MAV_FRAME_GLOBAL && - packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT && - packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { + switch (packet.coordinate_frame) { + case MAV_FRAME_GLOBAL: + case MAV_FRAME_GLOBAL_INT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT: + case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + break; + + default: return; } + bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; From f849187333b02a898b7cb9e07117eada751f2045 Mon Sep 17 00:00:00 2001 From: aditya Date: Mon, 2 Oct 2023 03:13:19 +0530 Subject: [PATCH 28/77] build_options.py: add winch backends --- Tools/scripts/build_options.py | 3 +++ Tools/scripts/extract_features.py | 1 + 2 files changed, 4 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index bb19e6b4c69bf2..e613550f16e156 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -191,6 +191,9 @@ def __init__(self, Feature('Payload', 'SPRAYER', 'HAL_SPRAYER_ENABLED', 'Enable Sprayer', 0, None), Feature('Payload', 'LANDING_GEAR', 'AP_LANDINGGEAR_ENABLED', 'Enable Landing Gear', 0, None), Feature('Payload', 'WINCH', 'AP_WINCH_ENABLED', 'Enable Winch', 0, None), + Feature('Payload', 'WINCH_DAIWA', 'AP_WINCH_DAIWA_ENABLED', 'Enable DAIWA Winch support', 0, 'WINCH'), + Feature('Payload', 'WINCH_PWM', 'AP_WINCH_PWM_ENABLED', 'Enable PWM Winch support', 0, 'WINCH'), + Feature('Payload', 'RELAY', 'AP_RELAY_ENABLED', 'Enable Relay support', 0, None), Feature('Payload', 'SERVORELAY_EVENTS', 'AP_SERVORELAYEVENTS_ENABLED', 'Enable Servo/Relay Event support', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index c1f3aa86b6cb5f..7143758470446b 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -170,6 +170,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_SPRAYER_ENABLED', 'AC_Sprayer::AC_Sprayer',), ('AP_LANDINGGEAR_ENABLED', r'AP_LandingGear::init\b',), ('AP_WINCH_ENABLED', 'AP_Winch::AP_Winch',), + ('AP_WINCH_{type}_ENABLED', r'AP_Winch_(?P.*)::update\b',), ('AP_RELAY_ENABLED', 'AP_Relay::init',), ('AP_SERVORELAYEVENTS_ENABLED', 'AP_ServoRelayEvents::update_events',), From 278595cf83c9d7a02ca366442277bc585d489987 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Jul 2024 12:25:24 +0100 Subject: [PATCH 29/77] AP_HAL_ChibiOS: add support for sdcard to iFlight 2RAW --- libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat index 800775bb31e304..d90fe2e67904ec 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat @@ -36,6 +36,11 @@ PB13 SPI2_SCK SPI2 PB14 SPI2_MISO SPI2 PB15 SPI2_MOSI SPI2 +# SPI3 +PC12 SPI3_MOSI SPI3 +PC11 SPI3_MISO SPI3 +PC10 SPI3_SCK SPI3 + # Chip select pins PA15 SDCARD1_CS CS PB12 OSD1_CS CS @@ -123,10 +128,11 @@ define HAL_GPIO_B_LED_PIN 91 BARO SPL06 I2C:1:0x76 -# IMU setup +# SPI setup +SPIDEV imu1 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ +SPIDEV sdcard SPI3 DEVID1 SDCARD1_CS MODE0 400*KHZ 25*MHZ # IMU setup -SPIDEV imu1 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ IMU Invensensev3 SPI:imu1 ROTATION_ROLL_180 DMA_NOSHARE TIM3_UP TIM5_UP TIM4_UP SPI1* @@ -141,3 +147,4 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 define HAL_DEFAULT_INS_FAST_SAMPLE 1 # Motor order implies Betaflight/X for standard ESCs define HAL_FRAME_TYPE_DEFAULT 12 +define HAL_OS_FATFS_IO 1 From 74f72db4eb0b2838c73ca5b199104f291090afef Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Jul 2024 12:27:21 +0100 Subject: [PATCH 30/77] AP_HAL_ChibiOS: rename 2RAWH743 to IFLIGHT_2RAW_H7 --- .../hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/README.md | 0 .../{2RAWH743 => IFLIGHT_2RAW_H7}/Thunder-H7-1.png | Bin .../{2RAWH743 => IFLIGHT_2RAW_H7}/Thunder-H7-2.png | Bin .../{2RAWH743 => IFLIGHT_2RAW_H7}/defaults.parm | 0 .../{2RAWH743 => IFLIGHT_2RAW_H7}/hwdef-bl.dat | 0 .../hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/hwdef.dat | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/README.md (100%) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/Thunder-H7-1.png (100%) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/Thunder-H7-2.png (100%) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/defaults.parm (100%) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/hwdef-bl.dat (100%) rename libraries/AP_HAL_ChibiOS/hwdef/{2RAWH743 => IFLIGHT_2RAW_H7}/hwdef.dat (100%) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/README.md b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/README.md rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-1.png b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-1.png similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-1.png rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-1.png diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-2.png b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-2.png similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/Thunder-H7-2.png rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/Thunder-H7-2.png diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/defaults.parm similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/defaults.parm rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef-bl.dat similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef-bl.dat rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef.dat similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/2RAWH743/hwdef.dat rename to libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/hwdef.dat From 3c9cf98cca4be87ef32487f4540d10320661f9eb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Jul 2024 12:28:32 +0100 Subject: [PATCH 31/77] bootloaders: rename 2RAWH743 to IFLIGHT_2RAW_H7 --- .../{2RAWH743_bl.bin => IFLIGHT_2RAW_H7_bl.bin} | Bin .../{2RAWH743_bl.hex => IFLIGHT_2RAW_H7_bl.hex} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename Tools/bootloaders/{2RAWH743_bl.bin => IFLIGHT_2RAW_H7_bl.bin} (100%) rename Tools/bootloaders/{2RAWH743_bl.hex => IFLIGHT_2RAW_H7_bl.hex} (100%) diff --git a/Tools/bootloaders/2RAWH743_bl.bin b/Tools/bootloaders/IFLIGHT_2RAW_H7_bl.bin similarity index 100% rename from Tools/bootloaders/2RAWH743_bl.bin rename to Tools/bootloaders/IFLIGHT_2RAW_H7_bl.bin diff --git a/Tools/bootloaders/2RAWH743_bl.hex b/Tools/bootloaders/IFLIGHT_2RAW_H7_bl.hex similarity index 100% rename from Tools/bootloaders/2RAWH743_bl.hex rename to Tools/bootloaders/IFLIGHT_2RAW_H7_bl.hex From a560f89b35990ac251b10724e6dcb212913608ac Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 25 Jul 2024 12:33:56 +0100 Subject: [PATCH 32/77] AP_HAL_ChibiOS: update logging docs for 2RAW H743 --- libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md index 1264790660f38f..9cb60c81804dcc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/IFLIGHT_2RAW_H7/README.md @@ -7,7 +7,7 @@ The iFlight 2RAW H743 is a flight controller produced by [iFlight](https://shop. - MCU - STM32H743 32-bit processor running at 480 MHz - IMU - ICM42688P - Barometer - SPL06-001 - - Onboard Flash: 1GBit (not useable by ArduPilot) + - Onboard Flash: 1GBit exposed as microSD card - 6x UARTs - 9x PWM Outputs (8 Motor Output, 1 LED) - Battery input voltage: 2S-6S @@ -88,6 +88,11 @@ The iFlight 2RAW H743 does not have a builtin compass, but you can attach an ext GPIO 81 controls the VTX BEC output to pins marked "10V". Setting this GPIO low removes voltage supply to pins. By default RELAY2 is configured to control this pin and sets the GPIO high. +## Logging + +Logging is via a 1GBit flash chip exposed via a microSD interface. In order to be used you must format the card using mission planner +after the first time you have loaded the firmware + ## Loading Firmware Initial firmware load can be done with DFU by plugging in USB with the From 62a03f18d8ce8e9fa48732e02070f7ccba676f03 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Jul 2024 11:07:42 +0900 Subject: [PATCH 33/77] AP_Camera: type param desc gets viewpro and xacti --- libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 7fb915082c991e..8890148684638e 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), From 44b32171785490ed3f4d66d45786074b59019a55 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 25 Jul 2024 18:06:55 +0900 Subject: [PATCH 34/77] AP_Camera: type param desc gets topotek --- libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 8890148684638e..749d2985f0ab40 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), From 5b5dd638c1ebb450dbd788f21b5a2eac8168b611 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 13:07:31 +1000 Subject: [PATCH 35/77] hwdef: FlyingMoonH743: remove MAX7456 --- libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat index 4d81362bd1f951..9e1c637b83bcc2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonH743/hwdef.dat @@ -73,7 +73,7 @@ PD1 CAN1_TX CAN1 PB12 CAN2_RX CAN2 PB13 CAN2_TX CAN2 -# SPI1 for IMU OSD +# SPI1 for IMU PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 PA7 SPI1_MOSI SPI1 @@ -81,7 +81,6 @@ PB0 ICM20689_CS CS PB1 ICM42605_CS CS PB2 ICM20649_CS CS PA4 RM3100_CS CS -PA8 MAX7456_CS CS # SPI bus for dataflash AND SD PD3 SPI2_SCK SPI2 @@ -100,7 +99,6 @@ SPIDEV icm20689 SPI1 DEVID1 ICM20689_CS MODE3 2*MHZ 8*MHZ SPIDEV icm20649 SPI1 DEVID2 ICM20649_CS MODE3 2*MHZ 8*MHZ SPIDEV icm42605 SPI1 DEVID3 ICM42605_CS MODE3 2*MHZ 8*MHZ SPIDEV rm3100 SPI1 DEVID4 RM3100_CS MODE3 1*MHZ 1*MHZ -SPIDEV osd SPI1 DEVID5 MAX7456_CS MODE0 10*MHZ 10*MHZ SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV sdcard SPI2 DEVID2 FLASH_CS MODE0 1*MHZ 8*MHZ @@ -195,8 +193,3 @@ PE8 SAFETY_IN INPUT PULLDOWN # PWM output for buzzer PE5 TIM15_CH1 TIM15 GPIO(77) ALARM - -# setup for OSD -define OSD_ENABLED 1 -define HAL_OSD_TYPE_DEFAULT 1 -ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin From 78dc5a7b5db3e85d411a1134b0ffe2c2c8375a12 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 13:08:22 +1000 Subject: [PATCH 36/77] hwdef: FlyingMoonF427: remove MAX7456 --- libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat index 15306778fb10bb..5df193ee03de25 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlyingMoonF427/hwdef.dat @@ -66,7 +66,7 @@ PD2 UART5_RX UART5 NODMA PD0 CAN1_RX CAN1 PD1 CAN1_TX CAN1 -# SPI1 for IMU OSD +# SPI1 for IMU PA5 SPI1_SCK SPI1 PA6 SPI1_MISO SPI1 PA7 SPI1_MOSI SPI1 @@ -74,7 +74,6 @@ PB0 ICM20689_CS CS PB1 ICM42605_CS CS PB2 ICM20649_CS CS PA4 RM3100_CS CS -PE10 MAX7456_CS CS # SPI bus for dataflash AND SD PB13 SPI2_SCK SPI2 @@ -93,7 +92,6 @@ SPIDEV icm20689 SPI1 DEVID1 ICM20689_CS MODE3 2*MHZ 8*MHZ SPIDEV icm20649 SPI1 DEVID2 ICM20649_CS MODE3 2*MHZ 8*MHZ SPIDEV icm42605 SPI1 DEVID3 ICM42605_CS MODE3 2*MHZ 8*MHZ SPIDEV rm3100 SPI1 DEVID4 RM3100_CS MODE3 1*MHZ 1*MHZ -SPIDEV osd SPI1 DEVID5 MAX7456_CS MODE0 10*MHZ 10*MHZ SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV sdcard SPI2 DEVID2 FLASH_CS MODE0 1*MHZ 8*MHZ @@ -187,7 +185,3 @@ PE7 LED_SAFETY OUTPUT PE8 SAFETY_IN INPUT PULLDOWN PE5 TIM9_CH1 TIM9 ALARM -# setup for OSD -define OSD_ENABLED 1 -define HAL_OSD_TYPE_DEFAULT 1 -ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin From 82d59f6ba7b68b5352b143415a1ccc6a0dd0d191 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 11:48:33 +1000 Subject: [PATCH 37/77] autotest: tidy various tests to take advantage of new infrastructure autotest: clean MotorVibration test for new infrastructure autotest: clean VisionPosition test for new infrastructure autotest: clean MotorFail test for new infrastructure autotest: clean OpticalFlowLimits test for new infrastructure autotest: tidy OpticalFlow test for new infrastructure autotest: tidy SetModeViaModeSwitch test for new infrastructure autotest: tidy SetModesViaAuxSwitch test for new infrastructure autotest: tidy SplineLastWaypoint test for new infrastructure autotest: tidy PIDNotches test for new infrastructure autotest: tidy ThrottleGainBoost test for new infrastructure autotest: tidy PrecisionLoiterCompanion test for new infrastructure autotest: tidy BeaconPosition test for new infrastructure --- Tools/autotest/arducopter.py | 990 +++++++++++++++-------------------- 1 file changed, 410 insertions(+), 580 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 47f4fcd5cf0e22..29c3205c3eaf77 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2472,10 +2472,10 @@ def OpticalFlow(self): '''test OpticalFlow in flight''' self.start_subtest("Make sure no crash if no rangefinder") - self.context_push() - - self.set_parameter("SIM_FLOW_ENABLE", 1) - self.set_parameter("FLOW_TYPE", 10) + self.set_parameters({ + "SIM_FLOW_ENABLE": 1, + "FLOW_TYPE": 10, + }) self.set_analog_rangefinder_parameters() @@ -2519,74 +2519,57 @@ def check_optical_flow(mav, m): self.fly_generic_mission("CMAC-copter-navtest.txt") - self.context_pop() - - self.reboot_sitl() - def OpticalFlowLimits(self): '''test EKF navigation limiting''' - ex = None - self.context_push() - try: - - self.set_parameter("SIM_FLOW_ENABLE", 1) - self.set_parameter("FLOW_TYPE", 10) - - self.configure_EKFs_to_use_optical_flow_instead_of_GPS() - - self.set_analog_rangefinder_parameters() - - self.set_parameter("SIM_GPS_DISABLE", 1) - self.set_parameter("SIM_TERRAIN", 0) + self.set_parameters({ + "SIM_FLOW_ENABLE": 1, + "FLOW_TYPE": 10, + "SIM_GPS_DISABLE": 1, + "SIM_TERRAIN": 0, + }) - self.reboot_sitl() + self.configure_EKFs_to_use_optical_flow_instead_of_GPS() - # we can't takeoff in loiter as we need flow healthy - self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800) - self.change_mode('LOITER') + self.set_analog_rangefinder_parameters() - # speed should be limited to <10m/s - self.set_rc(2, 1000) + self.reboot_sitl() - tstart = self.get_sim_time() - timeout = 60 - started_climb = False - while self.get_sim_time_cached() - tstart < timeout: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - spd = math.sqrt(m.vx**2 + m.vy**2) * 0.01 - alt = m.relative_alt*0.001 - - # calculate max speed from altitude above the ground - margin = 2.0 - max_speed = alt * 1.5 + margin - self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" % - (self.get_sim_time_cached() - tstart, - spd, - max_speed, alt)) - if spd > max_speed: - raise NotAchievedException(("Speed should be limited by" - "EKF optical flow limits")) - - # after 30 seconds start climbing - if not started_climb and self.get_sim_time_cached() - tstart > 30: - started_climb = True - self.set_rc(3, 1900) - self.progress("Moving higher") - - # check altitude is not climbing above 35m - if alt > 35: - raise NotAchievedException("Alt should be limited by EKF optical flow limits") + # we can't takeoff in loiter as we need flow healthy + self.takeoff(alt_min=5, mode='ALT_HOLD', require_absolute=False, takeoff_throttle=1800) + self.change_mode('LOITER') - except Exception as e: - self.print_exception_caught(e) - ex = e + # speed should be limited to <10m/s + self.set_rc(2, 1000) - self.set_rc(2, 1500) + tstart = self.get_sim_time() + timeout = 60 + started_climb = False + while self.get_sim_time_cached() - tstart < timeout: + m = self.assert_receive_message('GLOBAL_POSITION_INT') + spd = math.sqrt(m.vx**2 + m.vy**2) * 0.01 + alt = m.relative_alt*0.001 + + # calculate max speed from altitude above the ground + margin = 2.0 + max_speed = alt * 1.5 + margin + self.progress("%0.1f: Low Speed: %f (want <= %u) alt=%.1f" % + (self.get_sim_time_cached() - tstart, + spd, + max_speed, alt)) + if spd > max_speed: + raise NotAchievedException(("Speed should be limited by" + "EKF optical flow limits")) + + # after 30 seconds start climbing + if not started_climb and self.get_sim_time_cached() - tstart > 30: + started_climb = True + self.set_rc(3, 1900) + self.progress("Moving higher") + + # check altitude is not climbing above 35m + if alt > 35: + raise NotAchievedException("Alt should be limited by EKF optical flow limits") self.reboot_sitl(force=True) - self.context_pop() - - if ex is not None: - raise ex def OpticalFlowCalibration(self): '''test optical flow calibration''' @@ -3259,122 +3242,109 @@ def GuidedEKFLaneChange(self): self.disarm_vehicle(force=True) self.reboot_sitl() - def MotorFail(self, fail_servo=0, fail_mul=0.0, holdtime=30): + def MotorFail(self, ): """Test flight with reduced motor efficiency""" - # we only expect an octocopter to survive ATM: - servo_counts = { - # 2: 6, # hexa - 3: 8, # octa - # 5: 6, # Y6 - } - frame_class = int(self.get_parameter("FRAME_CLASS")) - if frame_class not in servo_counts: - self.progress("Test not relevant for frame_class %u" % frame_class) - return - - servo_count = servo_counts[frame_class] + self.MotorFail_test_frame('octa', 8, frame_class=3) + # self.MotorFail_test_frame('hexa', 6, frame_class=2) + # self.MotorFail_test_frame('y6', 6, frame_class=5) - if fail_servo < 0 or fail_servo > servo_count: - raise ValueError('fail_servo outside range for frame class') - - self.takeoff(10, mode="LOITER") + def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fail_mul=0.0, holdtime=30): + self.set_parameters({ + 'FRAME_CLASS': frame_class, + }) + self.customise_SITL_commandline([], model=model) - self.change_alt(alt_min=50) + self.takeoff(25, mode="LOITER") # Get initial values - start_hud = self.mav.recv_match(type='VFR_HUD', blocking=True) - start_attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + start_hud = self.assert_receive_message('VFR_HUD') + start_attitude = self.assert_receive_message('ATTITUDE') hover_time = 5 - try: - tstart = self.get_sim_time() - int_error_alt = 0 - int_error_yaw_rate = 0 - int_error_yaw = 0 - self.progress("Hovering for %u seconds" % hover_time) - failed = False - while True: - now = self.get_sim_time_cached() - if now - tstart > holdtime + hover_time: - break + tstart = self.get_sim_time() + int_error_alt = 0 + int_error_yaw_rate = 0 + int_error_yaw = 0 + self.progress("Hovering for %u seconds" % hover_time) + failed = False + while True: + now = self.get_sim_time_cached() + if now - tstart > holdtime + hover_time: + break - servo = self.mav.recv_match(type='SERVO_OUTPUT_RAW', - blocking=True) - hud = self.mav.recv_match(type='VFR_HUD', blocking=True) - attitude = self.mav.recv_match(type='ATTITUDE', blocking=True) + servo = self.assert_receive_message('SERVO_OUTPUT_RAW') + hud = self.assert_receive_message('VFR_HUD') + attitude = self.assert_receive_message('ATTITUDE') - if not failed and now - tstart > hover_time: - self.progress("Killing motor %u (%u%%)" % - (fail_servo+1, fail_mul)) - self.set_parameters({ - "SIM_ENGINE_FAIL": fail_servo, - "SIM_ENGINE_MUL": fail_mul, - }) - failed = True + if not failed and now - tstart > hover_time: + self.progress("Killing motor %u (%u%%)" % + (fail_servo+1, fail_mul)) + self.set_parameters({ + "SIM_ENGINE_FAIL": fail_servo, + "SIM_ENGINE_MUL": fail_mul, + }) + failed = True - if failed: - self.progress("Hold Time: %f/%f" % (now-tstart, holdtime)) - - servo_pwm = [servo.servo1_raw, - servo.servo2_raw, - servo.servo3_raw, - servo.servo4_raw, - servo.servo5_raw, - servo.servo6_raw, - servo.servo7_raw, - servo.servo8_raw] - - self.progress("PWM output per motor") - for i, pwm in enumerate(servo_pwm[0:servo_count]): - if pwm > 1900: - state = "oversaturated" - elif pwm < 1200: - state = "undersaturated" - else: - state = "OK" + if failed: + self.progress("Hold Time: %f/%f" % (now-tstart, holdtime)) + + servo_pwm = [ + servo.servo1_raw, + servo.servo2_raw, + servo.servo3_raw, + servo.servo4_raw, + servo.servo5_raw, + servo.servo6_raw, + servo.servo7_raw, + servo.servo8_raw, + ] - if failed and i == fail_servo: - state += " (failed)" + self.progress("PWM output per motor") + for i, pwm in enumerate(servo_pwm[0:servo_count]): + if pwm > 1900: + state = "oversaturated" + elif pwm < 1200: + state = "undersaturated" + else: + state = "OK" - self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) + if failed and i == fail_servo: + state += " (failed)" - alt_delta = hud.alt - start_hud.alt - yawrate_delta = attitude.yawspeed - start_attitude.yawspeed - yaw_delta = attitude.yaw - start_attitude.yaw + self.progress("servo %u [pwm=%u] [%s]" % (i+1, pwm, state)) - self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) - self.progress("Yaw rate=%f (delta=%f) (rad/s)" % - (attitude.yawspeed, yawrate_delta)) - self.progress("Yaw=%f (delta=%f) (deg)" % - (attitude.yaw, yaw_delta)) + alt_delta = hud.alt - start_hud.alt + yawrate_delta = attitude.yawspeed - start_attitude.yawspeed + yaw_delta = attitude.yaw - start_attitude.yaw - dt = self.get_sim_time() - now - int_error_alt += abs(alt_delta/dt) - int_error_yaw_rate += abs(yawrate_delta/dt) - int_error_yaw += abs(yaw_delta/dt) - self.progress("## Error Integration ##") - self.progress(" Altitude: %fm" % int_error_alt) - self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) - self.progress(" Yaw: %f deg" % int_error_yaw) - self.progress("----") + self.progress("Alt=%fm (delta=%fm)" % (hud.alt, alt_delta)) + self.progress("Yaw rate=%f (delta=%f) (rad/s)" % + (attitude.yawspeed, yawrate_delta)) + self.progress("Yaw=%f (delta=%f) (deg)" % + (attitude.yaw, yaw_delta)) - if int_error_yaw_rate > 0.1: - raise NotAchievedException("Vehicle is spinning") + dt = self.get_sim_time() - now + int_error_alt += abs(alt_delta/dt) + int_error_yaw_rate += abs(yawrate_delta/dt) + int_error_yaw += abs(yaw_delta/dt) + self.progress("## Error Integration ##") + self.progress(" Altitude: %fm" % int_error_alt) + self.progress(" Yaw rate: %f rad/s" % int_error_yaw_rate) + self.progress(" Yaw: %f deg" % int_error_yaw) + self.progress("----") - if alt_delta < -20: - raise NotAchievedException("Vehicle is descending") + if int_error_yaw > 5: + raise NotAchievedException("Vehicle is spinning") - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) - except Exception as e: - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) - raise e + if alt_delta < -20: + raise NotAchievedException("Vehicle is descending") + + self.progress("Fixing motors") + self.set_parameters({ + "SIM_ENGINE_FAIL": 0, + "SIM_ENGINE_MUL": 1.0, + }) self.do_RTL() @@ -3392,83 +3362,67 @@ def hover_for_interval(self, hover_time): def MotorVibration(self): """Test flight with motor vibration""" - self.context_push() - - ex = None - try: - self.set_rc_default() - # magic tridge EKF type that dramatically speeds up the test - self.set_parameters({ - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "LOG_BITMASK": 958, - "LOG_DISARMED": 0, - "SIM_VIB_MOT_MAX": 350, - # these are real values taken from a 180mm Quad: - "SIM_GYR1_RND": 20, - "SIM_ACC1_RND": 5, - "SIM_ACC2_RND": 5, - "SIM_INS_THR_MIN": 0.1, - }) - self.reboot_sitl() - - # do a simple up-and-down flight to gather data: - self.takeoff(15, mode="ALT_HOLD") - tstart, tend, hover_throttle = self.hover_for_interval(15) - # if we don't reduce vibes here then the landing detector - # may not trigger - self.set_parameter("SIM_VIB_MOT_MAX", 0) - self.do_RTL() - - psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) - # ignore the first 20Hz and look for a peak at -15dB or more - # it should be at about 190Hz, each bin is 1000/1024Hz wide - ignore_bins = int(100 * 1.024) # start at 100Hz to be safe - freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] - if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300: - raise NotAchievedException( - "Did not detect a motor peak, found %f at %f dB" % - (freq, numpy.amax(psd["X"][ignore_bins:]))) - else: - self.progress("Detected motor peak at %fHz" % freq) - - # now add a notch and check that post-filter the peak is squashed below 40dB - self.set_parameters({ - "INS_LOG_BAT_OPT": 2, - "INS_HNTC2_ENABLE": 1, - "INS_HNTC2_FREQ": freq, - "INS_HNTC2_ATT": 50, - "INS_HNTC2_BW": freq/2, - "INS_HNTC2_MODE": 0, - "SIM_VIB_MOT_MAX": 350, - }) - self.reboot_sitl() - - # do a simple up-and-down flight to gather data: - self.takeoff(15, mode="ALT_HOLD") - tstart, tend, hover_throttle = self.hover_for_interval(15) - self.set_parameter("SIM_VIB_MOT_MAX", 0) - self.do_RTL() + # magic tridge EKF type that dramatically speeds up the test + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "LOG_BITMASK": 958, + "LOG_DISARMED": 0, + "SIM_VIB_MOT_MAX": 350, + # these are real values taken from a 180mm Quad: + "SIM_GYR1_RND": 20, + "SIM_ACC1_RND": 5, + "SIM_ACC2_RND": 5, + "SIM_INS_THR_MIN": 0.1, + }) + self.reboot_sitl() - psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) - freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] - peakdB = numpy.amax(psd["X"][ignore_bins:]) - if peakdB < -23: - self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB)) - else: - raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB)) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) + # do a simple up-and-down flight to gather data: + self.takeoff(15, mode="ALT_HOLD") + tstart, tend, hover_throttle = self.hover_for_interval(15) + # if we don't reduce vibes here then the landing detector + # may not trigger + self.set_parameter("SIM_VIB_MOT_MAX", 0) + self.do_RTL() - self.context_pop() + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + # ignore the first 20Hz and look for a peak at -15dB or more + # it should be at about 190Hz, each bin is 1000/1024Hz wide + ignore_bins = int(100 * 1.024) # start at 100Hz to be safe + freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] + if numpy.amax(psd["X"][ignore_bins:]) < -15 or freq < 100 or freq > 300: + raise NotAchievedException( + "Did not detect a motor peak, found %f at %f dB" % + (freq, numpy.amax(psd["X"][ignore_bins:]))) + else: + self.progress("Detected motor peak at %fHz" % freq) + # now add a notch and check that post-filter the peak is squashed below 40dB + self.set_parameters({ + "INS_LOG_BAT_OPT": 2, + "INS_HNTC2_ENABLE": 1, + "INS_HNTC2_FREQ": freq, + "INS_HNTC2_ATT": 50, + "INS_HNTC2_BW": freq/2, + "INS_HNTC2_MODE": 0, + "SIM_VIB_MOT_MAX": 350, + }) self.reboot_sitl() - if ex is not None: - raise ex + # do a simple up-and-down flight to gather data: + self.takeoff(15, mode="ALT_HOLD") + tstart, tend, hover_throttle = self.hover_for_interval(15) + self.set_parameter("SIM_VIB_MOT_MAX", 0) + self.do_RTL() + + psd = self.mavfft_fttd(1, 0, tstart * 1.0e6, tend * 1.0e6) + freq = psd["F"][numpy.argmax(psd["X"][ignore_bins:]) + ignore_bins] + peakdB = numpy.amax(psd["X"][ignore_bins:]) + if peakdB < -23: + self.progress("Did not detect a motor peak, found %f at %f dB" % (freq, peakdB)) + else: + raise NotAchievedException("Detected peak %.1f Hz %.2f dB" % (freq, peakdB)) def VisionPosition(self): """Disable GPS navigation, enable Vicon input.""" @@ -3482,91 +3436,62 @@ def VisionPosition(self): old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) print("old_pos=%s" % str(old_pos)) - self.context_push() - - ex = None - try: - # configure EKF to use external nav instead of GPS - ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE") - if ahrs_ekf_type == 2: - self.set_parameter("EK2_GPS_TYPE", 3) - if ahrs_ekf_type == 3: - self.set_parameters({ - "EK3_SRC1_POSXY": 6, - "EK3_SRC1_VELXY": 6, - "EK3_SRC1_POSZ": 6, - "EK3_SRC1_VELZ": 6, - }) + # configure EKF to use external nav instead of GPS + ahrs_ekf_type = self.get_parameter("AHRS_EKF_TYPE") + if ahrs_ekf_type == 2: + self.set_parameter("EK2_GPS_TYPE", 3) + if ahrs_ekf_type == 3: self.set_parameters({ - "GPS1_TYPE": 0, - "VISO_TYPE": 1, - "SERIAL5_PROTOCOL": 1, + "EK3_SRC1_POSXY": 6, + "EK3_SRC1_VELXY": 6, + "EK3_SRC1_POSZ": 6, + "EK3_SRC1_VELZ": 6, }) - self.reboot_sitl() - # without a GPS or some sort of external prompting, AP - # doesn't send system_time messages. So prompt it: - self.mav.mav.system_time_send(int(time.time() * 1000000), 0) - self.progress("Waiting for non-zero-lat") - tstart = self.get_sim_time() - while True: - self.mav.mav.set_gps_global_origin_send(1, - old_pos.lat, - old_pos.lon, - old_pos.alt) - gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - self.progress("gpi=%s" % str(gpi)) - if gpi.lat != 0: - break - - if self.get_sim_time_cached() - tstart > 60: - raise AutoTestTimeoutException("Did not get non-zero lat") - - self.takeoff() - self.set_rc(1, 1600) - tstart = self.get_sim_time() - while True: - vicon_pos = self.mav.recv_match(type='VISION_POSITION_ESTIMATE', - blocking=True) - # print("vpe=%s" % str(vicon_pos)) - self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - # self.progress("gpi=%s" % str(gpi)) - if vicon_pos.x > 40: - break - - if self.get_sim_time_cached() - tstart > 100: - raise AutoTestTimeoutException("Vicon showed no movement") - - # recenter controls: - self.set_rc(1, 1500) - self.progress("# Enter RTL") - self.change_mode('RTL') - self.set_rc(3, 1500) - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 200: - raise NotAchievedException("Did not disarm") - self.mav.recv_match(type='GLOBAL_POSITION_INT', - blocking=True) - # print("gpi=%s" % str(gpi)) - self.mav.recv_match(type='SIMSTATE', - blocking=True) - # print("ss=%s" % str(ss)) - # wait for RTL disarm: - if not self.armed(): - break + self.set_parameters({ + "GPS1_TYPE": 0, + "VISO_TYPE": 1, + "SERIAL5_PROTOCOL": 1, + }) + self.reboot_sitl() + # without a GPS or some sort of external prompting, AP + # doesn't send system_time messages. So prompt it: + self.mav.mav.system_time_send(int(time.time() * 1000000), 0) + self.progress("Waiting for non-zero-lat") + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 60: + raise AutoTestTimeoutException("Did not get non-zero lat") + self.mav.mav.set_gps_global_origin_send(1, + old_pos.lat, + old_pos.lon, + old_pos.alt) + gpi = self.assert_receive_message('GLOBAL_POSITION_INT') + self.progress("gpi=%s" % str(gpi)) + if gpi.lat != 0: + break - except Exception as e: - self.print_exception_caught(e) - ex = e + self.takeoff() + self.set_rc(1, 1600) + tstart = self.get_sim_time() + while True: + vicon_pos = self.assert_receive_message('VISION_POSITION_ESTIMATE') + # print("vpe=%s" % str(vicon_pos)) + # gpi = self.assert_receive_message('GLOBAL_POSITION_INT') + # self.progress("gpi=%s" % str(gpi)) + if vicon_pos.x > 40: + break - self.context_pop() - self.zero_throttle() - self.reboot_sitl() + if self.get_sim_time_cached() - tstart > 100: + raise AutoTestTimeoutException("Vicon showed no movement") - if ex is not None: - raise ex + # recenter controls: + self.set_rc(1, 1500) + self.progress("# Enter RTL") + self.change_mode('RTL') + self.set_rc(3, 1500) + tstart = self.get_sim_time() + # self.install_messageprinter_handlers_context(['SIMSTATE', 'GLOBAL_POSITION_INT']) + self.wait_disarmed(timeout=200) def BodyFrameOdom(self): """Disable GPS navigation, enable input of VISION_POSITION_DELTA.""" @@ -4682,75 +4607,52 @@ def ModeZigZag(self): def SetModesViaModeSwitch(self): '''Set modes via modeswitch''' - self.context_push() - ex = None - try: - fltmode_ch = 5 - self.set_parameter("FLTMODE_CH", fltmode_ch) - self.set_rc(fltmode_ch, 1000) # PWM for mode1 - testmodes = [("FLTMODE1", 4, "GUIDED", 1165), - ("FLTMODE2", 2, "ALT_HOLD", 1295), - ("FLTMODE3", 6, "RTL", 1425), - ("FLTMODE4", 7, "CIRCLE", 1555), - ("FLTMODE5", 1, "ACRO", 1685), - ("FLTMODE6", 17, "BRAKE", 1815), - ] - for mode in testmodes: - (parm, parm_value, name, pwm) = mode - self.set_parameter(parm, parm_value) - - for mode in reversed(testmodes): - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - for mode in testmodes: - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - for mode in reversed(testmodes): - (parm, parm_value, name, pwm) = mode - self.set_rc(fltmode_ch, pwm) - self.wait_mode(name) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + fltmode_ch = 5 + self.set_parameter("FLTMODE_CH", fltmode_ch) + self.set_rc(fltmode_ch, 1000) # PWM for mode1 + testmodes = [("FLTMODE1", 4, "GUIDED", 1165), + ("FLTMODE2", 2, "ALT_HOLD", 1295), + ("FLTMODE3", 6, "RTL", 1425), + ("FLTMODE4", 7, "CIRCLE", 1555), + ("FLTMODE5", 1, "ACRO", 1685), + ("FLTMODE6", 17, "BRAKE", 1815), + ] + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_parameter(parm, parm_value) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in testmodes: + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) + + for mode in reversed(testmodes): + (parm, parm_value, name, pwm) = mode + self.set_rc(fltmode_ch, pwm) + self.wait_mode(name) def SetModesViaAuxSwitch(self): '''"Set modes via auxswitch"''' - self.context_push() - ex = None - try: - fltmode_ch = int(self.get_parameter("FLTMODE_CH")) - self.set_rc(fltmode_ch, 1000) - self.wait_mode("CIRCLE") - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.set_parameters({ - "RC9_OPTION": 18, # land - "RC10_OPTION": 55, # guided - }) - self.set_rc(9, 1900) - self.wait_mode("LAND") - self.set_rc(10, 1900) - self.wait_mode("GUIDED") - self.set_rc(10, 1000) # this re-polls the mode switch - self.wait_mode("CIRCLE") - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + fltmode_ch = int(self.get_parameter("FLTMODE_CH")) + self.set_rc(fltmode_ch, 1000) + self.wait_mode("CIRCLE") + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameters({ + "RC9_OPTION": 18, # land + "RC10_OPTION": 55, # guided + }) + self.set_rc(9, 1900) + self.wait_mode("LAND") + self.set_rc(10, 1900) + self.wait_mode("GUIDED") + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("CIRCLE") def fly_guided_stop(self, timeout=20, @@ -5294,24 +5196,14 @@ def TestGripperMission(self): def SplineLastWaypoint(self): '''Test Spline as last waypoint''' - self.context_push() - ex = None - try: - self.load_mission("copter-spline-last-waypoint.txt") - self.change_mode('LOITER') - self.wait_ready_to_arm() - self.arm_vehicle() - self.change_mode('AUTO') - self.set_rc(3, 1500) - self.wait_altitude(10, 3000, relative=True) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.load_mission("copter-spline-last-waypoint.txt") + self.change_mode('LOITER') + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode('AUTO') + self.set_rc(3, 1500) + self.wait_altitude(10, 3000, relative=True) self.do_RTL() - self.wait_disarmed() - if ex is not None: - raise ex def ManualThrottleModeChange(self): '''Check manual throttle mode changes denied on high throttle''' @@ -6420,90 +6312,60 @@ def extract_median_FTN1_PkAvg_from_current_onboard_log(self, tstart, tend): def PIDNotches(self): """Use dynamic harmonic notch to control motor noise.""" self.progress("Flying with PID notches") - self.context_push() - - ex = None - try: - self.set_parameters({ - "FILT1_TYPE": 1, - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour - "LOG_BITMASK": 65535, - "LOG_DISARMED": 0, - "SIM_VIB_FREQ_X": 120, # roll - "SIM_VIB_FREQ_Y": 120, # pitch - "SIM_VIB_FREQ_Z": 180, # yaw - "FILT1_NOTCH_FREQ": 120, - "ATC_RAT_RLL_NEF": 1, - "ATC_RAT_PIT_NEF": 1, - "ATC_RAT_YAW_NEF": 1, - "SIM_GYR1_RND": 5, - }) - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() + self.set_parameters({ + "FILT1_TYPE": 1, + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 1, + "SIM_GYR1_RND": 5, + }) + self.reboot_sitl() - if ex is not None: - raise ex + self.hover_and_check_matched_frequency_with_fft(dblevel=5, minhz=20, maxhz=350, reverse=True) def ThrottleGainBoost(self): """Use PD and Angle P boost for anti-gravity.""" # basic gyro sample rate test self.progress("Flying with Throttle-Gain Boost") - self.context_push() - - ex = None - try: - # magic tridge EKF type that dramatically speeds up the test - self.set_parameters({ - "AHRS_EKF_TYPE": 10, - "EK2_ENABLE": 0, - "EK3_ENABLE": 0, - "INS_FAST_SAMPLE": 0, - "LOG_BITMASK": 959, - "LOG_DISARMED": 0, - "ATC_THR_G_BOOST": 5.0, - }) - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - hover_time = 15 - self.progress("Hovering for %u seconds" % hover_time) - tstart = self.get_sim_time() - while self.get_sim_time_cached() < tstart + hover_time: - self.mav.recv_match(type='ATTITUDE', blocking=True) - - # fly fast forrest! - self.set_rc(3, 1900) - self.set_rc(2, 1200) - self.wait_groundspeed(5, 1000) - self.set_rc(3, 1500) - self.set_rc(2, 1500) - - self.do_RTL() + # magic tridge EKF type that dramatically speeds up the test + self.set_parameters({ + "AHRS_EKF_TYPE": 10, + "EK2_ENABLE": 0, + "EK3_ENABLE": 0, + "INS_FAST_SAMPLE": 0, + "LOG_BITMASK": 959, + "LOG_DISARMED": 0, + "ATC_THR_G_BOOST": 5.0, + }) - except Exception as e: - self.print_exception_caught(e) - ex = e + self.reboot_sitl() - self.context_pop() + self.takeoff(10, mode="ALT_HOLD") + hover_time = 15 + self.progress("Hovering for %u seconds" % hover_time) + tstart = self.get_sim_time() + while self.get_sim_time_cached() < tstart + hover_time: + self.assert_receive_message('ATTITUDE') - # must reboot after we move away from EKF type 10 to EKF2 or EKF3 - self.reboot_sitl() + # fly fast forrest! + self.set_rc(3, 1900) + self.set_rc(2, 1200) + self.wait_groundspeed(5, 1000) + self.set_rc(3, 1500) + self.set_rc(2, 1500) - if ex is not None: - raise ex + self.do_RTL() def test_gyro_fft_harmonic(self, averaging): """Use dynamic harmonic notch to control motor noise with harmonic matching of the first harmonic.""" @@ -7267,55 +7129,41 @@ def AltTypes(self): def PrecisionLoiterCompanion(self): """Use Companion PrecLand backend precision messages to loiter.""" - self.context_push() - - ex = None - try: - self.set_parameters({ - "PLND_ENABLED": 1, - "PLND_TYPE": 1, # enable companion backend: - "RC7_OPTION": 39, # set up a channel switch to enable precision loiter: - }) - self.set_analog_rangefinder_parameters() - self.reboot_sitl() - - self.progress("Waiting for location") - self.change_mode('LOITER') - self.wait_ready_to_arm() + self.set_parameters({ + "PLND_ENABLED": 1, + "PLND_TYPE": 1, # enable companion backend: + "RC7_OPTION": 39, # set up a channel switch to enable precision loiter: + }) + self.set_analog_rangefinder_parameters() + self.reboot_sitl() - # we should be doing precision loiter at this point - start = self.assert_receive_message('LOCAL_POSITION_NED') + self.progress("Waiting for location") + self.change_mode('LOITER') + self.wait_ready_to_arm() - self.takeoff(20, mode='ALT_HOLD') + # we should be doing precision loiter at this point + start = self.assert_receive_message('LOCAL_POSITION_NED') - # move away a little - self.set_rc(2, 1550) - self.wait_distance(5, accuracy=1) - self.set_rc(2, 1500) - self.change_mode('LOITER') + self.takeoff(20, mode='ALT_HOLD') - # turn precision loiter on: - self.context_collect('STATUSTEXT') - self.set_rc(7, 2000) + # move away a little + self.set_rc(2, 1550) + self.wait_distance(5, accuracy=1) + self.set_rc(2, 1500) + self.change_mode('LOITER') - # try to drag aircraft to a position 5 metres north-east-east: - self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10) - self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10) - self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10) - # .... then northwest - self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10) + # turn precision loiter on: + self.context_collect('STATUSTEXT') + self.set_rc(7, 2000) - except Exception as e: - self.print_exception_caught(e) - ex = e + # try to drag aircraft to a position 5 metres north-east-east: + self.precision_loiter_to_pos(start.x + 5, start.y + 10, start.z + 10) + self.wait_statustext("PrecLand: Target Found", check_context=True, timeout=10) + self.wait_statustext("PrecLand: Init Complete", check_context=True, timeout=10) + # .... then northwest + self.precision_loiter_to_pos(start.x + 5, start.y - 10, start.z + 10) self.disarm_vehicle(force=True) - self.context_pop() - self.reboot_sitl() - self.progress("All done") - - if ex is not None: - raise ex def loiter_requires_position(self): # ensure we can't switch to LOITER without position @@ -7870,87 +7718,69 @@ def BeaconPosition(self): old_pos = self.get_global_position_int() print("old_pos=%s" % str(old_pos)) - self.context_push() - ex = None - try: - self.set_parameters({ - "BCN_TYPE": 10, - "BCN_LATITUDE": SITL_START_LOCATION.lat, - "BCN_LONGITUDE": SITL_START_LOCATION.lng, - "BCN_ALT": SITL_START_LOCATION.alt, - "BCN_ORIENT_YAW": 0, - "AVOID_ENABLE": 4, - "GPS1_TYPE": 0, - "EK3_ENABLE": 1, - "EK3_SRC1_POSXY": 4, # Beacon - "EK3_SRC1_POSZ": 1, # Baro - "EK3_SRC1_VELXY": 0, # None - "EK3_SRC1_VELZ": 0, # None - "EK2_ENABLE": 0, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() + self.set_parameters({ + "BCN_TYPE": 10, + "BCN_LATITUDE": SITL_START_LOCATION.lat, + "BCN_LONGITUDE": SITL_START_LOCATION.lng, + "BCN_ALT": SITL_START_LOCATION.alt, + "BCN_ORIENT_YAW": 0, + "AVOID_ENABLE": 4, + "GPS1_TYPE": 0, + "EK3_ENABLE": 1, + "EK3_SRC1_POSXY": 4, # Beacon + "EK3_SRC1_POSZ": 1, # Baro + "EK3_SRC1_VELXY": 0, # None + "EK3_SRC1_VELZ": 0, # None + "EK2_ENABLE": 0, + "AHRS_EKF_TYPE": 3, + }) + self.reboot_sitl() - # turn off GPS arming checks. This may be considered a - # bug that we need to do this. - old_arming_check = int(self.get_parameter("ARMING_CHECK")) - if old_arming_check == 1: - old_arming_check = 1 ^ 25 - 1 - new_arming_check = int(old_arming_check) & ~(1 << 3) - self.set_parameter("ARMING_CHECK", new_arming_check) + # turn off GPS arming checks. This may be considered a + # bug that we need to do this. + old_arming_check = int(self.get_parameter("ARMING_CHECK")) + if old_arming_check == 1: + old_arming_check = 1 ^ 25 - 1 + new_arming_check = int(old_arming_check) & ~(1 << 3) + self.set_parameter("ARMING_CHECK", new_arming_check) - self.reboot_sitl() + self.reboot_sitl() - # require_absolute=True infers a GPS is present - self.wait_ready_to_arm(require_absolute=False) + # require_absolute=True infers a GPS is present + self.wait_ready_to_arm(require_absolute=False) - tstart = self.get_sim_time() - timeout = 20 - while True: - if self.get_sim_time_cached() - tstart > timeout: - raise NotAchievedException("Did not get new position like old position") - self.progress("Fetching location") - new_pos = self.get_global_position_int() - pos_delta = self.get_distance_int(old_pos, new_pos) - max_delta = 1 - self.progress("delta=%u want <= %u" % (pos_delta, max_delta)) - if pos_delta <= max_delta: - break + tstart = self.get_sim_time() + timeout = 20 + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get new position like old position") + self.progress("Fetching location") + new_pos = self.get_global_position_int() + pos_delta = self.get_distance_int(old_pos, new_pos) + max_delta = 1 + self.progress("delta=%u want <= %u" % (pos_delta, max_delta)) + if pos_delta <= max_delta: + break - self.progress("Moving to ensure location is tracked") - self.takeoff(10, mode="STABILIZE") - self.change_mode("CIRCLE") + self.progress("Moving to ensure location is tracked") + self.takeoff(10, mode="STABILIZE") + self.change_mode("CIRCLE") - tstart = self.get_sim_time() - max_delta = 0 - max_allowed_delta = 10 - while True: - if self.get_sim_time_cached() - tstart > timeout: - break + self.context_push() + validator = vehicle_test_suite.TestSuite.ValidateGlobalPositionIntAgainstSimState(self, max_allowed_divergence=10) + self.install_message_hook_context(validator) - pos_delta = self.get_distance_int(self.sim_location_int(), self.get_global_position_int()) - self.progress("pos_delta=%f max_delta=%f max_allowed_delta=%f" % (pos_delta, max_delta, max_allowed_delta)) - if pos_delta > max_delta: - max_delta = pos_delta - if pos_delta > max_allowed_delta: - raise NotAchievedException("Vehicle location not tracking simulated location (%f > %f)" % - (pos_delta, max_allowed_delta)) - self.progress("Tracked location just fine (max_delta=%f)" % max_delta) - self.change_mode("LOITER") - self.wait_groundspeed(0, 0.3, timeout=120) - self.land_and_disarm() + self.delay_sim_time(20) + self.progress("Tracked location just fine") + self.context_pop() - self.assert_current_onboard_log_contains_message("BCN") + self.change_mode("LOITER") + self.wait_groundspeed(0, 0.3, timeout=120) + self.land_and_disarm() + + self.assert_current_onboard_log_contains_message("BCN") - except Exception as e: - self.print_exception_caught(e) - ex = e self.disarm_vehicle(force=True) - self.reboot_sitl() - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex def AC_Avoidance_Beacon(self): '''Test beacon avoidance slide behaviour''' From 1f60ca334227716829f1e01b1a78fed7184262e6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 12 May 2021 11:09:16 +1000 Subject: [PATCH 38/77] autotest: add SmartRTL test for rapid switch between smartrtl and althold --- Tools/autotest/arducopter.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 29c3205c3eaf77..7f70a37a0fe7a2 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9698,6 +9698,23 @@ def GPSForYaw(self): if ex is not None: raise ex + def SMART_RTL_EnterLeave(self): + '''check SmartRTL behaviour when entering/leaving''' + # we had a bug where we would consume points when re-entering smartrtl + + self.upload_simple_relhome_mission([ + # N E U + (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 10), + (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0), + ]) + self.set_parameter('AUTO_OPTIONS', 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.change_mode('ALT_HOLD') + self.change_mode('SMART_RTL') + self.change_mode('ALT_HOLD') + self.change_mode('SMART_RTL') + def GPSForYawCompassLearn(self): '''Moving baseline GPS yaw - with compass learning''' self.context_push() @@ -11711,6 +11728,7 @@ def tests2b(self): # this block currently around 9.5mins here self.GSF_reset, self.AP_Avoidance, self.SMART_RTL, + self.SMART_RTL_EnterLeave, self.RTL_TO_RALLY, self.FlyEachFrame, self.GPSBlending, @@ -11864,6 +11882,7 @@ def disabled_tests(self): "FlyMissionTwice": "See https://github.com/ArduPilot/ardupilot/pull/18561", "GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion", "CompassMot": "Cuases an arithmetic exception in the EKF", + "SMART_RTL_EnterLeave": "Causes a panic", } From bea2c5b59b633375f3e4f35d755e03c7e4cc603c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 21 Jul 2024 20:58:24 +0930 Subject: [PATCH 39/77] AC_Autotune: Clean up Multi Variables and non functional changes --- libraries/AC_AutoTune/AC_AutoTune.cpp | 11 +----- libraries/AC_AutoTune/AC_AutoTune.h | 9 ----- libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 44 ++++++++++++--------- libraries/AC_AutoTune/AC_AutoTune_Multi.h | 10 ++++- 4 files changed, 36 insertions(+), 38 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 6dac22cc742157..40e2e9e2e27d47 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -340,13 +340,6 @@ void AC_AutoTune::control_attitude() step_start_time_ms = now; step_time_limit_ms = get_testing_step_timeout_ms(); // set gains to their to-be-tested values - twitch_first_iter = true; - test_rate_max = 0.0f; - test_rate_min = 0.0f; - test_angle_max = 0.0f; - test_angle_min = 0.0f; - rotation_rate_filt.reset(0.0f); - rate_max = 0.0f; load_gains(GAIN_TEST); } else { // when waiting for level we use the intra-test gains @@ -356,18 +349,15 @@ void AC_AutoTune::control_attitude() // Initialize test-specific variables switch (axis) { case ROLL: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f; start_angle = ahrs_view->roll_sensor; break; case PITCH: - angle_finish = target_angle_max_rp_cd(); start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f; start_angle = ahrs_view->pitch_sensor; break; case YAW: case YAW_D: - angle_finish = target_angle_max_y_cd(); start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f; start_angle = ahrs_view->yaw_sensor; break; @@ -539,6 +529,7 @@ void AC_AutoTune::control_attitude() } if (axis == YAW || axis == YAW_D) { + // todo: check to make sure we need this attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); } diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 510fd6c59fbb4b..fb4233a23ef88b 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -259,24 +259,15 @@ class AC_AutoTune bool positive_direction; // false = tuning in negative direction (i.e. left for roll), true = positive direction (i.e. right for roll) StepType step; // see StepType for what steps are performed TuneType tune_type; // see TuneType - bool ignore_next; // true = ignore the next test bool twitch_first_iter; // true on first iteration of a twitch (used to signal we must step the attitude or rate target) uint8_t axes_completed; // bitmask of completed axes - float test_rate_min; // the minimum angular rate achieved during TESTING_RATE step-multi only - float test_rate_max; // the maximum angular rate achieved during TESTING_RATE step-multi only - float test_angle_min; // the minimum angle achieved during TESTING_ANGLE step-multi only - float test_angle_max; // the maximum angle achieved during TESTING_ANGLE step-multi only uint32_t step_start_time_ms; // start time of current tuning step (used for timeout checks) uint32_t step_time_limit_ms; // time limit of current autotune process uint32_t level_start_time_ms; // start time of waiting for level int8_t counter; // counter for tuning gains - float target_rate; // target rate-multi only - float target_angle; // target angle-multi only float start_angle; // start angle float start_rate; // start rate - parent and multi - float rate_max; // maximum rate variable - parent and multi float test_accel_max; // maximum acceleration variable - float angle_finish; // Angle that test is aborted- parent and multi float desired_yaw_cd; // yaw heading during tune - parent and Tradheli float step_scaler; // scaler to reduce maximum target step - parent and multi diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index d3ffdefe1acfb8..89c1a45b2444d5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -60,15 +60,15 @@ #define AUTOTUNE_RP_MAX 2.0 // maximum Rate P value #define AUTOTUNE_SP_MAX 40.0 // maximum Stab P value #define AUTOTUNE_SP_MIN 0.5 // maximum Stab P value -#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch -#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw +#define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch +#define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw #define AUTOTUNE_Y_FILT_FREQ 10.0 // Autotune filter frequency when testing Yaw #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2 // The margin below the target that we tune D in -#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning +#define AUTOTUNE_RD_BACKOFF 1.0 // Rate D gains are reduced to 50% of their maximum value discovered during tuning #define AUTOTUNE_RP_BACKOFF 1.0 // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning -#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning +#define AUTOTUNE_SP_BACKOFF 0.9 // Stab P gains are reduced to 90% of their maximum value discovered during tuning #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0 // back off from maximum acceleration -#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration +#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration // roll and pitch axes #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step @@ -653,11 +653,11 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl } // twitching_measure_acceleration - measure rate of change of measurement -void AC_AutoTune_Multi::twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const +void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const { - if (rate_measurement_max < rate_measurement) { - rate_measurement_max = rate_measurement; - rate_of_change = (1000.0f*rate_measurement_max)/(AP_HAL::millis() - step_start_time_ms); + if (rate_max < rate) { + rate_max = rate; + accel_average = (1000.0 * rate_max) / (AP_HAL::millis() - step_start_time_ms); } } @@ -1189,6 +1189,7 @@ void AC_AutoTune_Multi::twitch_test_init() float target_max_rate; switch (axis) { case ROLL: { + angle_abort = target_angle_max_rp_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); @@ -1196,6 +1197,7 @@ void AC_AutoTune_Multi::twitch_test_init() break; } case PITCH: { + angle_abort = target_angle_max_rp_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd()); @@ -1204,6 +1206,7 @@ void AC_AutoTune_Multi::twitch_test_init() } case YAW: case YAW_D: { + angle_abort = target_angle_max_y_cd(); target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS); target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw() * 0.75) * 100.0, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate); target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw() * 0.75) * 100.0, target_angle_min_y_cd(), target_angle_max_y_cd()); @@ -1217,11 +1220,16 @@ void AC_AutoTune_Multi::twitch_test_init() } if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { + // todo: consider if this should be done for other axis rotation_rate_filt.reset(start_rate); } else { - rotation_rate_filt.reset(0); + rotation_rate_filt.reset(0.0); } - + twitch_first_iter = true; + test_rate_max = 0.0; + test_rate_min = 0.0; + test_angle_max = 0.0; + test_angle_min = 0.0; } //run twitch test @@ -1312,18 +1320,18 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign case RD_UP: case RD_DOWN: twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case RP_UP: - twitching_test_rate(lean_angle, rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max, test_angle_min); - twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max); - twitching_abort_rate(lean_angle, rotation_rate, angle_finish, test_rate_min, test_angle_min); + twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min); + twitching_measure_acceleration(test_accel_max, rotation_rate, test_rate_max); + twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min); break; case SP_DOWN: case SP_UP: - twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); - twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, rate_max); + twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max); + twitching_measure_acceleration(test_accel_max, rotation_rate - dir_sign * start_rate, test_rate_max); break; case RFF_UP: case MAX_GAINS: diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index 3308ae5d8afb44..e580e4afcbf778 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -156,7 +156,7 @@ class AC_AutoTune_Multi : public AC_AutoTune void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); // measure acceleration during twitch test - void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max) const; + void twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const; // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P @@ -185,6 +185,14 @@ class AC_AutoTune_Multi : public AC_AutoTune AP_Int8 axis_bitmask; // axes to be tuned AP_Float aggressiveness; // aircraft response aggressiveness to be tuned AP_Float min_d; // minimum rate d gain allowed during tuning + bool ignore_next; // ignore the results of the next test when true + float target_angle; // target angle for the test + float target_rate; // target rate for the test + float angle_abort; // Angle that test is aborted + float test_rate_min; // the minimum angular rate achieved during TESTING_RATE + float test_rate_max; // the maximum angular rate achieved during TESTING_RATE + float test_angle_min; // the minimum angle achieved during TESTING_ANGLE + float test_angle_max; // the maximum angle achieved during TESTING_ANGLE }; #endif // AC_AUTOTUNE_ENABLED From bb1758ecb1d9167996a80c30f078dc838fc417d0 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 21 Jul 2024 22:41:41 +0930 Subject: [PATCH 40/77] AC_Autotune: Add ABORT state for consistency between tests --- libraries/AC_AutoTune/AC_AutoTune.cpp | 6 ++++++ libraries/AC_AutoTune/AC_AutoTune.h | 1 + libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 6 +----- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 40e2e9e2e27d47..bf86386c69fd40 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -134,6 +134,9 @@ void AC_AutoTune::send_step_string() case UPDATE_GAINS: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains"); return; + case ABORT: + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Aborting Test"); + return; case TESTING: gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Testing"); return; @@ -527,7 +530,9 @@ void AC_AutoTune::control_attitude() } } } + FALLTHROUGH; + case ABORT: if (axis == YAW || axis == YAW_D) { // todo: check to make sure we need this attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false); @@ -585,6 +590,7 @@ void AC_AutoTune::backup_gains_and_initialise() */ void AC_AutoTune::load_gains(enum GainType gain_type) { + // todo: add previous setting so gains are not loaded on each loop. switch (gain_type) { case GAIN_ORIGINAL: load_orig_gains(); diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index fb4233a23ef88b..68a8dcee507e75 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -198,6 +198,7 @@ class AC_AutoTune WAITING_FOR_LEVEL = 0, // autotune is waiting for vehicle to return to level before beginning the next twitch TESTING = 1, // autotune has begun a test and is watching the resulting vehicle movement UPDATE_GAINS = 2, // autotune has completed a test and is updating the gains based on the results + ABORT = 3 // load normal gains and return to WAITING_FOR_LEVEL }; // mini steps performed while in Tuning mode, Testing step diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 89c1a45b2444d5..434f67671dcec3 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -573,7 +573,6 @@ void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_ // update min and max and test for end conditions void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min) { - const uint32_t now = AP_HAL::millis(); if (angle >= angle_max) { if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) { // we have reached the angle limit before completing the measurement of maximum and minimum @@ -587,10 +586,7 @@ void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angl LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); } // ignore result and start test again - step = WAITING_FOR_LEVEL; - positive_direction = twitch_reverse_direction(); - step_start_time_ms = now; - level_start_time_ms = now; + step = ABORT; } else { step = UPDATE_GAINS; } From e86d5e113f5aa863e4d91f6be6b8d64d49885b6d Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 20 Apr 2024 15:31:49 +0900 Subject: [PATCH 41/77] AP_Filesystem: BOOL for binary types --- libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp index c234779d050165..28a2073492e97e 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp @@ -47,7 +47,7 @@ typedef struct { #define MAX_FILES 16 static FAT_FILE *file_table[MAX_FILES]; -static int isatty_(int fileno) +static bool isatty_(int fileno) { if (fileno >= 0 && fileno <= 2) { return true; From 5b7aaae0080ada57a839cba09f3812a8237edb44 Mon Sep 17 00:00:00 2001 From: yaapu Date: Wed, 7 Dec 2022 14:32:05 +0100 Subject: [PATCH 42/77] AP_Frsky_Telem: fix for HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL = 0 --- .../AP_Frsky_Telem/AP_Frsky_Parameters.cpp | 5 ++-- .../AP_Frsky_Telem/AP_Frsky_Parameters.h | 7 +++--- .../AP_Frsky_SPort_Passthrough.cpp | 23 +++++++++---------- .../AP_Frsky_SPort_Passthrough.h | 3 +-- 4 files changed, 17 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp index f9bf7872e892d6..6d474337ef227e 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp @@ -14,9 +14,9 @@ */ #include "AP_Frsky_Parameters.h" -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = { +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL // @Param: UPLINK_ID // @DisplayName: Uplink sensor id // @Description: Change the uplink sensor id (SPort only) @@ -37,6 +37,7 @@ const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = { // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26 // @User: Advanced AP_GROUPINFO("DNLINK2_ID", 3, AP_Frsky_Parameters, _dnlink2_id, 7), +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL // @Param: DNLINK_ID // @DisplayName: Default downlink sensor id @@ -58,5 +59,3 @@ AP_Frsky_Parameters::AP_Frsky_Parameters() { AP_Param::setup_object_defaults(this, var_info); } - -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h index 3826f340e6191f..f6a06ac962570b 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h @@ -16,7 +16,6 @@ #include "AP_Frsky_Telem.h" -#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL #include #include @@ -33,11 +32,11 @@ class AP_Frsky_Parameters private: // settable parameters - AP_Int8 _uplink_id; AP_Int8 _dnlink_id; +#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL + AP_Int8 _uplink_id; AP_Int8 _dnlink1_id; AP_Int8 _dnlink2_id; +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL AP_Int8 _options; }; - -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index ec883049f141b2..26212761307b40 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -20,8 +20,8 @@ #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL #include "AP_Frsky_MAVlite.h" -#include "AP_Frsky_Parameters.h" #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL +#include "AP_Frsky_Parameters.h" /* for FrSky SPort Passthrough @@ -940,6 +940,16 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue() send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data); } +/* + * Send a mavlite message + * Message is chunked in sport packets pushed in the tx queue + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg) +{ + return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg); +} +#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL /* * Utility method to apply constraints in changing sensor id values * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) @@ -956,17 +966,6 @@ void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &senso sensor = calc_sensor_id(idx); } -/* - * Send a mavlite message - * Message is chunked in sport packets pushed in the tx queue - * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) - */ -bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg) -{ - return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg); -} -#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL - namespace AP { AP_Frsky_SPort_Passthrough *frsky_passthrough_telem() diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index e48541d8cdd8d3..441d8b5dbf4f17 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -149,7 +149,6 @@ class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite; AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport; - void set_sensor_id(AP_Int8 idx, uint8_t &sensor); // tx/rx sport packet processing void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp); void process_rx_queue(void); @@ -160,7 +159,7 @@ class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry bool send_message(const AP_Frsky_MAVlite_Message &txmsg); AP_Frsky_MAVliteMsgHandler mavlite{FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::send_message, bool, const AP_Frsky_MAVlite_Message &)}; #endif - + void set_sensor_id(AP_Int8 idx, uint8_t &sensor); void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); // true if we need to respond to the last polling byte From 1b1ca35927fea5669ce539b4d4ea4870b28ce368 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 25 Jul 2024 13:06:28 +1000 Subject: [PATCH 43/77] build_options.py: add HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index e613550f16e156..aa53a8c10ad7da 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -73,6 +73,7 @@ def __init__(self, Feature('Telemetry', 'FrSky D', 'AP_FRSKY_D_TELEM_ENABLED', 'Enable FrSkyD Telemetry', 0, 'FrSky'), Feature('Telemetry', 'FrSky SPort', 'AP_FRSKY_SPORT_TELEM_ENABLED', 'Enable FrSkySPort Telemetry', 0, 'FrSky'), # noqa Feature('Telemetry', 'FrSky SPort PassThrough', 'AP_FRSKY_SPORT_PASSTHROUGH_ENABLED', 'Enable FrSkySPort PassThrough Telemetry', 0, 'FrSky SPort,FrSky'), # noqa + Feature('Telemetry', 'Bidirectional FrSky Telemetry', 'HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', 'Enable Bidirectional FrSky telemetry', 0, 'FrSky SPort'), # noqa Feature('Telemetry', 'GHST', 'AP_GHST_TELEM_ENABLED', 'Enable Ghost Telemetry', 0, "RC_GHST"), # noqa Feature('Notify', 'PLAY_TUNE', 'AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', 'Enable MAVLink Play Tune', 0, None), # noqa diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 7143758470446b..d1a3d8192dfa5a 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -121,6 +121,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_FRSKY_D_TELEM_ENABLED', 'AP_Frsky_D::send',), ('AP_FRSKY_SPORT_TELEM_ENABLED', 'AP_Frsky_SPort::send_sport_frame',), ('AP_FRSKY_SPORT_PASSTHROUGH_ENABLED', 'AP::frsky_passthrough_telem',), + ('HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', 'AP_Frsky_SPort_Passthrough::set_telem_data'), ('MODE_{type}_ENABLED', r'Mode(?P.+)::init',), ('MODE_GUIDED_NOGPS_ENABLED', r'ModeGuidedNoGPS::init',), From 763292780a26a9980ccb932c954b4f60cd2bc19a Mon Sep 17 00:00:00 2001 From: CB Unmanned <103763456+CBUnmanned@users.noreply.github.com> Date: Sat, 11 Mar 2023 21:40:23 +0000 Subject: [PATCH 44/77] Tools: Reserve ID for CBUnmanned CM405 Flight Controller --- Tools/AP_Bootloader/board_types.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index ed687c3241f95c..fa534addbff4c7 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -290,6 +290,8 @@ AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 +AP_HW_CBUnmanned-CM405-FC 1301 + AP_HW_KHA_ETH 1315 AP_HW_CUBEORANGE_PERIPH 1400 From 605a9d34e3c6b906203d4f67f98c1c72345d4bd1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 20:46:18 +1000 Subject: [PATCH 45/77] autotest: tidy various Rover autotests autotest: tidy Sprayer test autotest: tidy rover AC_Avoidance test autotest: tidy rover ModeSwitch test autotest: tidy rover AuxModeSwitch autotest: tidy rover RCOverrides test autotest: tidy rover MANUAL_CONTROL test autotest: tidy rover PolyFenceObjectAvoidance test --- Tools/autotest/rover.py | 964 ++++++++++++++++++---------------------- 1 file changed, 437 insertions(+), 527 deletions(-) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index b11920ead0361d..9f4c8e08fa90f5 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -227,108 +227,98 @@ def drive_left_circuit(self): def Sprayer(self): """Test sprayer functionality.""" - self.context_push() - ex = None - try: - rc_ch = 5 - pump_ch = 5 - spinner_ch = 6 - pump_ch_min = 1050 - pump_ch_trim = 1520 - pump_ch_max = 1950 - spinner_ch_min = 975 - spinner_ch_trim = 1510 - spinner_ch_max = 1975 + rc_ch = 5 + pump_ch = 5 + spinner_ch = 6 + pump_ch_min = 1050 + pump_ch_trim = 1520 + pump_ch_max = 1950 + spinner_ch_min = 975 + spinner_ch_trim = 1510 + spinner_ch_max = 1975 - self.set_parameters({ - "SPRAY_ENABLE": 1, + self.set_parameters({ + "SPRAY_ENABLE": 1, - "SERVO%u_FUNCTION" % pump_ch: 22, - "SERVO%u_MIN" % pump_ch: pump_ch_min, - "SERVO%u_TRIM" % pump_ch: pump_ch_trim, - "SERVO%u_MAX" % pump_ch: pump_ch_max, + "SERVO%u_FUNCTION" % pump_ch: 22, + "SERVO%u_MIN" % pump_ch: pump_ch_min, + "SERVO%u_TRIM" % pump_ch: pump_ch_trim, + "SERVO%u_MAX" % pump_ch: pump_ch_max, - "SERVO%u_FUNCTION" % spinner_ch: 23, - "SERVO%u_MIN" % spinner_ch: spinner_ch_min, - "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, - "SERVO%u_MAX" % spinner_ch: spinner_ch_max, + "SERVO%u_FUNCTION" % spinner_ch: 23, + "SERVO%u_MIN" % spinner_ch: spinner_ch_min, + "SERVO%u_TRIM" % spinner_ch: spinner_ch_trim, + "SERVO%u_MAX" % spinner_ch: spinner_ch_max, - "SIM_SPR_ENABLE": 1, - "SIM_SPR_PUMP": pump_ch, - "SIM_SPR_SPIN": spinner_ch, + "SIM_SPR_ENABLE": 1, + "SIM_SPR_PUMP": pump_ch, + "SIM_SPR_SPIN": spinner_ch, - "RC%u_OPTION" % rc_ch: 15, - "LOG_DISARMED": 1, - }) + "RC%u_OPTION" % rc_ch: 15, + "LOG_DISARMED": 1, + }) - self.reboot_sitl() + self.reboot_sitl() - self.wait_ready_to_arm() - self.arm_vehicle() + self.wait_ready_to_arm() + self.arm_vehicle() - self.progress("test bootup state - it's zero-output!") - self.wait_servo_channel_value(spinner_ch, 0) - self.wait_servo_channel_value(pump_ch, 0) + self.progress("test bootup state - it's zero-output!") + self.wait_servo_channel_value(spinner_ch, 0) + self.wait_servo_channel_value(pump_ch, 0) - self.progress("Enable sprayer") - self.set_rc(rc_ch, 2000) + self.progress("Enable sprayer") + self.set_rc(rc_ch, 2000) - self.progress("Testing zero-speed state") - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing zero-speed state") + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing turning it off") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing turning it off") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing turning it back on") - self.set_rc(rc_ch, 2000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Testing turning it back on") + self.set_rc(rc_ch, 2000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.progress("Testing speed-ramping") - self.set_rc(3, 1700) # start driving forward + self.progress("Testing speed-ramping") + self.set_rc(3, 1700) # start driving forward - # this is somewhat empirical... - self.wait_servo_channel_value(pump_ch, 1695, timeout=60) + # this is somewhat empirical... + self.wait_servo_channel_value(pump_ch, 1695, timeout=60) - self.progress("Turning it off again") - self.set_rc(rc_ch, 1000) - self.wait_servo_channel_value(spinner_ch, spinner_ch_min) - self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.progress("Turning it off again") + self.set_rc(rc_ch, 1000) + self.wait_servo_channel_value(spinner_ch, spinner_ch_min) + self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.start_subtest("Sprayer Mission") - self.load_mission("sprayer-mission.txt") - self.change_mode("AUTO") + self.start_subtest("Sprayer Mission") + self.load_mission("sprayer-mission.txt") + self.change_mode("AUTO") # self.send_debug_trap() - self.progress("Waiting for sprayer to start") - self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt) - self.progress("Waiting for sprayer to stop") - self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) + self.progress("Waiting for sprayer to start") + self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt) + self.progress("Waiting for sprayer to stop") + self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) - self.start_subtest("Checking mavlink commands") - self.change_mode("MANUAL") - self.progress("Starting Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) - - self.progress("Testing speed-ramping") - self.set_rc(3, 1700) # start driving forward - self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) - self.start_subtest("Stopping Sprayer") - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) - self.wait_servo_channel_value(pump_ch, pump_ch_min) - self.set_rc(3, 1000) # start driving forward - - self.progress("Sprayer OK") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) - self.context_pop() - self.reboot_sitl() - if ex: - raise ex + self.start_subtest("Checking mavlink commands") + self.change_mode("MANUAL") + self.progress("Starting Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=1) + + self.progress("Testing speed-ramping") + self.set_rc(3, 1700) # start driving forward + self.wait_servo_channel_value(pump_ch, 1690, timeout=60, comparator=operator.gt) + self.start_subtest("Stopping Sprayer") + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SPRAYER, p1=0) + self.wait_servo_channel_value(pump_ch, pump_ch_min) + self.set_rc(3, 1000) # stop driving forward + + self.progress("Sprayer OK") + self.disarm_vehicle() def DriveMaxRCIN(self, timeout=30): """Drive rover at max RC inputs""" @@ -538,42 +528,30 @@ def RTL_SPEED(self, timeout=120): def AC_Avoidance(self): '''Test AC Avoidance switch''' - self.context_push() - ex = None - try: - self.load_fence("rover-fence-ac-avoid.txt") - self.set_parameters({ - "FENCE_ENABLE": 0, - "PRX1_TYPE": 10, - "RC10_OPTION": 40, # proximity-enable - }) - self.reboot_sitl() - # start = self.mav.location() - self.wait_ready_to_arm() - self.arm_vehicle() - # first make sure we can breach the fence: - self.set_rc(10, 1000) - self.change_mode("ACRO") - self.set_rc(3, 1550) - self.wait_distance_to_home(25, 100000, timeout=60) - self.change_mode("RTL") - self.wait_statustext("Reached destination", timeout=60) - # now enable avoidance and make sure we can't: - self.set_rc(10, 2000) - self.change_mode("ACRO") - self.wait_groundspeed(0, 0.7, timeout=60) - # watch for speed zero - self.wait_groundspeed(0, 0.2, timeout=120) - - except Exception as e: - self.print_exception_caught(e) - ex = e - self.disarm_vehicle(force=True) - self.context_pop() - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_FENCE) + self.load_fence("rover-fence-ac-avoid.txt") + self.set_parameters({ + "FENCE_ENABLE": 0, + "PRX1_TYPE": 10, + "RC10_OPTION": 40, # proximity-enable + }) self.reboot_sitl() - if ex: - raise ex + # start = self.mav.location() + self.wait_ready_to_arm() + self.arm_vehicle() + # first make sure we can breach the fence: + self.set_rc(10, 1000) + self.change_mode("ACRO") + self.set_rc(3, 1550) + self.wait_distance_to_home(25, 100000, timeout=60) + self.change_mode("RTL") + self.wait_statustext("Reached destination", timeout=60) + # now enable avoidance and make sure we can't: + self.set_rc(10, 2000) + self.change_mode("ACRO") + self.wait_groundspeed(0, 0.7, timeout=60) + # watch for speed zero + self.wait_groundspeed(0, 0.2, timeout=120) + self.disarm_vehicle() def ServoRelayEvents(self): '''Test ServoRelayEvents''' @@ -745,75 +723,53 @@ def MAVProxy_SetModeUsingMode(self): def ModeSwitch(self): ''''Set modes via modeswitch''' - self.context_push() - ex = None - try: - self.set_parameter("MODE_CH", 8) - self.set_rc(8, 1000) - # mavutil.mavlink.ROVER_MODE_HOLD: - self.set_parameter("MODE6", 4) - # mavutil.mavlink.ROVER_MODE_ACRO - self.set_parameter("MODE5", 1) - self.set_rc(8, 1800) # PWM for mode6 - self.wait_mode("HOLD") - self.set_rc(8, 1700) # PWM for mode5 - self.wait_mode("ACRO") - self.set_rc(8, 1800) # PWM for mode6 - self.wait_mode("HOLD") - self.set_rc(8, 1700) # PWM for mode5 - self.wait_mode("ACRO") - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() - - if ex is not None: - raise ex + self.set_parameter("MODE_CH", 8) + self.set_rc(8, 1000) + # mavutil.mavlink.ROVER_MODE_HOLD: + self.set_parameter("MODE6", 4) + # mavutil.mavlink.ROVER_MODE_ACRO + self.set_parameter("MODE5", 1) + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") + self.set_rc(8, 1800) # PWM for mode6 + self.wait_mode("HOLD") + self.set_rc(8, 1700) # PWM for mode5 + self.wait_mode("ACRO") def AuxModeSwitch(self): '''Set modes via auxswitches''' - self.context_push() - ex = None - try: - # from mavproxy_rc.py - mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815] - self.set_parameter("MODE1", 1) # acro - self.set_rc(8, mapping[1]) - self.wait_mode('ACRO') - - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.set_parameters({ - "RC9_OPTION": 53, # steering - "RC10_OPTION": 54, # hold - }) - self.set_rc(9, 1900) - self.wait_mode("STEERING") - self.set_rc(10, 1900) - self.wait_mode("HOLD") - - # reset both switches - should go back to ACRO - self.set_rc(9, 1000) - self.set_rc(10, 1000) - self.wait_mode("ACRO") - - self.set_rc(9, 1900) - self.wait_mode("STEERING") - self.set_rc(10, 1900) - self.wait_mode("HOLD") - - self.set_rc(10, 1000) # this re-polls the mode switch - self.wait_mode("ACRO") - self.set_rc(9, 1000) - except Exception as e: - self.print_exception_caught(e) - ex = e + # from mavproxy_rc.py + mapping = [0, 1165, 1295, 1425, 1555, 1685, 1815] + self.set_parameter("MODE1", 1) # acro + self.set_rc(8, mapping[1]) + self.wait_mode('ACRO') - self.context_pop() + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.set_parameters({ + "RC9_OPTION": 53, # steering + "RC10_OPTION": 54, # hold + }) + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + # reset both switches - should go back to ACRO + self.set_rc(9, 1000) + self.set_rc(10, 1000) + self.wait_mode("ACRO") - if ex is not None: - raise ex + self.set_rc(9, 1900) + self.wait_mode("STEERING") + self.set_rc(10, 1900) + self.wait_mode("HOLD") + + self.set_rc(10, 1000) # this re-polls the mode switch + self.wait_mode("ACRO") + self.set_rc(9, 1000) def RCOverridesCancel(self): '''Test RC overrides Cancel''' @@ -888,337 +844,314 @@ def RCOverridesCancel(self): def RCOverrides(self): '''Test RC overrides''' - self.context_push() self.set_parameter("SYSID_MYGCS", self.mav.source_system) - ex = None - try: - self.set_parameter("RC12_OPTION", 46) - self.reboot_sitl() - - self.change_mode('MANUAL') - self.wait_ready_to_arm() - self.set_rc(3, 1500) # throttle at zero - self.arm_vehicle() - # start moving forward a little: - normal_rc_throttle = 1700 - self.set_rc(3, normal_rc_throttle) - self.wait_groundspeed(5, 100) - - # allow overrides: - self.set_rc(12, 2000) - - # now override to stop: - throttle_override = 1500 + self.set_parameter("RC12_OPTION", 46) + self.reboot_sitl() - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not reach speed") - self.progress("Sending throttle of %u" % (throttle_override,)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - 65535, # chan1_raw - 65535, # chan2_raw - throttle_override, # chan3_raw - 65535, # chan4_raw - 65535, # chan5_raw - 65535, # chan6_raw - 65535, # chan7_raw - 65535) # chan8_raw - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 2.0 - self.progress("Speed=%f want=<%f" % (m.groundspeed, want_speed)) - if m.groundspeed < want_speed: - break + self.change_mode('MANUAL') + self.wait_ready_to_arm() + self.set_rc(3, 1500) # throttle at zero + self.arm_vehicle() + # start moving forward a little: + normal_rc_throttle = 1700 + self.set_rc(3, normal_rc_throttle) + self.wait_groundspeed(5, 100) - # now override to stop - but set the switch on the RC - # transmitter to deny overrides; this should send the - # speed back up to 5 metres/second: - self.set_rc(12, 1000) + # allow overrides: + self.set_rc(12, 2000) - throttle_override = 1500 - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not speed back up") - self.progress("Sending throttle of %u" % (throttle_override,)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - 65535, # chan1_raw - 65535, # chan2_raw - throttle_override, # chan3_raw - 65535, # chan4_raw - 65535, # chan5_raw - 65535, # chan6_raw - 65535, # chan7_raw - 65535) # chan8_raw - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 5.0 - self.progress("Speed=%f want=>%f" % (m.groundspeed, want_speed)) - - if m.groundspeed > want_speed: - break + # now override to stop: + throttle_override = 1500 - # re-enable RC overrides - self.set_rc(12, 2000) - - # check we revert to normal RC inputs when gcs overrides cease: - self.progress("Waiting for RC to revert to normal RC input") - self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) - - self.start_subtest("Check override time of zero disables overrides") - old = self.get_parameter("RC_OVERRIDE_TIME") - ch = 2 - self.set_rc(ch, 1000) - channels = [65535] * 18 - ch_override_value = 1700 - channels[ch-1] = ch_override_value - channels[7] = 1234 # that's channel 8! - self.progress("Sending override message %u" % ch_override_value) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - # long timeout required here as we may have sent a lot of - # things via MAVProxy... - self.wait_rc_channel_value(ch, ch_override_value, timeout=30) - self.set_parameter("RC_OVERRIDE_TIME", 0) - self.wait_rc_channel_value(ch, 1000) - self.set_parameter("RC_OVERRIDE_TIME", old) - self.wait_rc_channel_value(ch, ch_override_value) - - ch_override_value = 1720 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not reach speed") + self.progress("Sending throttle of %u" % (throttle_override,)) self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - self.set_parameter("RC_OVERRIDE_TIME", 0) - self.wait_rc_channel_value(ch, 1000) - self.set_parameter("RC_OVERRIDE_TIME", old) + 65535, # chan1_raw + 65535, # chan2_raw + throttle_override, # chan3_raw + 65535, # chan4_raw + 65535, # chan5_raw + 65535, # chan6_raw + 65535, # chan7_raw + 65535) # chan8_raw - self.progress("Ensuring timeout works") - self.wait_rc_channel_value(ch, 1000, timeout=5) - self.delay_sim_time(10) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 2.0 + self.progress("Speed=%f want=<%f" % (m.groundspeed, want_speed)) + if m.groundspeed < want_speed: + break - self.set_parameter("RC_OVERRIDE_TIME", 10) - self.progress("Sending override message") + # now override to stop - but set the switch on the RC + # transmitter to deny overrides; this should send the + # speed back up to 5 metres/second: + self.set_rc(12, 1000) - ch_override_value = 1730 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - tstart = self.get_sim_time() - self.progress("Waiting for channel to revert to 1000 in ~10s") - self.wait_rc_channel_value(ch, 1000, timeout=15) - delta = self.get_sim_time() - tstart - if delta > 12: - raise NotAchievedException("Took too long to revert RC channel value (delta=%f)" % delta) - min_delta = 9 - if delta < min_delta: - raise NotAchievedException("Didn't take long enough to revert RC channel value (delta=%f want>=%f)" % - (delta, min_delta)) - self.progress("Disabling RC override timeout") - self.set_parameter("RC_OVERRIDE_TIME", -1) - ch_override_value = 1740 - channels[ch-1] = ch_override_value - self.progress("Sending override message %u" % ch_override_value) + throttle_override = 1500 + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not speed back up") + self.progress("Sending throttle of %u" % (throttle_override,)) self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - tstart = self.get_sim_time() - while True: - # warning: this is get_sim_time() and can slurp messages on you! - delta = self.get_sim_time() - tstart - if delta > 20: - break - m = self.assert_receive_message('RC_CHANNELS', timeout=1) - channel_field = "chan%u_raw" % ch - m_value = getattr(m, channel_field) - if m_value != ch_override_value: - raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value)) # noqa - self.set_parameter("RC_OVERRIDE_TIME", old) + 65535, # chan1_raw + 65535, # chan2_raw + throttle_override, # chan3_raw + 65535, # chan4_raw + 65535, # chan5_raw + 65535, # chan6_raw + 65535, # chan7_raw + 65535) # chan8_raw - self.delay_sim_time(10) + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 5.0 + self.progress("Speed=%f want=>%f" % (m.groundspeed, want_speed)) - self.start_subtest("Checking higher-channel semantics") - self.context_push() - self.set_parameter("RC_OVERRIDE_TIME", 30) + if m.groundspeed > want_speed: + break - ch = 11 - rc_value = 1010 - self.set_rc(ch, rc_value) + # re-enable RC overrides + self.set_rc(12, 2000) + + # check we revert to normal RC inputs when gcs overrides cease: + self.progress("Waiting for RC to revert to normal RC input") + self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) + + self.start_subtest("Check override time of zero disables overrides") + old = self.get_parameter("RC_OVERRIDE_TIME") + ch = 2 + self.set_rc(ch, 1000) + channels = [65535] * 18 + ch_override_value = 1700 + channels[ch-1] = ch_override_value + channels[7] = 1234 # that's channel 8! + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + # long timeout required here as we may have sent a lot of + # things via MAVProxy... + self.wait_rc_channel_value(ch, ch_override_value, timeout=30) + self.set_parameter("RC_OVERRIDE_TIME", 0) + self.wait_rc_channel_value(ch, 1000) + self.set_parameter("RC_OVERRIDE_TIME", old) + self.wait_rc_channel_value(ch, ch_override_value) + + ch_override_value = 1720 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + self.set_parameter("RC_OVERRIDE_TIME", 0) + self.wait_rc_channel_value(ch, 1000) + self.set_parameter("RC_OVERRIDE_TIME", old) - channels = [65535] * 18 - ch_override_value = 1234 - channels[ch-1] = ch_override_value - self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.progress("Wait for override value") - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + self.progress("Ensuring timeout works") + self.wait_rc_channel_value(ch, 1000, timeout=5) + self.delay_sim_time(10) - self.progress("Sending return-to-RC-input value") - channels[ch-1] = 65534 - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - self.wait_rc_channel_value(ch, rc_value, timeout=10) + self.set_parameter("RC_OVERRIDE_TIME", 10) + self.progress("Sending override message") + + ch_override_value = 1730 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + tstart = self.get_sim_time() + self.progress("Waiting for channel to revert to 1000 in ~10s") + self.wait_rc_channel_value(ch, 1000, timeout=15) + delta = self.get_sim_time() - tstart + if delta > 12: + raise NotAchievedException("Took too long to revert RC channel value (delta=%f)" % delta) + min_delta = 9 + if delta < min_delta: + raise NotAchievedException("Didn't take long enough to revert RC channel value (delta=%f want>=%f)" % + (delta, min_delta)) + self.progress("Disabling RC override timeout") + self.set_parameter("RC_OVERRIDE_TIME", -1) + ch_override_value = 1740 + channels[ch-1] = ch_override_value + self.progress("Sending override message %u" % ch_override_value) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + tstart = self.get_sim_time() + while True: + # warning: this is get_sim_time() and can slurp messages on you! + delta = self.get_sim_time() - tstart + if delta > 20: + break + m = self.assert_receive_message('RC_CHANNELS', timeout=1) + channel_field = "chan%u_raw" % ch + m_value = getattr(m, channel_field) + if m_value != ch_override_value: + raise NotAchievedException("Value reverted after %f seconds when it should not have (got=%u) (want=%u)" % (delta, m_value, ch_override_value)) # noqa + self.set_parameter("RC_OVERRIDE_TIME", old) + + self.delay_sim_time(10) + + self.start_subtest("Checking higher-channel semantics") + self.context_push() + self.set_parameter("RC_OVERRIDE_TIME", 30) + + ch = 11 + rc_value = 1010 + self.set_rc(ch, rc_value) + + channels = [65535] * 18 + ch_override_value = 1234 + channels[ch-1] = ch_override_value + self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.progress("Wait for override value") + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + + self.progress("Sending return-to-RC-input value") + channels[ch-1] = 65534 + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.wait_rc_channel_value(ch, rc_value, timeout=10) + + channels[ch-1] = ch_override_value + self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + self.mav.mav.rc_channels_override_send( + 1, # target system + 1, # targe component + *channels + ) + self.progress("Wait for override value") + self.wait_rc_channel_value(ch, ch_override_value, timeout=10) - channels[ch-1] = ch_override_value - self.progress("Sending override message ch%u=%u" % (ch, ch_override_value)) + # make we keep the override vaue for at least 10 seconds: + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 10: + break + # try both ignore values: + ignore_value = 0 + if self.get_sim_time_cached() - tstart > 5: + ignore_value = 65535 + self.progress("Sending ignore value %u" % ignore_value) + channels[ch-1] = ignore_value self.mav.mav.rc_channels_override_send( 1, # target system 1, # targe component *channels ) - self.progress("Wait for override value") - self.wait_rc_channel_value(ch, ch_override_value, timeout=10) + if self.get_rc_channel_value(ch) != ch_override_value: + raise NotAchievedException("Did not maintain value") - # make we keep the override vaue for at least 10 seconds: - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 10: - break - # try both ignore values: - ignore_value = 0 - if self.get_sim_time_cached() - tstart > 5: - ignore_value = 65535 - self.progress("Sending ignore value %u" % ignore_value) - channels[ch-1] = ignore_value - self.mav.mav.rc_channels_override_send( - 1, # target system - 1, # targe component - *channels - ) - if self.get_rc_channel_value(ch) != ch_override_value: - raise NotAchievedException("Did not maintain value") - - self.context_pop() - - self.end_subtest("Checking higher-channel semantics") + self.context_pop() - except Exception as e: - self.print_exception_caught(e) - ex = e + self.end_subtest("Checking higher-channel semantics") self.disarm_vehicle() - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex def MANUAL_CONTROL(self): '''Test mavlink MANUAL_CONTROL''' - self.context_push() - self.set_parameter("SYSID_MYGCS", self.mav.source_system) - ex = None - try: - self.set_parameter("RC12_OPTION", 46) # enable/disable rc overrides - self.reboot_sitl() + self.set_parameters({ + "SYSID_MYGCS": self.mav.source_system, + "RC12_OPTION": 46, # enable/disable rc overrides + }) + self.reboot_sitl() - self.change_mode("MANUAL") - self.wait_ready_to_arm() - self.zero_throttle() - self.arm_vehicle() - self.progress("start moving forward a little") - normal_rc_throttle = 1700 - self.set_rc(3, normal_rc_throttle) - self.wait_groundspeed(5, 100) + self.change_mode("MANUAL") + self.wait_ready_to_arm() + self.arm_vehicle() + self.progress("start moving forward a little") + normal_rc_throttle = 1700 + self.set_rc(3, normal_rc_throttle) + self.wait_groundspeed(5, 100) - self.progress("allow overrides") - self.set_rc(12, 2000) + self.progress("allow overrides") + self.set_rc(12, 2000) - self.progress("now override to stop") - throttle_override_normalized = 0 - expected_throttle = 0 # in VFR_HUD + self.progress("now override to stop") + throttle_override_normalized = 0 + expected_throttle = 0 # in VFR_HUD - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not reach speed") - self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,)) - self.mav.mav.manual_control_send( - 1, # target system - 32767, # x (pitch) - 32767, # y (roll) - throttle_override_normalized, # z (thrust) - 32767, # r (yaw) - 0) # button mask - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 2.0 - self.progress("Speed=%f want=<%f throttle=%u want=%u" % - (m.groundspeed, want_speed, m.throttle, expected_throttle)) - if m.groundspeed < want_speed and m.throttle == expected_throttle: - break + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not reach speed") + self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,)) + self.mav.mav.manual_control_send( + 1, # target system + 32767, # x (pitch) + 32767, # y (roll) + throttle_override_normalized, # z (thrust) + 32767, # r (yaw) + 0) # button mask + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 2.0 + self.progress("Speed=%f want=<%f throttle=%u want=%u" % + (m.groundspeed, want_speed, m.throttle, expected_throttle)) + if m.groundspeed < want_speed and m.throttle == expected_throttle: + break - self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second") # noqa - self.set_rc(12, 1000) + self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second") # noqa + self.set_rc(12, 1000) - throttle_override_normalized = 500 - expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max + throttle_override_normalized = 500 + expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max - tstart = self.get_sim_time_cached() - while True: - if self.get_sim_time_cached() - tstart > 10: - raise AutoTestTimeoutException("Did not stop") - self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,)) - self.mav.mav.manual_control_send( - 1, # target system - 32767, # x (pitch) - 32767, # y (roll) - throttle_override_normalized, # z (thrust) - 32767, # r (yaw) - 0) # button mask - - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - want_speed = 5.0 - - self.progress("Speed=%f want=>%f throttle=%u want=%u" % - (m.groundspeed, want_speed, m.throttle, expected_throttle)) - if m.groundspeed > want_speed and m.throttle == expected_throttle: - break + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > 10: + raise AutoTestTimeoutException("Did not stop") + self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,)) + self.mav.mav.manual_control_send( + 1, # target system + 32767, # x (pitch) + 32767, # y (roll) + throttle_override_normalized, # z (thrust) + 32767, # r (yaw) + 0) # button mask + + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + want_speed = 5.0 + + self.progress("Speed=%f want=>%f throttle=%u want=%u" % + (m.groundspeed, want_speed, m.throttle, expected_throttle)) + if m.groundspeed > want_speed and m.throttle == expected_throttle: + break - # re-enable RC overrides - self.set_rc(12, 2000) + # re-enable RC overrides + self.set_rc(12, 2000) - # check we revert to normal RC inputs when gcs overrides cease: - self.progress("Waiting for RC to revert to normal RC input") - self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) - - except Exception as e: - self.print_exception_caught(e) - ex = e + # check we revert to normal RC inputs when gcs overrides cease: + self.progress("Waiting for RC to revert to normal RC input") + self.wait_rc_channel_value(3, normal_rc_throttle, timeout=10) self.disarm_vehicle() - self.context_pop() - self.reboot_sitl() - - if ex is not None: - raise ex def CameraMission(self): '''Test Camera Mission Items''' @@ -4880,35 +4813,24 @@ def test_poly_fence_object_avoidance_auto(self, target_system=1, target_componen self.stop_mavproxy(mavproxy) # self.load_fence("rover-path-planning-fence.txt") self.load_mission("rover-path-planning-mission.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 2, - "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 - }) - self.reboot_sitl() - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameter("FENCE_ENABLE", 1) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - # target_loc is copied from the mission file - target_loc = mavutil.location(40.073799, -105.229156) - self.wait_location(target_loc, height_accuracy=None, timeout=300) - # mission has RTL as last item - self.wait_distance_to_home(3, 7, timeout=300) - self.disarm_vehicle() - except Exception as e: - self.disarm_vehicle(force=True) - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 2, + "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + }) self.reboot_sitl() - if ex is not None: - raise ex + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + if self.mavproxy is not None: + self.mavproxy.send("fence list\n") + # target_loc is copied from the mission file + target_loc = mavutil.location(40.073799, -105.229156) + self.wait_location(target_loc, height_accuracy=None, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(3, 7, timeout=300) + self.disarm_vehicle() def send_guided_mission_item(self, loc, target_system=1, target_component=1): self.mav.mav.mission_item_send( @@ -4930,35 +4852,23 @@ def send_guided_mission_item(self, loc, target_system=1, target_component=1): def test_poly_fence_object_avoidance_guided_pathfinding(self, target_system=1, target_component=1): self.load_fence("rover-path-planning-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 2, - "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameter("FENCE_ENABLE", 1) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.073800, -105.229172) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - self.wait_location(target_loc, timeout=300) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 2, + "FENCE_MARGIN": 0, # FIXME: https://github.com/ArduPilot/ardupilot/issues/11601 + }) self.reboot_sitl() - if ex is not None: - raise ex + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_parameter("FENCE_ENABLE", 1) + target_loc = mavutil.location(40.073800, -105.229172) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + self.wait_location(target_loc, timeout=300) + self.do_RTL(timeout=300) + self.disarm_vehicle() def WheelEncoders(self): '''make sure wheel encoders are generally working''' From f1a1b1183062c6c3e34f5df70fbe81b57dd21398 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 23:56:24 +1000 Subject: [PATCH 46/77] autotest: tidy Helicopter missions to use new infrastructure autotest: tidy PosHoldTakeoff heli test autotest: tidy StabilizeTakeoff heli test autotest: tidy SplineWaypoint heli test autotest: tidy ManAutoRotation heli test autotest: tidy AirspeedDrivers heli test autotest: tidy PIDNotches heli test --- Tools/autotest/helicopter.py | 232 ++++++++++++++--------------------- 1 file changed, 89 insertions(+), 143 deletions(-) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 91c96e03643c18..14fbb1513ddd8d 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -224,101 +224,67 @@ def hover(self): def PosHoldTakeOff(self): """ensure vehicle stays put until it is ready to fly""" - self.context_push() - - ex = None - try: - self.set_parameter("PILOT_TKOFF_ALT", 700) - self.change_mode('POSHOLD') - self.zero_throttle() - self.set_rc(8, 1000) - self.wait_ready_to_arm() - # Arm - self.arm_vehicle() - self.progress("Raising rotor speed") - self.set_rc(8, 2000) - self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) - self.delay_sim_time(20) - # check we are still on the ground... - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - max_relalt_mm = 1000 - if abs(m.relative_alt) > max_relalt_mm: - raise NotAchievedException("Took off prematurely (abs(%f)>%f)" % - (m.relative_alt, max_relalt_mm)) - - self.progress("Pushing collective past half-way") - self.set_rc(3, 1600) - self.delay_sim_time(0.5) - self.hover() - - # make sure we haven't already reached alt: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - max_initial_alt = 1500 - if abs(m.relative_alt) > max_initial_alt: - raise NotAchievedException("Took off too fast (%f > %f" % - (abs(m.relative_alt), max_initial_alt)) - - self.progress("Monitoring takeoff-to-alt") - self.wait_altitude(6.9, 8, relative=True) - - self.progress("Making sure we stop at our takeoff altitude") - tstart = self.get_sim_time() - while self.get_sim_time() - tstart < 5: - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - delta = abs(7000 - m.relative_alt) - self.progress("alt=%f delta=%f" % (m.relative_alt/1000, - delta/1000)) - if delta > 1000: - raise NotAchievedException("Failed to maintain takeoff alt") - self.progress("takeoff OK") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.set_parameter("PILOT_TKOFF_ALT", 700) + self.change_mode('POSHOLD') + self.zero_throttle() + self.set_rc(8, 1000) + self.wait_ready_to_arm() + # Arm + self.arm_vehicle() + self.progress("Raising rotor speed") + self.set_rc(8, 2000) + self.progress("wait for rotor runup to complete") + self.wait_servo_channel_value(8, 1659, timeout=10) + self.delay_sim_time(20) + # check we are still on the ground... + max_relalt = 1 # metres + relative_alt = self.get_altitude(relative=True) + if abs(relative_alt) > max_relalt: + raise NotAchievedException("Took off prematurely (abs(%f)>%f)" % + (relative_alt, max_relalt)) + + self.progress("Pushing collective past half-way") + self.set_rc(3, 1600) + self.delay_sim_time(0.5) + self.hover() - self.land_and_disarm() + # make sure we haven't already reached alt: + relative_alt = self.get_altitude(relative=True) + max_initial_alt = 1.5 # metres + if abs(relative_alt) > max_initial_alt: + raise NotAchievedException("Took off too fast (%f > %f" % + (abs(relative_alt), max_initial_alt)) - self.context_pop() + self.progress("Monitoring takeoff-to-alt") + self.wait_altitude(6, 8, relative=True, minimum_duration=5) + self.progress("takeoff OK") - if ex is not None: - raise ex + self.land_and_disarm() def StabilizeTakeOff(self): """Fly stabilize takeoff""" - self.context_push() - - ex = None - try: - self.change_mode('STABILIZE') - self.set_rc(3, 1000) - self.set_rc(8, 1000) - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_rc(8, 2000) - self.progress("wait for rotor runup to complete") - self.wait_servo_channel_value(8, 1659, timeout=10) - self.delay_sim_time(20) - # check we are still on the ground... - m = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - if abs(m.relative_alt) > 100: - raise NotAchievedException("Took off prematurely") - self.progress("Pushing throttle past half-way") - self.set_rc(3, 1650) - - self.progress("Monitoring takeoff") - self.wait_altitude(6.9, 8, relative=True) - - self.progress("takeoff OK") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.change_mode('STABILIZE') + self.set_rc(3, 1000) + self.set_rc(8, 1000) + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(8, 2000) + self.progress("wait for rotor runup to complete") + self.wait_servo_channel_value(8, 1659, timeout=10) + self.delay_sim_time(20) + # check we are still on the ground... + relative_alt = self.get_altitude(relative=True) + if abs(relative_alt) > 0.1: + raise NotAchievedException("Took off prematurely") + self.progress("Pushing throttle past half-way") + self.set_rc(3, 1650) - self.land_and_disarm() + self.progress("Monitoring takeoff") + self.wait_altitude(6.9, 8, relative=True) - self.context_pop() + self.progress("takeoff OK") - if ex is not None: - raise ex + self.land_and_disarm() def SplineWaypoint(self, timeout=600): """ensure basic spline functionality works""" @@ -331,13 +297,7 @@ def SplineWaypoint(self, timeout=600): self.delay_sim_time(20) self.change_mode("AUTO") self.set_rc(3, 1500) - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > timeout: - raise AutoTestTimeoutException("Vehicle did not disarm after mission") - if not self.armed(): - break - self.delay_sim_time(1) + self.wait_disarmed(timeout=600) self.progress("Lowering rotor speed") self.set_rc(8, 1000) @@ -380,13 +340,15 @@ def ManAutoRotation(self, timeout=600): """Check autorotation power recovery behaviour""" RAMP_TIME = 4 AROT_RAMP_TIME = 2 - self.set_parameter("H_RSC_AROT_MN_EN", 1) - self.set_parameter("H_RSC_AROT_ENG_T", AROT_RAMP_TIME) - self.set_parameter("H_RSC_AROT_IDLE", 20) - self.set_parameter("H_RSC_RAMP_TIME", RAMP_TIME) - self.set_parameter("H_RSC_IDLE", 0) start_alt = 100 # metres - self.set_parameter("PILOT_TKOFF_ALT", start_alt * 100) + self.set_parameters({ + "H_RSC_AROT_MN_EN": 1, + "H_RSC_AROT_ENG_T": AROT_RAMP_TIME, + "H_RSC_AROT_IDLE": 20, + "H_RSC_RAMP_TIME": RAMP_TIME, + "H_RSC_IDLE": 0, + "PILOT_TKOFF_ALT": start_alt * 100, + }) self.change_mode('POSHOLD') self.set_rc(3, 1000) self.set_rc(8, 1000) @@ -680,7 +642,14 @@ def fly_mission(self, filename, strict=True): def AirspeedDrivers(self, timeout=600): '''Test AirSpeed drivers''' + # Copter's airspeed sensors are off by default + self.set_parameters({ + "ARSPD_ENABLE": 1, + "ARSPD_TYPE": 2, # Analog airspeed driver + "ARSPD_PIN": 1, # Analog airspeed driver pin for SITL + }) # set the start location to CMAC to use same test script as other vehicles + self.sitl_start_loc = mavutil.location(-35.362881, 149.165222, 582.000000, 90.0) # CMAC self.customise_SITL_commandline(["--home", "%s,%s,%s,%s" % (-35.362881, 149.165222, 582.000000, 90.0)]) @@ -703,12 +672,6 @@ def check_airspeeds(mav, m): if delta > 3: raise NotAchievedException("Airspeed mismatch (as1=%f as2=%f)" % (airspeed[0], airspeed[1])) - # Copter's airspeed sensors are off by default - self.set_parameter("ARSPD_ENABLE", 1) - self.set_parameter("ARSPD_TYPE", 2) # Analog airspeed driver - self.set_parameter("ARSPD_PIN", 1) # Analog airspeed driver pin for SITL - self.reboot_sitl() - airspeed_sensors = [ ("MS5525", 3, 1), ("DLVR", 7, 2), @@ -734,8 +697,6 @@ def check_airspeeds(mav, m): self.disarm_vehicle() self.context_pop() - self.reboot_sitl() - def TurbineStart(self, timeout=200): """Check Turbine Start Feature""" RAMP_TIME = 4 @@ -806,43 +767,28 @@ def TurbineStart(self, timeout=200): def PIDNotches(self): """Use dynamic harmonic notch to control motor noise.""" self.progress("Flying with PID notches") - self.context_push() - - ex = None - try: - self.set_parameters({ - "FILT1_TYPE": 1, - "FILT2_TYPE": 1, - "AHRS_EKF_TYPE": 10, - "INS_LOG_BAT_MASK": 3, - "INS_LOG_BAT_OPT": 0, - "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour - "LOG_BITMASK": 65535, - "LOG_DISARMED": 0, - "SIM_VIB_FREQ_X": 120, # roll - "SIM_VIB_FREQ_Y": 120, # pitch - "SIM_VIB_FREQ_Z": 180, # yaw - "FILT1_NOTCH_FREQ": 120, - "FILT2_NOTCH_FREQ": 180, - "ATC_RAT_RLL_NEF": 1, - "ATC_RAT_PIT_NEF": 1, - "ATC_RAT_YAW_NEF": 2, - "SIM_GYR1_RND": 5, - }) - self.reboot_sitl() - - self.takeoff(10, mode="ALT_HOLD") - - freq, hover_throttle, peakdb1 = self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True) - - except Exception as e: - self.print_exception_caught(e) - ex = e - - self.context_pop() + self.set_parameters({ + "FILT1_TYPE": 1, + "FILT2_TYPE": 1, + "AHRS_EKF_TYPE": 10, + "INS_LOG_BAT_MASK": 3, + "INS_LOG_BAT_OPT": 0, + "INS_GYRO_FILTER": 100, # set the gyro filter high so we can observe behaviour + "LOG_BITMASK": 65535, + "LOG_DISARMED": 0, + "SIM_VIB_FREQ_X": 120, # roll + "SIM_VIB_FREQ_Y": 120, # pitch + "SIM_VIB_FREQ_Z": 180, # yaw + "FILT1_NOTCH_FREQ": 120, + "FILT2_NOTCH_FREQ": 180, + "ATC_RAT_RLL_NEF": 1, + "ATC_RAT_PIT_NEF": 1, + "ATC_RAT_YAW_NEF": 2, + "SIM_GYR1_RND": 5, + }) + self.reboot_sitl() - if ex is not None: - raise ex + self.hover_and_check_matched_frequency_with_fft(5, 20, 350, reverse=True, takeoff=True) def AutoTune(self): """Test autotune mode""" From 98733882f508b69d0e1af3c28c52148d3f07d154 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 19 Jun 2024 07:52:05 +1000 Subject: [PATCH 47/77] AP_Math: added comments and a test for euler ordering our main euler functions did not have a comment on the ordering convention --- libraries/AP_Math/matrix3.h | 14 ++-- libraries/AP_Math/quaternion.h | 8 +-- libraries/AP_Math/tests/test_rotations.cpp | 78 ++++++++++++++++++++++ 3 files changed, 91 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index ab861e222efe4c..b346198d531c70 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -235,13 +235,17 @@ class Matrix3 { return a.is_nan() || b.is_nan() || c.is_nan(); } - // create a rotation matrix from Euler angles + /* + create a rotation matrix from Euler angles in 321 euler orderin + */ void from_euler(T roll, T pitch, T yaw); - // create eulers from a rotation matrix. - // roll is from -Pi to Pi - // pitch is from -Pi/2 to Pi/2 - // yaw is from -Pi to Pi + /* create eulers from a rotation matrix. + roll is from -Pi to Pi + pitch is from -Pi/2 to Pi/2 + yaw is from -Pi to Pi + euler order is 321 + */ void to_euler(T *roll, T *pitch, T *yaw) const; // create matrix from rotation enum diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 1c47540c678ae8..00f29d08989d2e 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -81,11 +81,11 @@ class QuaternionT { // convert a vector from earth to body frame void earth_to_body(Vector3 &v) const; - // create a quaternion from Euler angles + // create a quaternion from Euler angles using 321 euler ordering void from_euler(T roll, T pitch, T yaw); void from_euler(const Vector3 &v); - // create a quaternion from Euler angles applied in yaw, roll, pitch order + // create a quaternion from Euler angles applied in yaw, roll, pitch order (312) // instead of the normal yaw, pitch, roll order void from_vector312(T roll, T pitch, T yaw); @@ -129,7 +129,7 @@ class QuaternionT { // get euler yaw angle in radians T get_euler_yaw() const; - // create eulers (in radians) from a quaternion + // create eulers (in radians) from a quaternion, using 321 ordering void to_euler(float &roll, float &pitch, float &yaw) const; void to_euler(Vector3f &rpy) const { to_euler(rpy.x, rpy.y, rpy.z); @@ -139,7 +139,7 @@ class QuaternionT { to_euler(rpy.x, rpy.y, rpy.z); } - // create eulers from a quaternion + // create eulers from a quaternion with 312 ordering Vector3 to_vector312(void) const; T length_squared(void) const; diff --git a/libraries/AP_Math/tests/test_rotations.cpp b/libraries/AP_Math/tests/test_rotations.cpp index 15fdfa6238055a..d9de8f82e59dfe 100644 --- a/libraries/AP_Math/tests/test_rotations.cpp +++ b/libraries/AP_Math/tests/test_rotations.cpp @@ -189,5 +189,83 @@ TEST(RotationsTest, TestFailedGetLinux) } }*/ +/* + rotate a matrix using a give order, specified as a string + for example "321" + */ +static void rotate_ordered(Matrix3f &m, const char *order, + const float roll_deg, + const float pitch_deg, + const float yaw_deg) +{ + while (*order) { + Matrix3f m2; + switch (*order) { + case '1': + m2.from_euler(radians(roll_deg), 0, 0); + break; + case '2': + m2.from_euler(0, radians(pitch_deg), 0); + break; + case '3': + m2.from_euler(0, 0, radians(yaw_deg)); + break; + } + m *= m2; + order++; + } +} + +/* + test the two euler orders we use in ArduPilot + */ +TEST(RotationsTest, TestEulerOrder) +{ + const float roll_deg = 20; + const float pitch_deg = 31; + const float yaw_deg = 72; + float r, p, y; + Vector3f v; + + Matrix3f m; + + // apply in 321 ordering + m.identity(); + rotate_ordered(m, "321", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler + m.to_euler(&r, &p, &y); + + EXPECT_FLOAT_EQ(degrees(r), roll_deg); + EXPECT_FLOAT_EQ(degrees(p), pitch_deg); + EXPECT_FLOAT_EQ(degrees(y), yaw_deg); + + // get using to_euler312, should not match + v = m.to_euler312(); + + EXPECT_GE(fabsf(degrees(v.x)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(v.y)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(v.z)-yaw_deg), 1); + + // apply in 312 ordering + m.identity(); + rotate_ordered(m, "312", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler312 + v = m.to_euler312(); + + EXPECT_FLOAT_EQ(degrees(v.x), roll_deg); + EXPECT_FLOAT_EQ(degrees(v.y), pitch_deg); + EXPECT_FLOAT_EQ(degrees(v.z), yaw_deg); + + // get using to_euler, should not match + m.to_euler(&r, &p, &y); + + EXPECT_GE(fabsf(degrees(r)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(p)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1); +} + + AP_GTEST_PANIC() AP_GTEST_MAIN() From 2ce6532698477d7f3e7bc0ae9e18bf9178a94e0e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Jun 2024 19:06:56 +1000 Subject: [PATCH 48/77] AP_Math: updated EulerAngles.pdf link --- libraries/AP_Math/matrix3.cpp | 4 ++-- libraries/AP_Math/matrix3.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index bd64d62a02c8be..0dfd75032b9021 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -21,7 +21,7 @@ #include "AP_Math.h" // create a rotation matrix given some euler angles -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf +// this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf template void Matrix3::from_euler(T roll, T pitch, T yaw) { @@ -44,7 +44,7 @@ void Matrix3::from_euler(T roll, T pitch, T yaw) } // calculate euler angles from a rotation matrix -// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf +// this is based on https://github.com/ArduPilot/Datasheets/blob/main/References/EulerAngles.pdf template void Matrix3::to_euler(T *roll, T *pitch, T *yaw) const { diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index b346198d531c70..02e18699a53506 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -236,7 +236,7 @@ class Matrix3 { } /* - create a rotation matrix from Euler angles in 321 euler orderin + create a rotation matrix from Euler angles in 321 euler ordering */ void from_euler(T roll, T pitch, T yaw); From 317c59c7093161fc0235121aafeade3c4e44df23 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 22:55:25 +1000 Subject: [PATCH 49/77] autotest: tidy various ArduPlane autotests autotest: tidy Plane TerrainRally test autotest: tidy Plane TestFlaps test autotest: tidy Plane ThrottleFailsafe test autotest: tidy Plane GripperMission test autotest: tidy Plane FenceStatic test autotest: tidy Plane FenceRTL tests autotest: tidy FenceRetRally test autotest: tidy Plane ahrs2 test autotest: tidy Plane RangeFinder test autotest: tidy Plane ADSB test autotest: tidy Plane LOITER test autotest: tidy Plane EKFLaneswitch test autotest: tidy Plane FenceAltCeilFloor test autotest: tidy Plane FenceMinAltAutoEnable autotest: tidy Plane FenceMinAltEnableAutoland autotest: tidy Plane FenceMinAltAutoEnableAbort autotest: tidy Plane FenceCircleExclusionAutoEnable autotest: tidy self.homeloc out of MainFlight --- Tools/autotest/arduplane.py | 1074 ++++++++++++++--------------------- 1 file changed, 437 insertions(+), 637 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 6f90634470327e..cf319f97da59a4 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -173,7 +173,7 @@ def fly_left_circuit(self): def fly_RTL(self): """Fly to home.""" self.progress("Flying home in RTL") - target_loc = self.homeloc + target_loc = self.home_position_as_mav_location() target_loc.alt += 100 self.change_mode('RTL') self.wait_location(target_loc, @@ -275,8 +275,10 @@ def wait_level_flight(self, accuracy=5, timeout=30): return raise NotAchievedException("Failed to attain level flight") - def change_altitude(self, altitude, accuracy=30): + def change_altitude(self, altitude, accuracy=30, relative=False): """Get to a given altitude.""" + if relative: + altitude += self.home_position_as_mav_location().alt self.change_mode('FBWA') alt_error = self.mav.messages['VFR_HUD'].alt - altitude if alt_error > 0: @@ -293,7 +295,7 @@ def axial_left_roll(self, count=1): """Fly a left axial roll.""" # full throttle! self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) # fly the roll in manual self.change_mode('MANUAL') @@ -320,7 +322,7 @@ def inside_loop(self, count=1): """Fly a inside loop.""" # full throttle! self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) # fly the loop in manual self.change_mode('MANUAL') @@ -432,7 +434,7 @@ def test_stabilize(self, count=1): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) self.set_rc(2, 1500) self.change_mode('STABILIZE') @@ -458,7 +460,7 @@ def test_acro(self, count=1): # full throttle! self.set_rc(3, 2000) self.set_rc(2, 1300) - self.change_altitude(self.homeloc.alt+300) + self.change_altitude(300, relative=True) self.set_rc(2, 1500) self.change_mode('ACRO') @@ -1031,95 +1033,61 @@ def fly_home_land_and_disarm(self, timeout=120): def TestFlaps(self): """Test flaps functionality.""" filename = "flaps.txt" - self.context_push() - ex = None - try: + flaps_ch = 5 + flaps_ch_min = 1000 + flaps_ch_trim = 1500 + flaps_ch_max = 2000 - flaps_ch = 5 - flaps_ch_min = 1000 - flaps_ch_trim = 1500 - flaps_ch_max = 2000 + servo_ch = 5 + servo_ch_min = 1200 + servo_ch_trim = 1300 + servo_ch_max = 1800 - servo_ch = 5 - servo_ch_min = 1200 - servo_ch_trim = 1300 - servo_ch_max = 1800 + self.set_parameters({ + "SERVO%u_FUNCTION" % servo_ch: 3, # flapsauto + "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION + "LAND_FLAP_PERCNT": 50, + "LOG_DISARMED": 1, + "RTL_AUTOLAND": 1, - self.set_parameters({ - "SERVO%u_FUNCTION" % servo_ch: 3, # flapsauto - "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION - "LAND_FLAP_PERCNT": 50, - "LOG_DISARMED": 1, - "RTL_AUTOLAND": 1, - - "RC%u_MIN" % flaps_ch: flaps_ch_min, - "RC%u_MAX" % flaps_ch: flaps_ch_max, - "RC%u_TRIM" % flaps_ch: flaps_ch_trim, - - "SERVO%u_MIN" % servo_ch: servo_ch_min, - "SERVO%u_MAX" % servo_ch: servo_ch_max, - "SERVO%u_TRIM" % servo_ch: servo_ch_trim, - }) + "RC%u_MIN" % flaps_ch: flaps_ch_min, + "RC%u_MAX" % flaps_ch: flaps_ch_max, + "RC%u_TRIM" % flaps_ch: flaps_ch_trim, - self.progress("check flaps are not deployed") - self.set_rc(flaps_ch, flaps_ch_min) - self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=3) - self.progress("deploy the flaps") - self.set_rc(flaps_ch, flaps_ch_max) - tstart = self.get_sim_time() - self.wait_servo_channel_value(servo_ch, servo_ch_max) - tstop = self.get_sim_time_cached() - delta_time = tstop - tstart - delta_time_min = 0.5 - delta_time_max = 1.5 - if delta_time < delta_time_min or delta_time > delta_time_max: - raise NotAchievedException(( - "Flaps Slew not working (%f seconds)" % (delta_time,))) - self.progress("undeploy flaps") - self.set_rc(flaps_ch, flaps_ch_min) - self.wait_servo_channel_value(servo_ch, servo_ch_min) - - self.progress("Flying mission %s" % filename) - self.load_mission(filename) - self.set_current_waypoint(1) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - last_mission_current_msg = 0 - last_seq = None - while self.armed(): - m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) - time_delta = (self.get_sim_time_cached() - - last_mission_current_msg) - if (time_delta > 1 or m.seq != last_seq): - dist = None - x = self.mav.messages.get("NAV_CONTROLLER_OUTPUT", None) - if x is not None: - dist = x.wp_dist - self.progress("MISSION_CURRENT.seq=%u (dist=%s)" % - (m.seq, str(dist))) - last_mission_current_msg = self.get_sim_time_cached() - last_seq = m.seq - # flaps should undeploy at the end - self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) - - # do a short flight in FBWA, watching for flaps - # self.mavproxy.send('switch 4\n') - # self.wait_mode('FBWA') - # self.delay_sim_time(10) - # self.mavproxy.send('switch 6\n') - # self.wait_mode('MANUAL') - # self.delay_sim_time(10) - - self.progress("Flaps OK") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - if ex: - if self.armed(): - self.disarm_vehicle() - raise ex + "SERVO%u_MIN" % servo_ch: servo_ch_min, + "SERVO%u_MAX" % servo_ch: servo_ch_max, + "SERVO%u_TRIM" % servo_ch: servo_ch_trim, + }) + + self.progress("check flaps are not deployed") + self.set_rc(flaps_ch, flaps_ch_min) + self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=3) + self.progress("deploy the flaps") + self.set_rc(flaps_ch, flaps_ch_max) + tstart = self.get_sim_time() + self.wait_servo_channel_value(servo_ch, servo_ch_max) + tstop = self.get_sim_time_cached() + delta_time = tstop - tstart + delta_time_min = 0.5 + delta_time_max = 1.5 + if delta_time < delta_time_min or delta_time > delta_time_max: + raise NotAchievedException(( + "Flaps Slew not working (%f seconds)" % (delta_time,))) + self.progress("undeploy flaps") + self.set_rc(flaps_ch, flaps_ch_min) + self.wait_servo_channel_value(servo_ch, servo_ch_min) + + self.progress("Flying mission %s" % filename) + self.load_mission(filename) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + # flaps should deploy for landing (RC input value used for position?!) + self.wait_servo_channel_value(servo_ch, flaps_ch_trim, timeout=300) + # flaps should undeploy at the end + self.wait_servo_channel_value(servo_ch, servo_ch_min, timeout=30) + + self.progress("Flaps OK") def TestRCRelay(self): '''Test Relay RC Channel Option''' @@ -1284,32 +1252,25 @@ def ThrottleFailsafe(self): self.progress("Ensure long failsafe can trigger when short failsafe disabled") self.context_push() self.context_collect("STATUSTEXT") - ex = None - try: - self.set_parameters({ - "FS_SHORT_ACTN": 3, # 3 means disabled - "SIM_RC_FAIL": 1, - }) - self.wait_statustext("Long failsafe on", check_context=True) - self.wait_mode("RTL") + self.set_parameters({ + "FS_SHORT_ACTN": 3, # 3 means disabled + "SIM_RC_FAIL": 1, + }) + self.wait_statustext("Long failsafe on", check_context=True) + self.wait_mode("RTL") # self.context_clear_collection("STATUSTEXT") - self.set_parameter("SIM_RC_FAIL", 0) - self.wait_text("Long Failsafe Cleared", check_context=True) - self.change_mode("MANUAL") + self.set_parameter("SIM_RC_FAIL", 0) + self.wait_text("Long Failsafe Cleared", check_context=True) + self.change_mode("MANUAL") - self.progress("Trying again with THR_FS_VALUE") - self.set_parameters({ - "THR_FS_VALUE": 960, - "SIM_RC_FAIL": 2, - }) - self.wait_statustext("Long Failsafe on", check_context=True) - self.wait_mode("RTL") - except Exception as e: - self.print_exception_caught(e) - ex = e + self.progress("Trying again with THR_FS_VALUE") + self.set_parameters({ + "THR_FS_VALUE": 960, + "SIM_RC_FAIL": 2, + }) + self.wait_statustext("Long Failsafe on", check_context=True) + self.wait_mode("RTL") self.context_pop() - if ex is not None: - raise ex self.start_subtest("Not use RC throttle input when THR_FAILSAFE==2") self.takeoff(100) @@ -1432,24 +1393,15 @@ def GCSFailsafe(self): def TestGripperMission(self): '''Test Gripper mission items''' - self.context_push() - ex = None - try: - self.set_parameter("RTL_AUTOLAND", 1) - self.load_mission("plane-gripper-mission.txt") - self.set_current_waypoint(1) - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.wait_statustext("Gripper Grabbed", timeout=60) - self.wait_statustext("Gripper Released", timeout=60) - self.wait_statustext("Auto disarmed", timeout=60) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - if ex is not None: - raise ex + self.set_parameter("RTL_AUTOLAND", 1) + self.load_mission("plane-gripper-mission.txt") + self.set_current_waypoint(1) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_statustext("Gripper Grabbed", timeout=60) + self.wait_statustext("Gripper Released", timeout=60) + self.wait_statustext("Auto disarmed", timeout=60) def assert_fence_sys_status(self, present, enabled, health): self.delay_sim_time(1) @@ -1538,213 +1490,166 @@ def MODE_SWITCH_RESET(self): def FenceStatic(self): '''Test Basic Fence Functionality''' - ex = None - try: - self.progress("Checking for bizarre healthy-when-not-present-or-enabled") - self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present - self.assert_fence_sys_status(False, False, True) - self.load_fence("CMAC-fence.txt") - m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) - if m is not None: - raise NotAchievedException("Got FENCE_STATUS unexpectedly") - self.set_parameter("FENCE_ACTION", 0) # report only - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 1) # RTL - self.assert_fence_sys_status(True, False, True) - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) - m = self.assert_receive_message('FENCE_STATUS', timeout=2) - if m.breach_status: - raise NotAchievedException("Breached fence unexpectedly (%u)" % - (m.breach_status)) - self.do_fence_disable() - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 1) - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_ACTION", 0) - self.assert_fence_sys_status(True, False, True) - self.clear_fence() - if self.get_parameter("FENCE_TOTAL") != 0: - raise NotAchievedException("Expected zero points remaining") - self.assert_fence_sys_status(False, False, True) - self.progress("Trying to enable fence with no points") - self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) - - # test a rather unfortunate behaviour: - self.progress("Killing a live fence with fence-clear") - self.load_fence("CMAC-fence.txt") - self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) - self.clear_fence() - self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True) - if self.get_parameter("FENCE_TOTAL") != 0: - raise NotAchievedException("Expected zero points remaining") - self.assert_fence_sys_status(False, False, True) - self.do_fence_disable() + self.progress("Checking for bizarre healthy-when-not-present-or-enabled") + self.set_parameter("FENCE_TYPE", 4) # Start by only setting polygon fences, otherwise fence will report present + self.assert_fence_sys_status(False, False, True) + self.load_fence("CMAC-fence.txt") + m = self.mav.recv_match(type='FENCE_STATUS', blocking=True, timeout=2) + if m is not None: + raise NotAchievedException("Got FENCE_STATUS unexpectedly") + self.set_parameter("FENCE_ACTION", 0) # report only + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 1) # RTL + self.assert_fence_sys_status(True, False, True) + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + m = self.assert_receive_message('FENCE_STATUS', timeout=2) + if m.breach_status: + raise NotAchievedException("Breached fence unexpectedly (%u)" % + (m.breach_status)) + self.do_fence_disable() + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 1) + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_ACTION", 0) + self.assert_fence_sys_status(True, False, True) + self.clear_fence() + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.progress("Trying to enable fence with no points") + self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + # test a rather unfortunate behaviour: + self.progress("Killing a live fence with fence-clear") + self.load_fence("CMAC-fence.txt") + self.set_parameter("FENCE_ACTION", 1) # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) + self.clear_fence() + self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE, False, False, True) + if self.get_parameter("FENCE_TOTAL") != 0: + raise NotAchievedException("Expected zero points remaining") + self.assert_fence_sys_status(False, False, True) + self.do_fence_disable() - # ensure that a fence is present if it is tin can, min alt or max alt - self.progress("Test other fence types (tin-can, min alt, max alt") - self.set_parameter("FENCE_TYPE", 1) # max alt - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_TYPE", 8) # min alt - self.assert_fence_sys_status(True, False, True) - self.set_parameter("FENCE_TYPE", 2) # tin can - self.assert_fence_sys_status(True, False, True) - - # Test cannot arm if outside of fence and fence is enabled - self.progress("Test Arming while vehicle below FENCE_ALT_MIN") - default_fence_alt_min = self.get_parameter("FENCE_ALT_MIN") - self.set_parameter("FENCE_ALT_MIN", 50) - self.set_parameter("FENCE_TYPE", 8) # Enables minimum altitude breaches - self.do_fence_enable() - self.delay_sim_time(2) # Allow breach to propagate - self.assert_fence_enabled() + # ensure that a fence is present if it is tin can, min alt or max alt + self.progress("Test other fence types (tin-can, min alt, max alt") + self.set_parameter("FENCE_TYPE", 1) # max alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 8) # min alt + self.assert_fence_sys_status(True, False, True) + self.set_parameter("FENCE_TYPE", 2) # tin can + self.assert_fence_sys_status(True, False, True) + + # Test cannot arm if outside of fence and fence is enabled + self.progress("Test Arming while vehicle below FENCE_ALT_MIN") + default_fence_alt_min = self.get_parameter("FENCE_ALT_MIN") + self.set_parameter("FENCE_ALT_MIN", 50) + self.set_parameter("FENCE_TYPE", 8) # Enables minimum altitude breaches + self.do_fence_enable() + self.delay_sim_time(2) # Allow breach to propagate + self.assert_fence_enabled() - self.try_arm(False, "vehicle outside Min Alt fence") - self.do_fence_disable() - self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) - - # Test arming outside inclusion zone - self.progress("Test arming while vehicle outside of inclusion zone") - self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types - self.upload_fences_from_locations([( - mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [ - mavutil.location(1.000, 1.000, 0, 0), - mavutil.location(1.000, 1.001, 0, 0), - mavutil.location(1.001, 1.001, 0, 0), - mavutil.location(1.001, 1.000, 0, 0) - ] - )]) - self.delay_sim_time(10) # let fence check run so it loads-from-eeprom - self.do_fence_enable() - self.assert_fence_enabled() - self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside Polygon fence") - self.do_fence_disable() - self.clear_fence() - - self.progress("Test arming while vehicle inside exclusion zone") - self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types - home_loc = self.mav.location() - locs = [ - mavutil.location(home_loc.lat - 0.001, home_loc.lng - 0.001, 0, 0), - mavutil.location(home_loc.lat - 0.001, home_loc.lng + 0.001, 0, 0), - mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), - mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), - ] - self.upload_fences_from_locations([ - (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs), - ]) - self.delay_sim_time(10) # let fence check run so it loads-from-eeprom - self.do_fence_enable() - self.assert_fence_enabled() - self.delay_sim_time(2) # Allow breach to propagate - self.try_arm(False, "vehicle outside Polygon fence") - self.do_fence_disable() - self.clear_fence() + self.try_arm(False, "vehicle outside Min Alt fence") + self.do_fence_disable() + self.set_parameter("FENCE_ALT_MIN", default_fence_alt_min) - except Exception as e: - self.print_exception_caught(e) - ex = e + # Test arming outside inclusion zone + self.progress("Test arming while vehicle outside of inclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + self.upload_fences_from_locations([( + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, [ + mavutil.location(1.000, 1.000, 0, 0), + mavutil.location(1.000, 1.001, 0, 0), + mavutil.location(1.001, 1.001, 0, 0), + mavutil.location(1.001, 1.000, 0, 0) + ] + )]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.delay_sim_time(2) # Allow breach to propagate + self.try_arm(False, "vehicle outside Polygon fence") + self.do_fence_disable() self.clear_fence() - if ex is not None: - raise ex - def test_fence_breach_circle_at(self, loc, disable_on_breach=False): - ex = None - try: - self.load_fence("CMAC-fence.txt") - want_radius = 100 - # when ArduPlane is fixed, remove this fudge factor - REALLY_BAD_FUDGE_FACTOR = 1.16 - expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius - self.set_parameters({ - "RTL_RADIUS": want_radius, - "NAVL1_LIM_BANK": 60, - "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 - }) - - self.wait_ready_to_arm() # need an origin to load fence - - self.do_fence_enable() - self.assert_fence_sys_status(True, True, True) + self.progress("Test arming while vehicle inside exclusion zone") + self.set_parameter("FENCE_TYPE", 4) # Enables polygon fence types + home_loc = self.mav.location() + locs = [ + mavutil.location(home_loc.lat - 0.001, home_loc.lng - 0.001, 0, 0), + mavutil.location(home_loc.lat - 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng + 0.001, 0, 0), + mavutil.location(home_loc.lat + 0.001, home_loc.lng - 0.001, 0, 0), + ] + self.upload_fences_from_locations([ + (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, locs), + ]) + self.delay_sim_time(10) # let fence check run so it loads-from-eeprom + self.do_fence_enable() + self.assert_fence_enabled() + self.delay_sim_time(2) # Allow breach to propagate + self.try_arm(False, "vehicle outside Polygon fence") + self.do_fence_disable() - self.takeoff(alt=45, alt_max=300) + def test_fence_breach_circle_at(self, loc, disable_on_breach=False): + self.load_fence("CMAC-fence.txt") + want_radius = 100 + # when ArduPlane is fixed, remove this fudge factor + REALLY_BAD_FUDGE_FACTOR = 1.16 + expected_radius = REALLY_BAD_FUDGE_FACTOR * want_radius + self.set_parameters({ + "RTL_RADIUS": want_radius, + "NAVL1_LIM_BANK": 60, + "FENCE_ACTION": 1, # AC_FENCE_ACTION_RTL_AND_LAND == 1. mavutil.mavlink.FENCE_ACTION_RTL == 4 + }) - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > 30: - raise NotAchievedException("Did not breach fence") - m = self.assert_receive_message('FENCE_STATUS', timeout=2) - if m.breach_status == 0: - continue + self.wait_ready_to_arm() # need an origin to load fence - # we've breached; check our state; - if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: - raise NotAchievedException("Unexpected breach type %u" % - (m.breach_type,)) - if m.breach_count == 0: - raise NotAchievedException("Unexpected breach count %u" % - (m.breach_count,)) - self.assert_fence_sys_status(True, True, False) - break + self.do_fence_enable() + self.assert_fence_sys_status(True, True, True) - if disable_on_breach: - self.do_fence_disable() + self.takeoff(alt=45, alt_max=300) - self.wait_circling_point_with_radius(loc, expected_radius) + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > 30: + raise NotAchievedException("Did not breach fence") + m = self.assert_receive_message('FENCE_STATUS', timeout=2) + if m.breach_status == 0: + continue - self.disarm_vehicle(force=True) - self.reboot_sitl() + # we've breached; check our state; + if m.breach_type != mavutil.mavlink.FENCE_BREACH_BOUNDARY: + raise NotAchievedException("Unexpected breach type %u" % + (m.breach_type,)) + if m.breach_count == 0: + raise NotAchievedException("Unexpected breach count %u" % + (m.breach_count,)) + self.assert_fence_sys_status(True, True, False) + break - except Exception as e: - self.print_exception_caught(e) - ex = e - self.clear_fence() - if ex is not None: - raise ex + self.wait_circling_point_with_radius(loc, expected_radius) + self.do_fence_disable() + self.disarm_vehicle(force=True) + self.reboot_sitl() def FenceRTL(self): '''Test Fence RTL''' self.progress("Testing FENCE_ACTION_RTL no rally point") # have to disable the fence once we've breached or we breach # it as part of the loiter-at-home! - self.test_fence_breach_circle_at(self.home_position_as_mav_location(), - disable_on_breach=True) + self.test_fence_breach_circle_at(self.home_position_as_mav_location()) def FenceRTLRally(self): '''Test Fence RTL Rally''' - ex = None - target_system = 1 - target_component = 1 - try: - self.progress("Testing FENCE_ACTION_RTL with rally point") + self.progress("Testing FENCE_ACTION_RTL with rally point") - self.wait_ready_to_arm() - loc = self.home_relative_loc_ne(50, -50) - - self.set_parameter("RALLY_TOTAL", 1) - self.mav.mav.rally_point_send(target_system, - target_component, - 0, # sequence number - 1, # total count - int(loc.lat * 1e7), - int(loc.lng * 1e7), - 15, - 0, # "break" alt?! - 0, # "land dir" - 0) # flags - self.delay_sim_time(1) - if self.mavproxy is not None: - self.mavproxy.send("rally list\n") - self.test_fence_breach_circle_at(loc) - except Exception as e: - self.print_exception_caught(e) - ex = e - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_RALLY) - if ex is not None: - raise ex + self.wait_ready_to_arm() + loc = self.home_relative_loc_ne(50, -50) + self.upload_rally_points_from_locations([loc]) + self.test_fence_breach_circle_at(loc) def FenceRetRally(self): """ Tests the FENCE_RET_RALLY flag, either returning to fence return point, @@ -1754,7 +1659,6 @@ def FenceRetRally(self): self.progress("Testing FENCE_ACTION_RTL with fence rally point") self.wait_ready_to_arm() - self.homeloc = self.mav.location() # Grab a location for fence return point, and upload it. fence_loc = self.home_position_as_mav_location() @@ -1784,18 +1688,7 @@ def FenceRetRally(self): # Grab a location for rally point, and upload it. rally_loc = self.home_relative_loc_ne(-50, 50) - self.set_parameter("RALLY_TOTAL", 1) - self.mav.mav.rally_point_send(target_system, - target_component, - 0, # sequence number - 1, # total count - int(rally_loc.lat * 1e7), - int(rally_loc.lng * 1e7), - 15, - 0, # "break" alt?! - 0, # "land dir" - 0) # flags - self.delay_sim_time(1) + self.upload_rally_points_from_locations([rally_loc]) return_radius = 100 return_alt = 80 @@ -1821,7 +1714,7 @@ def FenceRetRally(self): self.delay_sim_time(15) # Fly up before re-triggering fence breach. Fly to fence return point - self.change_altitude(self.homeloc.alt+30) + self.change_altitude(30, relative=True) self.set_parameters({ "FENCE_RET_RALLY": 0, "FENCE_ALT_MIN": 60, @@ -1867,12 +1760,12 @@ def terrain_wait_path(loc1, loc2, steps): self.progress("Got required terrain") self.wait_ready_to_arm() - self.homeloc = self.mav.location() + homeloc = self.mav.location() - guided_loc = mavutil.location(-35.39723762, 149.07284612, self.homeloc.alt+99.0, 0) - rally_loc = mavutil.location(-35.3654952000, 149.1558698000, self.homeloc.alt+100, 0) + guided_loc = mavutil.location(-35.39723762, 149.07284612, homeloc.alt+99.0, 0) + rally_loc = mavutil.location(-35.3654952000, 149.1558698000, homeloc.alt+100, 0) - terrain_wait_path(self.homeloc, rally_loc, 10) + terrain_wait_path(homeloc, rally_loc, 10) # set a rally point to the west of home self.upload_rally_points_from_locations([rally_loc]) @@ -1999,20 +1892,8 @@ def run_subtest(self, desc, func): def fly_ahrs2_test(self): '''check secondary estimator is looking OK''' - ahrs2 = self.mav.recv_match(type='AHRS2', blocking=True, timeout=1) - if ahrs2 is None: - raise NotAchievedException("Did not receive AHRS2 message") - self.progress("AHRS2: %s" % str(ahrs2)) - - # check location - gpi = self.mav.recv_match( - type='GLOBAL_POSITION_INT', - blocking=True, - timeout=5 - ) - if gpi is None: - raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") - self.progress("GPI: %s" % str(gpi)) + ahrs2 = self.assert_receive_message('AHRS2', verbose=1) + gpi = self.assert_receive_message('GLOBAL_POSITION_INT', verbose=1) if self.get_distance_int(gpi, ahrs2) > 10: raise NotAchievedException("Secondary location looks bad") @@ -2025,10 +1906,6 @@ def MainFlight(self): self.progress("Asserting we do support transfer of fence via mission item protocol") self.assert_capability(mavutil.mavlink.MAV_PROTOCOL_CAPABILITY_MISSION_FENCE) - # grab home position: - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.run_subtest("Takeoff", self.takeoff) self.run_subtest("Set Attitude Target", self.set_attitude_target) @@ -2322,9 +2199,8 @@ def sample_enable_parameter(self): def RangeFinder(self): '''Test RangeFinder Basic Functionality''' - self.context_push() self.progress("Making sure we don't ordinarily get RANGEFINDER") - self.assert_not_receive_message('RANGEFDINDER') + self.assert_not_receive_message('RANGEFINDER') self.set_analog_rangefinder_parameters() @@ -2338,12 +2214,8 @@ def RangeFinder(self): self.wait_ready_to_arm() self.arm_vehicle() self.wait_waypoint(5, 5, max_dist=100) - rf = self.mav.recv_match(type="RANGEFINDER", timeout=1, blocking=True) - if rf is None: - raise NotAchievedException("Did not receive rangefinder message") - gpi = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if gpi is None: - raise NotAchievedException("Did not receive GLOBAL_POSITION_INT message") + rf = self.assert_receive_message('RANGEFINDER') + gpi = self.assert_receive_message('GLOBAL_POSITION_INT') if abs(rf.distance - gpi.relative_alt/1000.0) > 3: raise NotAchievedException( "rangefinder alt (%s) disagrees with global-position-int.relative_alt (%s)" % @@ -2354,9 +2226,6 @@ def RangeFinder(self): if not self.current_onboard_log_contains_message("RFND"): raise NotAchievedException("No RFND messages in log") - self.context_pop() - self.reboot_sitl() - def rc_defaults(self): ret = super(AutoTestPlane, self).rc_defaults() ret[3] = 1000 @@ -2429,12 +2298,7 @@ def SimADSB(self): self.reboot_sitl() self.assert_receive_message('ADSB_VEHICLE', timeout=30) - def ADSB(self): - '''Test ADSB''' - self.ADSB_f_action_rtl() - self.ADSB_r_action_resume_or_loiter() - - def ADSB_r_action_resume_or_loiter(self): + def ADSBResumeActionResumeLoiter(self): '''ensure we resume auto mission or enter loiter''' self.set_parameters({ "ADSB_TYPE": 1, @@ -2472,68 +2336,58 @@ def ADSB_r_action_resume_or_loiter(self): self.fly_home_land_and_disarm() - def ADSB_f_action_rtl(self): - self.context_push() - ex = None - try: - # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17 - self.set_parameter("RC12_OPTION", 38) # avoid-adsb - self.set_rc(12, 2000) - self.set_parameters({ - "ADSB_TYPE": 1, - "AVD_ENABLE": 1, - "AVD_F_ACTION": mavutil.mavlink.MAV_COLLISION_ACTION_RTL, - }) - self.reboot_sitl() - self.wait_ready_to_arm() - here = self.mav.location() - self.change_mode("FBWA") - self.delay_sim_time(2) # TODO: work out why this is required... - self.test_adsb_send_threatening_adsb_message(here) - self.progress("Waiting for collision message") - m = self.assert_receive_message('COLLISION', timeout=4) - if m.threat_level != 2: - raise NotAchievedException("Expected some threat at least") - if m.action != mavutil.mavlink.MAV_COLLISION_ACTION_RTL: - raise NotAchievedException("Incorrect action; want=%u got=%u" % - (mavutil.mavlink.MAV_COLLISION_ACTION_RTL, m.action)) - self.wait_mode("RTL") - - self.progress("Sending far-away ABSD_VEHICLE message") - self.mav.mav.adsb_vehicle_send( - 37, # ICAO address - int(here.lat+1 * 1e7), - int(here.lng * 1e7), - mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH, - int(here.alt*1000 + 10000), # 10m up - 0, # heading in cdeg - 0, # horizontal velocity cm/s - 0, # vertical velocity cm/s - "bob".encode("ascii"), # callsign - mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT, - 1, # time since last communication - 65535, # flags - 17 # squawk - ) - self.wait_for_collision_threat_to_clear() - self.change_mode("FBWA") + def ADSBFailActionRTL(self): + '''test ADSB avoidance action of RTL''' + # message ADSB_VEHICLE 37 -353632614 1491652305 0 584070 0 0 0 "bob" 3 1 255 17 + self.set_parameter("RC12_OPTION", 38) # avoid-adsb + self.set_rc(12, 2000) + self.set_parameters({ + "ADSB_TYPE": 1, + "AVD_ENABLE": 1, + "AVD_F_ACTION": mavutil.mavlink.MAV_COLLISION_ACTION_RTL, + }) + self.reboot_sitl() + self.wait_ready_to_arm() + here = self.mav.location() + self.change_mode("FBWA") + self.delay_sim_time(2) # TODO: work out why this is required... + self.test_adsb_send_threatening_adsb_message(here) + self.progress("Waiting for collision message") + m = self.assert_receive_message('COLLISION', timeout=4) + if m.threat_level != 2: + raise NotAchievedException("Expected some threat at least") + if m.action != mavutil.mavlink.MAV_COLLISION_ACTION_RTL: + raise NotAchievedException("Incorrect action; want=%u got=%u" % + (mavutil.mavlink.MAV_COLLISION_ACTION_RTL, m.action)) + self.wait_mode("RTL") - self.progress("Disabling ADSB-avoidance with RC channel") - self.set_rc(12, 1000) - self.delay_sim_time(1) # let the switch get polled - self.test_adsb_send_threatening_adsb_message(here) - m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4) - self.progress("Got (%s)" % str(m)) - if m is not None: - raise NotAchievedException("Got collision message when I shouldn't have") + self.progress("Sending far-away ABSD_VEHICLE message") + self.mav.mav.adsb_vehicle_send( + 37, # ICAO address + int(here.lat+1 * 1e7), + int(here.lng * 1e7), + mavutil.mavlink.ADSB_ALTITUDE_TYPE_PRESSURE_QNH, + int(here.alt*1000 + 10000), # 10m up + 0, # heading in cdeg + 0, # horizontal velocity cm/s + 0, # vertical velocity cm/s + "bob".encode("ascii"), # callsign + mavutil.mavlink.ADSB_EMITTER_TYPE_LIGHT, + 1, # time since last communication + 65535, # flags + 17 # squawk + ) + self.wait_for_collision_threat_to_clear() + self.change_mode("FBWA") - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.reboot_sitl() - if ex is not None: - raise ex + self.progress("Disabling ADSB-avoidance with RC channel") + self.set_rc(12, 1000) + self.delay_sim_time(1) # let the switch get polled + self.test_adsb_send_threatening_adsb_message(here) + m = self.mav.recv_match(type='COLLISION', blocking=True, timeout=4) + self.progress("Got (%s)" % str(m)) + if m is not None: + raise NotAchievedException("Got collision message when I shouldn't have") def GuidedRequest(self, target_system=1, target_component=1): '''Test handling of MISSION_ITEM in guided mode''' @@ -2634,9 +2488,7 @@ def LOITER(self): now = self.get_sim_time_cached() if now - tstart > 60: break - m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=5) - if m is None: - raise NotAchievedException("Did not get VFR_HUD") + m = self.assert_receive_message('VFR_HUD') new_throttle = m.throttle alt = m.alt m = self.assert_receive_message('ATTITUDE', timeout=5) @@ -3463,9 +3315,6 @@ def IMUTempCal(self): def EKFlaneswitch(self): '''Test EKF3 Affinity and Lane Switching''' - self.context_push() - ex = None - # new lane swtich available only with EK3 self.set_parameters({ "EK3_ENABLE": 1, @@ -3496,165 +3345,149 @@ def statustext_hook(mav, message): return newlane = int(message.text[-1]) self.lane_switches.append(newlane) - self.install_message_hook(statustext_hook) + self.install_message_hook_context(statustext_hook) # get flying self.takeoff(alt=50) self.change_mode('CIRCLE') - try: - ################################################################### - self.progress("Checking EKF3 Lane Switching trigger from all sensors") - ################################################################### - self.start_subtest("ACCELEROMETER: Change z-axis offset") - # create an accelerometer error by changing the Z-axis offset - self.context_collect("STATUSTEXT") - old_parameter = self.get_parameter("INS_ACCOFFS_Z") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5), - check_context=True) - if self.lane_switches != [1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("INS_ACCOFFS_Z", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("BAROMETER: Freeze to last measured value") - self.context_collect("STATUSTEXT") - # create a barometer error by inhibiting any pressure change while changing altitude - old_parameter = self.get_parameter("SIM_BAR2_FREEZE") - self.set_parameter("SIM_BAR2_FREEZE", 1) - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=lambda: self.set_rc(2, 2000), - check_context=True) - if self.lane_switches != [1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_rc(2, 1500) - self.set_parameter("SIM_BAR2_FREEZE", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("GPS: Apply GPS Velocity Error in NED") - self.context_push() - self.context_collect("STATUSTEXT") - - # create a GPS velocity error by adding a random 2m/s - # noise on each axis - def sim_gps_verr(): - self.set_parameters({ - "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2, - "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2, - "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2, - }) - self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True) - if self.lane_switches != [1, 0, 1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.context_pop() - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("MAGNETOMETER: Change X-Axis Offset") - self.context_collect("STATUSTEXT") - # create a magnetometer error by changing the X-axis offset - old_parameter = self.get_parameter("SIM_MAG2_OFS_X") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("SIM_MAG2_OFS_X", old_parameter + 150), - check_context=True) - if self.lane_switches != [1, 0, 1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("SIM_MAG2_OFS_X", old_parameter) - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.start_subtest("AIRSPEED: Fail to constant value") - self.context_push() - self.context_collect("STATUSTEXT") - - old_parameter = self.get_parameter("SIM_ARSPD_FAIL") - - def fail_speed(): - self.change_mode("GUIDED") - loc = self.mav.location() - self.run_cmd_int( - mavutil.mavlink.MAV_CMD_DO_REPOSITION, - p5=int(loc.lat * 1e7), - p6=int(loc.lng * 1e7), - p7=50, # alt - ) - self.delay_sim_time(5) - # create an airspeed sensor error by freezing to the - # current airspeed then changing the airspeed demand - # to a higher value and waiting for the TECS speed - # loop to diverge - m = self.mav.recv_match(type='VFR_HUD', blocking=True) - self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) - self.run_cmd( - mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, - p1=0, # airspeed - p2=30, - p3=-1, # throttle / no change - p4=0, # absolute values - ) - self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True) - if self.lane_switches != [1, 0, 1, 0, 1]: - raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("SIM_ARSPD_FAIL", old_parameter) - self.change_mode('CIRCLE') - self.context_pop() - self.context_clear_collection("STATUSTEXT") - self.wait_heading(0, accuracy=10, timeout=60) - self.wait_heading(180, accuracy=10, timeout=60) - ################################################################### - self.progress("GYROSCOPE: Change Y-Axis Offset") - self.context_collect("STATUSTEXT") - # create a gyroscope error by changing the Y-axis offset - old_parameter = self.get_parameter("INS_GYR2OFFS_Y") - self.wait_statustext( - text="EKF3 lane switch", - timeout=30, - the_function=self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1), - check_context=True) - if self.lane_switches != [1, 0, 1, 0, 1, 0]: - raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) - # Cleanup - self.set_parameter("INS_GYR2OFFS_Y", old_parameter) - self.context_clear_collection("STATUSTEXT") - ################################################################### - - self.disarm_vehicle(force=True) + ################################################################### + self.progress("Checking EKF3 Lane Switching trigger from all sensors") + ################################################################### + self.start_subtest("ACCELEROMETER: Change z-axis offset") + # create an accelerometer error by changing the Z-axis offset + self.context_collect("STATUSTEXT") + old_parameter = self.get_parameter("INS_ACCOFFS_Z") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("INS_ACCOFFS_Z", old_parameter + 5), + check_context=True) + if self.lane_switches != [1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("INS_ACCOFFS_Z", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("BAROMETER: Freeze to last measured value") + self.context_collect("STATUSTEXT") + # create a barometer error by inhibiting any pressure change while changing altitude + old_parameter = self.get_parameter("SIM_BAR2_FREEZE") + self.set_parameter("SIM_BAR2_FREEZE", 1) + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=lambda: self.set_rc(2, 2000), + check_context=True) + if self.lane_switches != [1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_rc(2, 1500) + self.set_parameter("SIM_BAR2_FREEZE", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("GPS: Apply GPS Velocity Error in NED") + self.context_push() + self.context_collect("STATUSTEXT") - except Exception as e: - self.print_exception_caught(e) - ex = e + # create a GPS velocity error by adding a random 2m/s + # noise on each axis + def sim_gps_verr(): + self.set_parameters({ + "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2, + "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2, + "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2, + }) + self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True) + if self.lane_switches != [1, 0, 1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.context_pop() + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("MAGNETOMETER: Change X-Axis Offset") + self.context_collect("STATUSTEXT") + # create a magnetometer error by changing the X-axis offset + old_parameter = self.get_parameter("SIM_MAG2_OFS_X") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("SIM_MAG2_OFS_X", old_parameter + 150), + check_context=True) + if self.lane_switches != [1, 0, 1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("SIM_MAG2_OFS_X", old_parameter) + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.start_subtest("AIRSPEED: Fail to constant value") + self.context_push() + self.context_collect("STATUSTEXT") - self.remove_message_hook(statustext_hook) + old_parameter = self.get_parameter("SIM_ARSPD_FAIL") + def fail_speed(): + self.change_mode("GUIDED") + loc = self.mav.location() + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + p5=int(loc.lat * 1e7), + p6=int(loc.lng * 1e7), + p7=50, # alt + ) + self.delay_sim_time(5) + # create an airspeed sensor error by freezing to the + # current airspeed then changing the airspeed demand + # to a higher value and waiting for the TECS speed + # loop to diverge + m = self.mav.recv_match(type='VFR_HUD', blocking=True) + self.set_parameter("SIM_ARSPD_FAIL", m.airspeed) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, + p1=0, # airspeed + p2=30, + p3=-1, # throttle / no change + p4=0, # absolute values + ) + self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=fail_speed, check_context=True) + if self.lane_switches != [1, 0, 1, 0, 1]: + raise NotAchievedException("Expected lane switch 1, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("SIM_ARSPD_FAIL", old_parameter) + self.change_mode('CIRCLE') self.context_pop() + self.context_clear_collection("STATUSTEXT") + self.wait_heading(0, accuracy=10, timeout=60) + self.wait_heading(180, accuracy=10, timeout=60) + ################################################################### + self.progress("GYROSCOPE: Change Y-Axis Offset") + self.context_collect("STATUSTEXT") + # create a gyroscope error by changing the Y-axis offset + old_parameter = self.get_parameter("INS_GYR2OFFS_Y") + self.wait_statustext( + text="EKF3 lane switch", + timeout=30, + the_function=self.set_parameter("INS_GYR2OFFS_Y", old_parameter + 1), + check_context=True) + if self.lane_switches != [1, 0, 1, 0, 1, 0]: + raise NotAchievedException("Expected lane switch 0, got %s" % str(self.lane_switches[-1])) + # Cleanup + self.set_parameter("INS_GYR2OFFS_Y", old_parameter) + self.context_clear_collection("STATUSTEXT") + ################################################################### - # some parameters need reboot to take effect - self.reboot_sitl() - - if ex is not None: - raise ex + self.disarm_vehicle(force=True) def FenceAltCeilFloor(self): '''Tests the fence ceiling and floor''' - fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE self.set_parameters({ "FENCE_TYPE": 9, # Set fence type to max and min alt "FENCE_ACTION": 0, # Set action to report @@ -3663,36 +3496,32 @@ def FenceAltCeilFloor(self): }) # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() + self.wait_ready_to_arm() + startpos = self.mav.location() cruise_alt = 150 self.takeoff(cruise_alt) + # note that while we enable the fence here, since the action + # is set to report-only the fence continues to show as + # not-enabled in the assert calls below self.do_fence_enable() self.progress("Fly above ceiling and check for breach") - self.change_altitude(self.homeloc.alt + cruise_alt + 80) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if ((m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling did not breach") + self.change_altitude(startpos.alt + cruise_alt + 80) + self.assert_fence_sys_status(True, False, False) - self.progress("Return to cruise alt and check for breach clear") - self.change_altitude(self.homeloc.alt + cruise_alt) + self.progress("Return to cruise alt") + self.change_altitude(startpos.alt + cruise_alt) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence breach did not clear") + self.progress("Ensure breach has clearned") + self.assert_fence_sys_status(True, False, True) self.progress("Fly below floor and check for breach") - self.change_altitude(self.homeloc.alt + cruise_alt - 80) + self.change_altitude(startpos.alt + cruise_alt - 80) - m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if ((m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Floor did not breach") + self.progress("Ensure breach has clearned") + self.assert_fence_sys_status(True, False, False) self.do_fence_disable() @@ -3713,9 +3542,6 @@ def FenceMinAltAutoEnable(self): # check we can takeoff again for i in [1, 2]: # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() # max alt fence should now be enabled @@ -3754,9 +3580,6 @@ def FenceMinAltEnableAutoland(self): }) # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() @@ -3781,9 +3604,6 @@ def FenceMinAltEnableAutoland(self): self.set_rc(3, 1000) # lower throttle # Now check we can land self.fly_home_land_and_disarm() - self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) - self.set_current_waypoint(0, check_afterwards=False) - self.set_rc(3, 1000) # lower throttle def FenceMinAltAutoEnableAbort(self): '''Tests autoenablement of the alt min fence and fences on arming''' @@ -3797,10 +3617,6 @@ def FenceMinAltAutoEnableAbort(self): "RTL_AUTOLAND" : 2, }) - # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - self.wait_ready_to_arm() self.arm_vehicle() @@ -3837,7 +3653,6 @@ def FenceMinAltAutoEnableAbort(self): self.change_mode("AUTO") self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_ALL) self.fly_home_land_and_disarm(timeout=150) - self.wait_disarmed() def FenceAutoEnableDisableSwitch(self): '''Tests autoenablement of regular fences and manual disablement''' @@ -3859,7 +3674,6 @@ def FenceAutoEnableDisableSwitch(self): fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE # Grab Home Position self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() self.set_rc_from_map({7: 1000}) # Turn fence off with aux function self.wait_ready_to_arm() @@ -3868,7 +3682,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Fly above ceiling and check there is no breach") self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt + cruise_alt + 80) + self.change_altitude(cruise_alt + 80, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_health & fence_bit)): @@ -3876,10 +3690,10 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Return to cruise alt") self.set_rc(3, 1500) - self.change_altitude(self.homeloc.alt + cruise_alt) + self.change_altitude(cruise_alt, relative=True) self.progress("Fly below floor and check for no breach") - self.change_altitude(self.homeloc.alt + 25) + self.change_altitude(25, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_health & fence_bit)): @@ -3887,7 +3701,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Fly above floor and check fence is not re-enabled") self.set_rc(3, 2000) - self.change_altitude(self.homeloc.alt + 75) + self.change_altitude(75, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) self.progress("Got (%s)" % str(m)) if (m.onboard_control_sensors_enabled & fence_bit): @@ -3895,7 +3709,7 @@ def FenceAutoEnableDisableSwitch(self): self.progress("Return to cruise alt") self.set_rc(3, 1500) - self.change_altitude(self.homeloc.alt + cruise_alt) + self.change_altitude(cruise_alt, relative=True) self.fly_home_land_and_disarm(timeout=250) def FenceCircleExclusionAutoEnable(self): @@ -3908,10 +3722,6 @@ def FenceCircleExclusionAutoEnable(self): "RTL_AUTOLAND" : 2, }) - # Grab Home Position - self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.homeloc = self.mav.location() - fence_loc = self.home_position_as_mav_location() self.location_offset_ne(fence_loc, 300, 0) @@ -3922,23 +3732,12 @@ def FenceCircleExclusionAutoEnable(self): } )]) - self.wait_ready_to_arm() - self.arm_vehicle() - self.takeoff(alt=50, mode='TAKEOFF') self.change_mode("FBWA") self.set_rc(3, 1100) # lower throttle self.progress("Waiting for RTL") - tstart = self.get_sim_time() - mode = "RTL" - while not self.mode_is(mode, drain_mav=False): - self.mav.messages['HEARTBEAT'].custom_mode - self.progress("mav.flightmode=%s Want=%s Alt=%f" % ( - self.mav.flightmode, mode, self.get_altitude(relative=True))) - if (self.get_sim_time_cached() > tstart + 120): - raise WaitModeTimeout("Did not change mode") - self.progress("Got mode %s" % mode) + self.wait_mode('RTL') # Now check we can land self.fly_home_land_and_disarm() @@ -5873,7 +5672,8 @@ def tests(self): self.FenceNoFenceReturnPoint, self.FenceNoFenceReturnPointInclusion, self.FenceDisableUnderAction, - self.ADSB, + self.ADSBFailActionRTL, + self.ADSBResumeActionResumeLoiter, self.SimADSB, self.Button, self.FRSkySPort, From 84efdd570085eef3768a9dbf2374c2bd33531cca Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 19:35:09 +1000 Subject: [PATCH 50/77] hwdef: SpeedyBeeF405WING: remove landing gear support --- libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat | 2 -- 1 file changed, 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index a3aff9a67c3778..fb22e7e4dcdba6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -172,13 +172,11 @@ PC13 PINIO1 OUTPUT GPIO(81) LOW include ../include/minimize_fpv_osd.inc undef AP_CAMERA_MOUNT_ENABLED -undef AP_LANDINGGEAR_ENABLED undef HAL_MOUNT_ENABLED undef HAL_MOUNT_SERVO_ENABLED undef QAUTOTUNE_ENABLED define AP_CAMERA_MOUNT_ENABLED 1 -define AP_LANDINGGEAR_ENABLED 1 define HAL_MOUNT_ENABLED 1 define HAL_MOUNT_SERVO_ENABLED 1 define QAUTOTUNE_ENABLED 1 From debf3cb28e7443f75771005245637070232e04d5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 19:42:17 +1000 Subject: [PATCH 51/77] hwdef: SpeedyBeeF405WING: restrict mount support to the servo gimbal --- libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index fb22e7e4dcdba6..f063295ea6d73d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -178,6 +178,7 @@ undef QAUTOTUNE_ENABLED define AP_CAMERA_MOUNT_ENABLED 1 define HAL_MOUNT_ENABLED 1 +define AP_MOUNT_BACKEND_DEFAULT_ENABLED 0 define HAL_MOUNT_SERVO_ENABLED 1 define QAUTOTUNE_ENABLED 1 From 16c2e23ee7128d6e301123ca69c2ae0736432d53 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 19:43:32 +1000 Subject: [PATCH 52/77] AP_Mount: allow AP_MOUNT_BACKEND_DEFAULT_ENABLED to be overridden --- libraries/AP_Mount/AP_Mount_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 6c0f63509e17db..a2da37fac58805 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -7,7 +7,9 @@ #define HAL_MOUNT_ENABLED 1 #endif +#ifndef AP_MOUNT_BACKEND_DEFAULT_ENABLED #define AP_MOUNT_BACKEND_DEFAULT_ENABLED HAL_MOUNT_ENABLED +#endif #ifndef HAL_MOUNT_ALEXMOS_ENABLED #define HAL_MOUNT_ALEXMOS_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED From 3ddd477d04669e97699453d4aba86f5ca184ed72 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 28 Jul 2024 06:06:07 +1000 Subject: [PATCH 53/77] CI: only run qurt build on ArduPilot main repo this prevents user repos getting failures thanks to Pete for the suggestion --- .github/workflows/qurt_build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/qurt_build.yml b/.github/workflows/qurt_build.yml index 5d303fa65aedeb..debf75c7f3db1e 100644 --- a/.github/workflows/qurt_build.yml +++ b/.github/workflows/qurt_build.yml @@ -141,8 +141,8 @@ concurrency: jobs: build: + if: github.repository == 'ArduPilot/ardupilot' runs-on: 'ardupilot-qurt' - steps: - uses: actions/checkout@v4 with: From bbde876353914b910e743dca3ca2f3aff27f9ce3 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 27 Jul 2024 17:36:11 -0500 Subject: [PATCH 54/77] hwdef: remove EKF opinion from FlywooF745 No effect as lines have always been commented out, but not useful and EKF2 is not something to encourage. Verified no compiler output change. --- libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat index 1543f2b3379a81..0624b4e0680e4f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat @@ -162,10 +162,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin # define HAL_GYROFFT_ENABLED 1 # define AP_OAPATHPLANNER_ENABLED 0 -# EK2 options (disabled by default) -# define HAL_NAVEKF2_AVAILABLE 1 -# define HAL_NAVEKF3_AVAILABLE 0 - # save some flash include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc From 317fab2f1933bbabc43e864c170fe97dbb818ab5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 26 Jul 2024 10:58:16 +1000 Subject: [PATCH 55/77] AP_Mission: add and use an option_is_set method --- libraries/AP_Mission/AP_Mission.cpp | 4 ++-- libraries/AP_Mission/AP_Mission.h | 18 ++++++++++++------ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 2e5fb77d996332..ea441be219ba89 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -95,7 +95,7 @@ void AP_Mission::init() init_jump_tracking(); // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission. - if (AP_MISSION_MASK_MISSION_CLEAR & _options) { + if (option_is_set(Option::CLEAR_ON_BOOT)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Clearing Mission"); clear(); } @@ -2502,7 +2502,7 @@ bool AP_Mission::is_best_land_sequence(const Location ¤t_loc) } // check if MIS_OPTIONS bit set to allow distance calculation to be done - if (!(_options & AP_MISSION_MASK_DIST_TO_LAND_CALC)) { + if (!option_is_set(Option::FAILSAFE_TO_BEST_LANDING)) { return false; } diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 69f8fe2b46b979..32bb2fa77dd8de 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -45,9 +45,6 @@ #define AP_MISSION_RESTART_DEFAULT 0 // resume the mission from the last command run by default #define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting -#define AP_MISSION_MASK_MISSION_CLEAR (1<<0) // If set then Clear the mission on boot -#define AP_MISSION_MASK_DIST_TO_LAND_CALC (1<<1) // Allow distance to best landing calculation to be run on failsafe -#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2) // Allow mission to continue after land #define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history #define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2) @@ -732,14 +729,23 @@ class AP_Mission void reset_wp_history(void); /* - return true if MIS_OPTIONS is set to allow continue of mission - logic after a land and the next waypoint is a takeoff. If this + Option::FailsafeToBestLanding - continue mission + logic after a land if the next waypoint is a takeoff. If this is false then after a landing is complete the vehicle should disarm and mission logic should stop */ + enum class Option { + CLEAR_ON_BOOT = 0, // clear mission on vehicle boot + FAILSAFE_TO_BEST_LANDING = 1, // on failsafe, find fastest path along mission home + CONTINUE_AFTER_LAND = 2, // continue running mission (do not disarm) after land if takeoff is next waypoint + }; + bool option_is_set(Option option) const { + return (_options.get() & (uint16_t)option) != 0; + } + bool continue_after_land_check_for_takeoff(void); bool continue_after_land(void) const { - return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0; + return option_is_set(Option::CONTINUE_AFTER_LAND); } // user settable parameters From 08c8ef379beefbe24fb8a07d8851fac09d0dfe56 Mon Sep 17 00:00:00 2001 From: cuav-chen2 Date: Wed, 3 Jul 2024 10:12:28 +0800 Subject: [PATCH 56/77] HAL_ChibiOS: Added support for CUAV-7-Nano flight controller --- .../hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png | Bin 0 -> 46169 bytes .../hwdef/CUAV-7-Nano/README.md | 80 +++++ .../hwdef/CUAV-7-Nano/defaults.parm | 2 + .../hwdef/CUAV-7-Nano/hwdef-bl.dat | 101 ++++++ .../hwdef/CUAV-7-Nano/hwdef.dat | 294 ++++++++++++++++++ 5 files changed, 477 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/CUAV-7-Nano-pinout.png new file mode 100644 index 0000000000000000000000000000000000000000..fe2ea5b4827bc6be0486a5d952d32e1171393b47 GIT binary patch literal 46169 zcmbTdbx<5n6fTOpy9Wpwf(H-oPH=)I1b0|$ad%6Q;JXmqoh%k0xa|VLC4mqI2?SUm zS?uw<@7{WKZ`G@M_2!S6>Hg07PWRW-BWGrk4D{5A2^a{_(9noAHB=1I&@cfsH0%vL z%*UE|HDj~KjlY48vFgLagQ%!zL`1~&^mKlHzJh{+qod=%z`!GZetxd2tNZKMFAEEc z)z#J3)>a`Qp|rHLii(QU(^Ee`KRY|S($doH?QIeglDWA#Z*Ol_R#pQ8gN%%frlzLo z=;-U~>%_!F1_lN$F0SR}p z{I~o6Z?U^NSiy=U!;6G4g=c*wVo+eE$E=_@&12=Mv#YQ+U>zvmSSp z(?y2FY#a5Z`kR)(M(y+k1-iS}t`ik*tKA=WaH)*ZPal%&1#(}$3gG$>^?l}Vq(s)H z3*Q!#wL5pnIg)21&PS3Z(8Lv{4SJd{#d5}4RAfbam*STJ_*O)xNlN`A?>0huf7K64 z%peb|Oeg(ohBy84$Io}WbY&<|4II zn?pm9W8k|MsrIqsQcTs05T3Y^sQl`jB`GPtYeQSh@BG`}cNr(|M(Hes@mk}tj`1dC znwUHX>sWc-CVap_vosu96w9(;7j|QQvRdAREI8BE?D0s}a9*qKAWbHKWzU$J&bT3r zj?FaPom~OwczXg{(l~ooX-QdxER`I#k(ySmhO?5I8x>0ZMy-mSlU-UzgkD^~?R+(L zC#v{8Q1Um*4;d$bP`;Eo8`AWG?SJ0zZKdMv*WCPQAXLM8LeclrvUAxY=0nqHahc3# zQL>OWTtv_QQ~-S!mu*cYztodtMW#B(KW@j> zc?w_D*VgcnH+-;icf(Jv@4W1YN?c^G`dJnBEu8M1>%0VNyH`Op!8jbUjB1b`7gjE8|CcK0v=&mm!8u| z4rCc#dHd|LRj3E;o63H}Kl5=to}jYa&jXG;0q->DaoPQ9ibp2~B5zs*8s$`wNzZH$ z$P(aPk}>0Zo0C!U;mj4Sd0BI$n90;ZWDXV)2d97~MvH8q?oi(Vzf!#CoACwm1fs4g z-D8DMj_e^-z#m?RhC%WqJ_@x22qk$wgqeapZy7r$KP!_Vk(bGAxQg$`n+h>2d?&Yk zz8^vtsqtF^?K%N6fs+gnCVfdEg1~cfgjBwp|54T!GhVfpfp z4hVIYbBI^%x1#_wH3{v?P|FJ6wlPkI^OLE)WF1}7*zqggw0Igrf<4xxG|Ji}y&ngv z`hZzk26xR13)R}waQk?EE>;H6I!ZUS%_PS5mF9BJI!R9GD~0A@`|a; zf9H-%FUO#iWv>Dl<+~0;a6VB0)ncd><8Nx_VRiNJo#obzw-nc9NdBo1TRI}J&ogxBX_sg*kMY-yZwT<2>0TAUYbS%4*-YnncV;>g;iekc z2MwjT{m3iBeP0=#S_pigo)bWYC|?Vp4!xvy84a7~9v0du`@k&cw)(f*DrPDRx@Dpr zx&_ikh^3R$PVx?FtBVC{eC}%2HLAjt`p_!CJdq^SC9T6~p+2jQsiu%nUVqX#p5usD ze3xtQr8Y+Px5JfDI0Yq!NK$JX!eo)tAUU-; znaBC2a1)Zo#1a~;%e~-8VIEM%2wJo(yKZr^D>dsD@ucN)tnrXn;Uf4(5?yEgnYA?r z@FLxmxgXD4GPi&AGr90Jq>fHQ)p^tv-E{nCYpaWBnIc3oc1|2$GJaUIQYbc$uZ#9a zIZ)7t8_!}LofD#lh$2n&y#4;qqjJow2Y)jD8F$i-)Iaq(MBclCH<+8M*R=dmYbW%* zWY%rNB>PhuYk5RPBG&B;Q@_r1iVqq;_&AV`-92yB3#XQ_ZtD8fsd?(O{CO(Z#VPHElgPINW>^F1i@XF zPx;xMmfozjGV#9K($2as%fx^-Y%s3SM*{kU<2Y}VXpdO+n!iF}=o^)vF0{H$t^rW+ zQCrz8bdJY&yOKq9t^%32Plom+oPF|seDKAiO85i8Vvr4`Vy3KMZuP6%OgUNfAw3xo zR-YU1Qk96e{*vI$rWVr&yfRy(Rpl=nn?+b|*SaK#21#dmZ)y-h9il8t&H zG;cSo@h%#3nu-}d>EW07hNx+xF@B2ghlptWazWb1@{*-qedOFO=E-R2pAk|rW<>-a zyTG69=3I(5+3=rOwZ*CSTEE-|rpB`WtWR{`rHSxm^*sacCs5tJ`;J$}80f|-mn5~J zuZgYCOg-jWznED5;d5zYb$!$61So7qhY?0sYqRMdksN*wk%9%-^}lH;6$>lwa`z5S z$_&m*3_d8tmRAAMnHpPcGRlip#16B9H4TiL)(2e)CFFuPj8@D~I}2^C2SOYXQacRq znH#_NQ?k%65AXG?ZqG$O`Lm31`WsOZ)$8En92%f66pZg3?1SgB>J`3{pO+`C_}J#{)$bA8LL+3F5mGs& zKT-Z{3TZSei~e`7t5ln}fuD3c2;HV~NRX%ZXU>jOsD~axLJyJg94vn6bcTVZ#BoX) zQQ2sdI1t$`JUF1wZZs3HZK{tJeslPcdBKYs&=h<0 zmj@bXrVX|N5Bf-1c9q$0_}f2%lm z`%D4v6~sd&kNX8?*_dD6sG3T~c5uZ0*#F~z(S$aJ;iji9X&=)aX1-PCsgmI4eoVdj z7F*k%uWW#SrQuUFz4CnV3*VP<_Mz0+GltSORGQpw$$qXtd z+A}caByVyY)|nHh=QUi;CdF<6CY(K-=VaucT@i{Q zxoth1eG?{0WnECnxxu_F{wp+-;glz)eL zIc68niH%U`)m+VeI?%-fM6T-QbQ1Y6-h4D zo?IHt(sD#?ofodxmD(0OJl$RvbCnsKI}FhRzVzT0Sv3IfgY>%0e-`@~rI0*R_@zk_ zroY!1NS={)k{V7s_$`^__;;vh&dMqScN-AF3k#dh|Je(Mv(jrz@m#FXE`=M zp>#}Ssq%HC+%!0nO$fj>^#vm^h5HDqz9P#kxz{GXE)D*A;J<%+Y6Du29TXypxF4ib ziWzLdAG;ZtQOtc#RyCC*#&ywisqm@RuudJveuG;LB(0p0@a)Bvsx;}cOqt&_#qAhm z`InC6EE`?_3lLk)*BgqWx;Us=>rnf_%hmy{2x=Jm2n$MNVxmg+*SE`7c$%>T|G&cb z8(+qKUjdUaQK`=c65%^{ZvaPvJ{A&Lq(kisJ^CSssgr1gwgW>DoSok zS_BnvN7`PzL`IPIy(WRN(n9!1NW&&_6G+-h)Am_VFuhIN1e<`~B*v|rAU2YdUo|)4 z-K#XEa9x1MKU62rTm}>{&IZ7ut(}UZ8V~;(H!wxbQXeBJKlrfEwS9NA(4#jkCEnDd zpD#$oc!_-Sxo=e43}PlC$E_^^`4D9tV{};zkGZ)^wvNn^hzT#xh-G0$@d8(wo7Ham zy^2gpSF7vW%|Tzrrry5mYeQ2EEv+Zf|VqjQtKhg@H~EwOU;0 zK7D3vF!>5%BeF$(GTwO89%gGnw?TZCVOnAZEeKCyt0}B+=P|8&#B6h+v z6=|2Z^ZLi$8!Zg7BbEh02<{uYWUOoBcPyxdZ+BgVgA6rRQCp*D{TOyQCunKPFiN=3 zzf2;R+ZukKo)@^SWZk6F2JeQ~YnvW@6WkM>x;JxMKw-yT`G4sy)aE9DeUPr zVt@IE)`OG?F?ypysa$Wh6c0=7Mr3;O5kbOY=nenoZovBSo6a7r)q(^NAy|k3|NWmM zKAmKqIJ#7>BQ+P|6X%DB82PK(^cl|}Ho7+=xlDWBpPrw?OZ6rQ zOg$9^n-t6h6uav1uc_ps#C4i~1qTwMXyXPG4_T}2b_X((BGXhOc&d&K4x#eaoH@I2 zL=pFHI-F6RYqx*Gd&7u=o&S#=yF%xV!zm3iCUGILMC3k1XIlhgUUaWW^%Zi&m-Oym zs&Tq<(_GhbI()(fxV~43vAgy1zezED$r#8iU50)Rc#jeMEZK=e$%4|J z*Y1Mm-CRWHJKi=6TF1*tU&BvhN>tZ(CuSTexgrw{-N%+G*4)prUZuCNP6`>TL?zrS zl^qnvrwf!Ue{R9L0*jdm=Iai7P0Ep>ssy9_38kr;a;D(WU6-}As^vy5jy`e8h&f3l z{6P$45+)A%qG3p)tj=8eRJ}xfz;GlOpERr(K6C0e47+^?7+9Bw1VWYnJiX}jbx6pc zn$+vW_yae??{nQTLQcRFY74Y=+rY1+FqnCW$uBvJR7%jKBz!^l?O30I@?8@l`B z4tIN*01Bo2+)m523;XQ{&J#%{ZI`G@1&nYp%CgBrm14Q%i6l(fIjw~V`}nhNC#gGZ zp!H?z(Fwk_@1#5f=-RQr@za!&*#}Kt+Ye{w4ohVcq?(32X-F||cAVIE%zSiT?&x;m-#S@0To;BI- z`ME{w<+oA0pI;!CBO9Sv@f8oU{(1^cWC}6O#2-IT2id)hhIg219sN)mTSz(VQ4TBV zuQA;GWH>{#1=i!8o_Z^d0dzVi<~S*$iK;MHT+nF@$M#G3OOU?w$DO3OJ1l7mXsgsu z$t%&BQF@C7x4`^k_uD;sE7;3B2L$D-Ml*LncLWF`0sP{R4>l;fqM`Ml)V^E|O=6B!<`ZJBu%>X*Q7g;k0N$F|k z-!qX|OS3zxPV*e{Dbre624PdRN89k-U;?i{IqRkf$oi{G@ot~pN^1&UbgB2*Ihw@F zVK(Ae2UK2?fWmfI`re~~qFJokVr|^4o;GzOBE`4fU>Cc};sA3WNM~5iC{`GiGFE81 z-^7UTbVhda6t3-n>GAFUo+27{6a*C2`RU5IOwr9I30TFgydcWUZ=K{$Vr?$BBT&i_ z#ao|H3a1f9+EQW80B55W|0gk^HO<*PWeDT=BjaFXfK|;-A`&;2M6kp0>ql?ZcF$)S zbwBA9(G6z3pq66MveI@mjQfIt;f04SRE<__u@vOI2N;pxk%tgpsuC5n=)}MMxUu1F z^#WE{24X8p3G~=VkpA01Uyp!o(h@p$kY%EH%+LHxg5yE(KWqvb?rbP&?E*wy?_V9W zzpc~s?d%gyF%5u~4Yn%1^1GO;F|MmtNYW-=t~%V8MONsc;?{fIK8*^j>#qYhvD#g# z!884xk{Ia~`<`WN5}vRq)RZ~}_rjIc@{L->S45*ltJW1>5nkroR;#n07ey-OK%Iam zZIe^cWZ77DvFekAm`f`Y!u^8EPmJlo@%?7c-4`R(wP*u^jcMHg{D^otmNw@m-cC>^0k{a_urP+yG`x0 zxVv&=*exPtXc`>A8s~4?iZ6i* zhKOG;fzaR);YNfqjaDm4UbUZOr6}I7aswU12Mz$ew4KdVes7mCZ&mjl%iYGh@h*-u z?Kd20DZ)rbcL?n)cF~3%lrkwA?N&&GPv6#LHisxzUnmn&i&f@ zmK#{?;2Sr6E#U})J57FP33=UXOA7vZhx(21=eENBLe-p(hUE}7v$$DDsF*QL?jGf- z7)v*gQf*9qiio>w*AQL`qiYfk4?Y4N9hH8>`Ofi_JCt6Q6R0W924U11VcY%&#-|aD97lCKtb5l#K*v9T0OYpu6`Jkcc z6=>59k+mAT#CK9}XCsM-pVR&!;3@drGzP|>Mqx%Eam3*YPz-!Cp!Ui2n0hwGCdK%I zK1w4_F#bux$$|vk`e3)B$L6V%jR^9BU{a&_kEA=a{GSI?29B=baLy9u zoh^c#QYLNvuyJ&O$bVxsVq;F!v>kIVAdif9F!~3L82a(!cEmd(DClm)J0vLR<;&14 zC}}IB!fF5FwX8H|j-509I8;`^?nd-^h>}^_>cMZ@=b{nQo_qPjF0w`PM1hQjeeq5} zjQdx($6&J|k8FD%YBm_z52u&OkC1P?1xfyH0?~I_Ncjboz%n3NN&^m1afRkc&LDOa z3F@W>E}JzDAK+J>gE!I3EMdPu=lD|K|7M`y&T&ame(!bQM1&~vbm~!?BkFAI9UN&) z)!SdlH73Ix*UeI|6PqccYx~E^gSvd1v`7Q&JeU9#gi=zdm`1AGH3(_O)HRJF(zW$#y&XxslylM`b@zT z-+_(K;T^FvzOmRE7P@)5XI|P{ed4LZ3ZV7;|-z36)5=f0M!c zEpZBtyV?L<7MpmoQTh7wM^*{7B}SM^dvyBiBRq~X5j0ctr-ZxAk)-7^CmRqC1Ndq6 zqnMin;JI~Ua>iGKe3_Pdo0#|wmkiPbX#%!Vv~U&rqi3d;z$QSA{01iqGl~z7*dzfr ztsC>(QGY8-lhfZs|1~4Il$h4GC5Db$c~Z^sc=pw$N8MG71`ExS*Aoh$Js*f-VN8KV zQ4@b>B2jG0QMZim=T#mUQzO`vTBkMdGz9LYC)~g3cx-4G#+3nY5u&Me?(!{ z@%Oy?wl6kPWKVEVXU9^yeWOo}1MZ=iuPgSU)olkpmTf zdw8Ini@Yeqi%yLp`en10Za=!0U=xQ_&4X|QSE~e~y`pnBH}EjHTJZj0 z-0%|{B8kTxpGDwLnqt?lJ2)F?@<%oYAc58AUY|=uQ;Vr2#R&e+wca*Fbo%FIlQe01 zxg03y`o(VJ#diZK=}>#P8;nSL#B5HFGZzx!*n(59C`2TVO$eJQ?aPHcKKUY31>!v2 zuPp}KDUfs11sFNe&(O0SqWe97CAj4`O2`L2cwT8G3cZFvPJu2m7Zdq|+R;k3dq^i* zt~?I93>wEO@&i?W#3)Y23j60$n@4N)13d{1v>CT${f2A(PU8Qzbbva6p6?V@Dsyrv zn%T0T&KHd3*#)T$@a?Rb*8R;g41NjAILSBN?1J^ z3f=GKlc&*gaV7W?20yJ*!kwoH`;?;~kG^{V-y{M);ujC0AcC^G{*D*W(@cmL5m>x+ z@0h!uHrB=iAJywpYv1tm9_S6pA2NFQ)w91B^z1fx`OjOspl|X_lnD{kK(Ub2RMwDmi2&W3;v{z}=DH02^X69y@67lGlt0vvYxHlZrrU*f3ls1b^e&^}ZIK=gfraI#%UUwAB=a)}>dz z05Qqd=!Siz(-BE1omAl zUm?LA5xlfiwF~QxDfSR^a*RSc|$XYd0Pzc7o z1C(!b=`-g7(xtX6Y7(>J+`QKA`Ip?Z7JkqdR1hZ+=c(LPT}U^k7VmU7EG$&yZ^T4r z12K{$l~?Y{SweUc!lV_MHX!SA0Q>?n6#yA1>HGD$rnVqsYv{tImKbCZ*&<*EO~3F| zS;hqtMY@NkORt(f^GyqNKnp>7d$Ww`Q9lX_-q}fJ5N#J@f7Mka1&yy#BB2$HmxOIg zoPZy+Yr&WlF118F5I4`jI{!TL5KZ>OAhly+Aa?M}ICNHW!ab8$-O1SF8ZjR`c>B4KKlr6v*T@fu6}!0WUSdjf19>_|ab!}& z4QMJEajjY^PqWb`@yL09<62=@2LJ0}20B|4IT)kE-&P1=N^>Y~Be6wkZH{c5RrSmO znvTpv*+ad?)9R21Z)e@bh{fWfS$M;p0|*gUqWK{ly|1j&T?S65oRDk*A6D;Ip99d}T%f<$i?*CdgRhkvfZHYYXV%F*{H6iBR19jT6A!i+Gn zraT*oMca=U%;`8<@C2>@9PSp^f>zyt#3rcOVrh#J#0ei2@*h)7i8PCw&hv3-qF)CS zp`@{91Zqz<4^CBaqbz#hVc&CNWmjsw`+$p&>V$ZeF~+f^(qU)c42W>m*I~&j+FDD1 z8+ugC9Xh(cizw>DDUl}1rSc&Sxfqcyo6kvPseprXZ97V276p+q;I~m5O^G=R>iTx^MDDRY%cRzY zYa!-m9Z5Ru6fFj?G1;?5q~#R$gS9pJN&5W5b4wntF?9*)J1z>}F>wao zieik&vC!>1D`3}cG)j~EKXm(=6LRW{`7Ko3^2!=h_Hmq$SYdAP3rggv0jZBdFW>VEhVdX9qI9(*3Z*YA#x#2J^o4(>HyC5v*8RR z%UuDS+WI|&Uok;kX%dGZvZ=Xs3I9$wLZLI{YhX-gXhJ@GEH4Bn`fC%B^{Z$S)7QV{ z*KlFaI|_UxO!y;>t6zfBIy&?K`HNffFJW7s?B;`!M~Y26it;anbiW;NRXm<9cId%1 z=rw60YhX-NpxDN+KXkYBM{;E^hv>}OPLjLJ7bf{I)xg*J z7(6_0{fU(y^82V83GYrU?|YwHAN8Xli8zbT-E9;FiO=YzZyJ~BkPjWkf zr`FUc?Sq~tE>~+9@FOzztcA*dFLJ1r)eZ_0+v_@>QmDojj33Qs@xJQeR~q7_+5z3sN15!TI58sT7OryVu#qq;_0$jP}e_g)Ud05);h%C#lMUX z#+7czqP5#R4A@#GWDbM4^1+*DuK^!1inz))mzG)D1L9|&_F}TvXI)0bvsAC>g%3`1Pr>O?(@;L% zs<_7U2`J;l|0srtaU#9dW5G-?A%0~ipe=rjPK7uuk8NcaKJ{7@6fb8Fq=OdWn6stW z%=`x{Q0}@?>OxF|KRG@`VFrx>f9K#IkXabBr8vyF$P4JhLQGc;Mjh75l*iCA>@oi! z_A@AZu{3S*eTV?EUv0Z`O;H`Tt`d|u@EnL3l^wuGnwl0kjoxi)Fz26R!$$-cj-XvN z0q_>`#3l#D2T`pi6eo>A@h zP9{ocR4Y?y?d?P3Z5pPXsrV=xPrvXq7B*zfm~0+3z2$lLJ|eJ*S8@K(9OBf&Bsm)R zXBJ+yS?;E68nX|>g(M7EtkFTZ>07ZBXtq}+xk4{utGXk!d3>8IILnOG~`^=Gxvw$t!9_^n-H_mm9 zFKM98>U@drY=E`<@L7fgQVUTw}?8EAJf_#WKm0n1}tfez)zzE4kz!f z1ay-Go^kioa9S3JOS*((vEPuB5QR|yu=_twyV^y*i$X{tkgihTgAV_F<@CF*8m^-- zoKG0$e5|d-(#oY+;ESjWDGv`KWm3gu|B%_O9j}fO*NjF}w|x7a$xe!3n5L1+;x2ro z<}xn_aJz8eM=_v&*s25xFZX}EpSycO51L=zrZ~@q;zEpWNJD)kzpd2RlzUO~;4@t) zM}@Vf94&6*0;710JDoE(Vo?UBM$YeMpa137dwH0I0vrF}B!U>0Pt%MwC%oyI#?SDSb5Fbtm;|e?a$)$@eyS;aW z@x>7rEsMDHZ?n3D=MQ+_E5R~j-rEU!D^o#67>Pm`vA{gD*C|(Y`b$}~$mGM1cRNz` zp>5tk$D&{vhs-&@K;7DUBYvA+(mLrlg>@b&!3gd22H4QjE0=gPcxVc$&Ac`5FsCNF zAKP~qN(Md*T6aW;^GOL&Yz~qhs46#w#u=6F!I^NV{-&4JuEPuea#72Q&SUnUS)ZG} zMTc{aI)%151D8)_w4fz;eaNUBEx)Y_35=%@zB3+VSk`^@${Y*q{PI22Qt<2-Jg0>?}dPQj$g%6#A655IKNz`ZdUtr4(?UO zk>eNFf1jhV07}_<_Vi2y=sy)yduLMIzbE%E>uO@G>m$ApO(G5u}cw zDs^OlzNh-gK7OD@fZ4NmSHli~P8Lj4Kzj zIglRUiqLTQ4_AG#51c=1P#ek^=wIjWKLPhrR<~~~t #MS6?IlBk!gIVdj2zl&Q| z@|x)o1w4M!1i0Mb0c3{dQ{mo?ZbHbA`FZWrTukfrwak$e=AP$FqyRb zrZqk{U)n$t&R{Fm`mi@3yxG8h7St(j^STcYXk>pHmXxP#_Uo4ST8Wo4k?ITlG?IjW z{DoErb#PqcdvEoFxou2sQI!`WYCHHYuR@;IP$rn8=^NQq(|JZ zw^A#$GuQ#>fb}c^TQ-X@{{R{m4p$@wg5#{HzNxSNnbfCm`(CY0O{He7P0%Oz7C5w& z_iul8EzI}*DXD%Im8VFm&+7E(A5}jlOHEG_LgZRFOCcR;nA8JK$@j6Y^E|kU^&5Q`!c~w4q#heMsHvS%lp3Jcc_Hvsp}$%IWv>Qv<&+B)|I27DET9L3q#XZEIdpMaun3?|*@Q zZXfUjSucrpQ(YM*gba)(FHd@~TOkI(5(qsIr@v}aJiBP`{jQ_JL!fD^e z^e4_+A%Z6sob=yDR{q{?i^aO?!`>sG6-EJ;AJOMBz<#F0#q$xY%KE$&*@jyGYjs~t z8hP|n=yS}N4@PXp%2xuxeRK%PuH2jMAhgua8zd`tXvw5YitZ%k$r+c&OjIBE??oaF zgNOpR8HPvnuAW_fXNsKDm+4>CkEuYWC1TF>PO#XR5AevSYJ0fiey~Kd=2>D6b-N~p z7)DwX*tkJWXsvO3K9dEEgb6zB+kNZ=-$_Msr6mM>o2j;R=J)l{@WYN}#{~jWS@+rf zvX^5bQcMO-kgUk3;#&oi6t^dYhPoXCcqX;mH%Fj3>ZQs@KC zE)eQBmkp2_qy6!ax*213{hc?AL9tAdD*X2 zRr=VPC-|@QlWm#PQ8dzdVk(M9BPo1=`}koR5Q@jdv5XsVWG6?$c(mGtUiBbF=bL*t z`DxZO4K0)qPbNH0uKpds8^#Wg-Z6`7jh+42mpeP0<7^AHA9E`si4j9?<34Mh&N{g1x_&LcdqO#Zip`s%yF z|A59HI>?RxH|YN;{C~GJS&6|!|BsaQVOh=Kf2VmvN`mzt6nSgQsQ5pjH)A9r3-M7x zXaoB)wm(<&KD?^2?9#e<81O~^u$FH3T-kUL`48=Ha|=Zff1#kn^;A1RydEZ@i;Djp z6ps_^con2Er(dZm=(X;5e?A-Q$G^(y9mL~kT%_*kg zmoRa&<8SK1uL`Fx%9xe%zyB~d?p3CQJ>Y>8xGda-4JOoz$nwL&izSVvvtFIJl8}vjyM9_%p0@v$HmBa5rcH{Yr1vdL`taw!X6mj0_q2gV~w{HALbR zQ?e2y06gs@B$=3rk#iLh2vi{JfZ>cnDAU!_;Fl9segJ#(sN{(M7X#`~+ypzB%?wPL zx}4v@n=^NfMBuGX)!8{t^0U7(K6Fp@4mSn@u8wqlTuT9lw8Lyv+A@}`>f)6#S$&)c zx7It8OM^Dy2o17@Q|)UXDjh;|NVNYE1Xq_`p{<23L<*2D?_)E2zu2ywaIXXB^uWJ2 z@blDetTmu2{xH?rO_TfZ(8SG2M(ld6=L-wG3QB^SNkfP};|UY4etXbFSk@J#y)qts z=bZc*3Uzs1ll__84tRzCNzE0#cv^DdEr&%>ix6LJ7+sFAc9V?3yBYe=8>-6}tl+c| zb&%O}O|L`Fj|R3aZ`VWKqu_KaJ6E3*lcSA7N1x`uLR7X0x_4UEx@?b8qj%zbgQHP~ zSqK!O-xQIWVy?Y6no1p0sGYes&>x=PLtN)tWnGC%pZfhKllgX+4Sgi`AOC$fBv0Na zu6JiI>p++(#`<2Ts_f`yQ&h#1>vo9Fdbu7H7Fc!^a&oaLKpgo?sOE!kS$w;$`^hA& zJCWGutsv%#yTGewJp37qvM0m{>vq*6&2BWZ*Ikrhu>CorGQI|XD8t%Fq-Z6gh^3^H z4L)sggf?uk?Oh18bO4rV7cU~ON#I4>o(R#2RF@$YS33Kb!2yES8iziKH3fLBH|76Q zahS7^-lJ^u85#?;491z5wgKpHr03SVv5Ks22xC}tUyLyx1>8F(%q4m zN_s0<;9lKb+0KX=E*L}!Wn;te>9ulqudJE`$B*Zj&x;*Lm@Mx&eu^ZM=)Y#oqza{L zNcePkJ7ie5A6VA5xFg3V9s3SO(`S`*vhtPg-)CVIWm%W9{S*)Cs}^W7zUA2g;-VU2 z&dYB$1xXmQ+N{E|#6gU8o2hOS98DA9aq9&ln7@4uI3 z@2NZ|E($O~b~$avrNTeOlPT(-sUUSwCfP#CHX2+y9P%C&x~x))Ja8-0<>=3Xs$O~m z@3Q4-Wj_>IiZyjJkX@$vN~tYh`e6?6!9Ft5jRL_9;jg@h`9zZ&SDI5ji>NF}$`;z# z3T0$42r*3Rjzur^kQ{stwAZab90{Y+^Cf?TDCMy)FSvQC|F}ctFtn5JcSp-{w>+(N z?n-|FPR`fch(rNh5IP1~Mwz-V&T|(v#J)IGA$-#6rhL>Iv|UM${P)hPQST@Y>B;U9 zqy(~#Avy)*3;ZQyISZ-XZ`bI;90a9T&nrbSGwDgn)YY`lddcH|LAcXU>iPu1Twsq1 z+I7C3pulI~j1UVD8;dGwCkQcuncSFCwK^A5NtMhTGYtBC?yt zopSR^^GyxgGfyY2`jDQZe)lpXRfcI?gi&93GO`>%*{cSUmND|M^qdr*vRnN2i&=AbqCjdTC*FX)TWJ>zMR?V_6f* zAECc{FM$v?QeJ8Mlpit?V=>V;x9g!4m%4At7dQwxqH&go}4CVA1WZbv@edC)?*PR+j=g##0b&5bvdr0rE;HMF>fSNof z47#w0i=Z_7O42rmYOnCi`efVvzOIFZ8TXS(g_4P??WIg|e@ci6NL3<{gG|vV2Xe-)*EM81P9~?BdS?~p8faG`1 z$GCgaAj!wUYc)jk|! z{KjDvfux(*(qX;MjHBnt2Fru`T2nG`5!XI~$#TNwW&V`1E3N%%lf+ZroZz{XHfv=0 zem-@N_FA%M2|2TklpDlFd~VxTXxFXMn!~6r?+nZvx}xHReI2=5d7tpos%~}mw@H`l z$AJ?6<;V*$S4wLG3!4J}(}fe=qvXChScowaH(>MC^NUwV-+oi+<{v(9Vb?vLeQ-%u z9s#ua?`q#NqfO+~%jCcI@kqa&I-SPAJA!g%c7J4yyT}1Pa~25Fh^UF5=SyGlBGSY_ zzo)3DjcAY(A%vNdRy zu};~I3X;kJQlF+|miifV&4%{`J;e%dob8kS5XzlP9r3^C{bux-euAN@fuCw^Pht=I zC7um%O0(nA9OGM(nrRGB%JPRLX&!3#7o&ods z3fGNqY<@g`y6wh?bEFuQxgvY4su!s($?0uBn21w!^nmy+(a|2$?BR-VNvr)A>~+1S zk!%}8qEGXZNWg)%C_Za_?76>uLCIM!pb_wV#pinMMWq4eFK?}jGp0H2)f)uOormur zZwccv3&W-0sk#|9EfRJ5nZlbR>E_O(_Nix8jnrxb%hd>eA@DwM?QiZCuQC!Gu$mQX zrjlS(UmeNpxbY!^A0}}kEYZo0_S&30O|}#@OsN>=qiw=q5uYpvK@RYNdA5lFRyv=je}Zq6dL{e#;5q|h%rYM1a`S38}PpNIc?y}rKn(ikrDOiq(3X!M8GWt$f+cg$EYWc)q!4jYsb&m(M8_IyYQS-+l!@<@xzwrKfx#ZRb!^N9bfj| z&ndDPmVGp`NzM%7(I=%oC9zHtGyq?ozuFZ=`t|-C%Vrx1{Kr{F_#jg56g2rZ(?MU8 z=-kBjqlpdfTU?P3TQ997xcZ#VZRJWD!{q@}L((=r2&>1R@sLqjoj7b4O!7PQXT73& z`hO!T@zwV~d9aq1_6^C{uR>g+&(qNDDkoUUBH~;e(H3<%So|AU&~%u5y!F;_*iox| zy@LE9kwkA7FsPmCG3~~`x9{1iIjgxhZ+|crkmF9D(Og$hkq?QxRz-X&(?`Az(L)2b zW|Uonk&9_dvN8>p(;%i@7t9c*!VjHMiQ!iSr&=>4H9#(d&>H%m;%kzrqRn{$Clwsx z)J31;7AY6TM8K)(PveVv)3OX?gXIzu=5$j7WI^pyWXyX?+lf_>ZDv7cFD|t}RT`$c zWZmM%6Z3b)KyE#2nU|u9Z6<-2T?AscUKUxxWMG8OJhML49az?tlSdy$$&TW?e)Z!FhX|J5VNuQz{`PZ#_T+|;P%s{ z{-=$zCI-FsNA(hsm2p+Y(7?f;@%?(LfnnH0$a4|T|6G3X71{pnm_WVW=?Ba)F>C4{ zAFefJsytThE8jAzjA`T~rwb6#Ma3tq|1(vgMLaYI4)nPUD=>{kY^*wsv|P}#6X+DT z|Gpa-!Cv8liSe<$@%t{PR9kR`zH@F;L^1DDmf`J@$p)Q#!;urBVeO=Sa!U&;gQ43D zgma;8iN(UCnr3IDkjt2#r{EXF-tGS$u)V=YQ3Z7a%7qk}hu zCA3B!D%#G)&BMizuP}Ia>~nS(x4ppv46t~)WQ9NRIorW#vkH)T=%c;8cibOD@lR|t zH^2q)XfM2?@B)eb=m{de!*>Q4Q0?%(0|y51&I=H6*lS?p=@OjszoG9^=eb#ETgf=7Mu`~79Q>BEn9m4v^ z>9(^$?g-qs2NTo3j{;Y+b|Q`9E}@~ zNTu3l9_kgq_~L}ddEAqh1+`>|DN#jIY%Rg7`+qU^)^Sli(cd^BAl;qPNOwu^64FxA zUDB|CfONMK(y@zl2?)rNk}9u(jg2KBQzTy6^!R20k``Iw#V8GbBtlxEWu`@jZRk z(tG6F3m|UxHk&8ui;)9sKa6;#ehO;1?-pYmlUe$^l$D7dKmK-fhW)ENlbb&Lmx}OB zd7W_N{%;^LHUl=BrQfc3w#e!~!JxSvq1i@0@wB z8QluBI{ITO{bjej?SD*|t_JH!(v^L^=o(%Zpv^0PN_vlWD(#vJ$Lt3CJ_^Qa!gk?o za-kn?Ojy&fO=F3C)M3av{lVGdBR!AVDv64gpm)bgTDgz)BP|D$gLTl0zgI9Wnnzgk zJj30d6%5E7#J^Eo%FBj{VWdz*aAHJTW8J8q(LzmmGjLJ!j3Hh~^T`{gC^VjAJ+a?o z=?PJMnqx3Y{!iJw2>lM)GSt8*KfgCh>c!s`a4qxA3qL%!Wcaq?*k9UWHzBlU?l`}< zlrZcb5A=~zazy#I942Vv(w}P4Xx3L>ZUzYsLVC9r+mD4R1vSa;4ap!>4oX#RUh?I>`>fkX4K}d6 zQ7$>SVBv>Plvzn!%7iZ6@)H!#qaUHsKEc0yZok5=*u!vlyusb=$%B01oIgMy)nbp- zcR>mtNFHi_rXpm_WqyJ~2WZPZk@x?7n3$2I7NOkjiDpl+yC^5`a}^Z|-@ASj*aDLb z=Z=0z78dEs^25v{FP1p=b=6GM=OhXe20YhO4p5XQYz8a+D))+DUL!f|i~!cSa9_7X z8##!s=swT^oeV44+aHphEXTm!N06_|JHO%0w#O++_Rt}82hsnF`g6KC^#_0V3@obH zqtmG~64E)>`Z#w3<`UoY2P-*?J26xz0l%y})u~n?UmcHjOPnFJ zt-=k9djrf~{MJBBaGbv1XZAr1oF?0Y&eSl{-QZz3S-$nBoWR$aG-BFi}1r9;pALQEj=WfW%G^Az)z~ zlDkt!9q^;GzBkjCS7EjbCxkS9Ii^R}4v8_{CjndgAo3jO+$#$FI}QcCADt5GzMkPo z(GcfgO|$;*l|q!Q=4Xv*8(D5L%ZJ{yRU4rz7hb6%31S;qh07nWIKm$*Z*Ne)c{aGG z1I}*B&ePP$;WETWev*I+(r+g%vU&mIr;%@sbE_<9BiEy%?O@-V&GVq%yTEEsboQMR zvOizq_lT0A;M$=YN;Ty9iiXha(5KuqGJM@*A}`QoJRsyp(I2j8OZK+E0WfN${>n$$ zZ;f*p`|ohCDxKxzXE~qr2X>Iku1K#C#tpOm5rb7&r4Dx0CD)rz(nfQuYhVB!*pDr>1-bMRF+dZ4ZW|Qwgdr5`1W@uL0`dOE;)LohMrMeikRc+gCPf&?EA2!Z)9l*Uq_c~A}`Z__CH@ z=)a#H5#JWwl7lu6XK9>MHqLWkhWV4QDtF4rRGiheMmsYIQZ*eg*=q?{KNCEVPd6xj zkrDT`Mi`z?>P8v4Ri`NP*(r1Lef{LEH+Zw)0>A2g;@2AEkv;H9haq?P41HS^C9CPd zR>3UWce4)K{_l@w#Bf~S=%2pPn-x;n=6ogut4f2u_*V+EYh+Z>H)}q`x#C+jH#K;d zWDROhx!Zy{cg&imvggp^IVm|KgEL^C{oNOzwr+p<=1)9Q)~8YdqeIpv53hgTk`OfQ zx%(t|5!I`uR@eqq;!Iltig3FHa9_*Th0BYNV{uPfay<+S$hM@YjMC|jI_a^&%p?Lqf!G`ZvCpAVcG#eP?}^|BFy~RAdrU|z5}T7?XkYpK&3@WlP`Nrv0OoJsC%N&3qJXlLqj~D&B9Xh}hfiUr4N-}yEz<^3T9LfMs(PZn>Pm;i^!R5Z-@`z^|59Z= zfE6jJ{(}SNGGd)N{!H$VSwMcou{^aBZ`i@)xXw89sbq9zz%w%EE1Jp&Iub5fdT_>n z@@DcdAULt7%HOXq-`-28aChh00v2(s?aK;}clBuY1e0UW)7MCGqF}vtC=hH!OTW$N zQ{2plmA*?T9GNJQbMxAp z6iNW+8a*lEGV#NwcA$*>#U!)E+0pUu4tAw+u3fz^M%PV;)xk}xW>r-8K-dcWEK!vP zlsuIX0qIX)&>lTgwgyGNBvnzlqO=8hq=34GzjLk=Ckb&1n9Lu&{rUd>y+C$RY^rp; z9<5h~rsirmWhCV-buP`V5T)!7$#Ui=B9hH+Mm~q$Dt6g+rj|A$Fi}{A7#C6l>F@c+ z*}q6fI$p@G)%mL!U9gQ?n;pm=q~fRyGklO}ua@Vt#PCK)r16Lo&50&V2SR=EMF699 z!Y;9ow~vsN?%+)0FW;k`{>v~SEd5x?;GvQx_@`GKZgMHs5EU-`vs1=Y)S$?J!ZIv` z0TLb*rBy!%dEUSN^E$7avA$t~jmA?-n3g*jvxQ?^Q}{o&++TRDM<7?2 zocEgm%|s&YX1P3rKUbV~*HwUa*}@Pmi2R>1igyC2g!5+Tb)^iXM8(#x_tz9M3gvPe z?pPw{ABen=HlNcL)VYSHkOZEqL7wBYal+*kwKh!^GxT&8eE;K>$ayoHEz+5=sV&1C zLY_c?48r1eWWB;qHh9Ws6E3ytk5)r{7C56ongki3gpwM>L>gd$|3zdm>17C|V1;^u z{iF?8tv*PLnG^FR7&&srByf+AD~0pNg4n=Ui(am5kKl@e3Tc6??hMr5T~@vSiyU1e z9Ij$;^>{Wpsz_*sB@d?(Bsi1rr&~J%abuo91!NsR<8X9%tPyIu+LWOs#VtrgbL0AH z3M05&8l~T+7mnTQ?{-I+EJU9mTr*EFYmi&E*d%9T?F0Dl7t#swk!dlCG4uD_$?tV8kmFnmD9ZsyG1k z!vJl)f~Bw)?7zS%XRLPDkVz;O5R!086|<--`5Y9zz;rjUQN+)erD2RhdulYwp2}fA zt}oulSYO)uq<*}{qZ*>9I0)H(yQW)i_~EZ5yc}p6i&BI5!pfd`45e-N9QQnop?W%I zuj`0?{9MHJhl|`2CwZzCh-xw+and0%I%UWycoJ%}gNIu5YXgqHmZhTT2uog&S2>y!hHkqplSLe~MswkzFg2Qt86WgQ|+MemeX~Gy{ zQ;Cy5693i%jW2COMGJ$kIpB0ccU$a^5(W-hY1*F?eqswJ^&p}fb%k;ky}7Qy$r2ra z3bUg~4jc8U^s*bK1ESbEckQ%j*-QOV;({5 zUn}eBE#;c`G~@B9p$wQE-D>>Uy9``}+)r^zwUF|A{eZR`GFoEzJ5NL&y6mWsX8t94 z&9GfP`8DfrB;->9<2bWFQEU(!;*5jq28PL{nbTAlKrxwqo6c+`X>I1?`bZF z5eG}idVC4^{?o%rmj{lZwWkaq_j9w6wsaeK0BQJs0(S{ z=b1k3>l*jW3I!uCc)9qUUEBtDiiaeoENb(1j#kIaaz!{wcp?(mOH$(e0Z za&Cn*S2W0Q-y}9k9#1B)^V%9zZz?;5je0irO)>N{bEFG!uA;_`*hWX%ndnc zNk^kTE8`!og5BT7vHHHgKlCi_R)_Gx-dGJ^xFz4OJdYE2vFk>9d3P3Zwm3O&_@r&x zfK|;_YRjnA6BV<9^iv06FXkXeY%y^v+snxc&qb$D*y?k$cs@!sB36wz3^sqW?<(yU zjwbRmsanmOM;nv=&FY@_vPyHNQQzbK)0N+Fe5Tmh*Bz@d=!%;lY{~eiiMs6E4s}VA z<2WUR<_1~pw~w2xq`3yLA|+oQQ*Ka}-TM4?t;GV*dWyEr)WvI^W3%G|3)@jzHaSs}Qh1}OP3ys%Pp zvBOy^9d`*6%sV^ZgJtJ>LDCS=wq~TkgfL74*76Wxj zMnouXvhO7R12x1jMA}j!1L?CByXJr?G7V@`h${F~NgYxqMrm-coVpKSdtDIyie zWijMWnK1y?qNt{sFA3fhF*y_84-bapUGbo0z#5%;V+-1{-~xaDqKD-3+l!or8Tj)@ zztUjz;w&S>?VNSXd3D0kKnO@`fT-7kKKYz22SbAG3ZJS+T?dUuCUHlnpz@O+#h z+$q*4ePYcVg%;$RPn#1q*xCU31@|7`w^#1L^LLymYdqkcVINF9&By1@LUF3JV7kA5 zMD|Ldt~DxDWmOY2koNXx>aC)^;^aM-coxA}Wyl!t;`WGe+Aub2 z=yG$}xj~WeJ$6cRxgu46%5bw_X{^;L3;E$hMbw0hz>1ZWbakFGfxnNXV;I5i%9pYqVbpTR;MZJgmxEL8iR zN@jd40zN2Q)FjY9shis$tXF;V1zRj&7UoA-SK$7#T;%e3Iipy>r-vFrxgS2pR4w=A zJquJ3%AweJ`?lwpcw)ub8cnYXHD5*J)tSX!qR35`ye#PIOcTow1{b1fjaUayYIws- z8274iE6JCFuQ(?kNU~9|@Z-wbq{Yy^P*xO{|G@QM8DI*LmV~-G!_pzJTx=t^Dl38e5zT@Kq(UPTtdC{#_ye#qQsgK+Qe=&nNPTsV{-%)J!pj1cXgoD&; zuP>V~qfC&7Ez?vh&0wq;W&vC|QQRiSP+_>j;mJ#@3?t0s{pzgWCNzX!f$>m3gab~s zhPx57H!RUpfn!ZJsc4cael-V67cr?6S337zf(7xa{iIK`>%0x_4EH4?Eg4gvf*Ho0 z^!Lj|)$1eBNN$iWKM+h6_`9OUS}wvMo)BEl)_fZ&F^rGNx<=JEqt&TT!BOXC0LZO(!g#@ zwMdPJk;BH?% z&%zB8Em2jb*62dBOp|L;*hzVn=;Bi$y7M4Zy;yowt)3KK$V^5FzY14)rQ@Irm$vh> zCxDNCX7|#;Mb$3+URjx{taDlP=-V*%%p3VefsuDF5vxC#VB6QF)vSZpmsYH;I)1E< ztlA1;EPMibP0}~1G|#VTkvcE^Td0Zkj&#M+?=lnlXbfs76lV!*J8ZBQ(BqUd`nzY! z%97NZCTAqbL~k(>HY&ydO|k?vNK3|UDNi9=2jHGcW*mIjR-R=g$~kMe(4(v4V=kNd zUPMPDAZOxHOJ#kjReDH1T^X04J5ptL24pY1?P&Xh}l_PsM48+GlcAjCD|dSY&q++1K3N=GVd`9pT0Oeto?wm3YmX+P6A>|<`0YEatXaa z?I#DW%m2Vjrm{Tdy}YjgtLUPjn|9uy$1NI<7aK+F>p!|CG^-i>CHd;eyZFvl4MLYr ziz~EevYHS16JC%^as4BM!{jfy+cl)&*wh$mC>4$psc?<6q)0NXPc0jp z@AYG3upqyGrH6Oo9RO}_)+sYmqi^pTH$Cd>h8IiE_S!5gdPoR8^p0ySO*u)a9pKxA z7sQS1V3jN%NnujLq!kbI8u^(iI6dlH`@i|IQ>}mhIkfz(J<$#6jx?mV?NItB06VmC zW3s{hWHoaQdQ~X7eVk{BN>7_fJrQ#lW%3@pHM)xhz6ZQ5(3Vh@*nM88O&*G=o+9vI z6YyJ=inoVbqzde1$0tN zZ}nUF8$_@@X{D%f|F9o%EiJ&>ogz+E9&0!Hw+GF1VH5YAAbch`i1#aJReLrBljVu3 z{pG94KMzn6D^9xyhsy$<@G7yO?^tLkB0fQl7dE%hq`H^-d3IsR=-aD)JX9_|f;`c; zNg$+t2)}2{?4}~8ew^8Ts|sDL)#`SF`@i8LuXJ0i{lpvjwqFru9~`>KdB{H{_%A4! zt*QjH(RtuSvPdWFTp~8~^Bp#%3}N#6we>|l@tfk6uM9u@=dj=gfK>tUm4$TP_YMY-ZbjzL(FsRy)C)<7IK851t>6iP2% z{*Ql5@bQqZg60GkLFRv%oX|d3q4Cme+?iv-mn)TviF)N&a}O7rfBb&so4x2mR$P`S ziD)!?!rh5znG*W(`ziXw7Wuv8jgtOK`*UZz1%2TwBBAVa%R9a~MMt1c{sN?f#oVeIrLYRg$i4@42a7k16gJr;@wljd!u z%9+hoYJE}|lRH3@*GC*f1`w$Gk;e$9)-C-h3z^UnHVd)nMk8hsco@m6M1;bsc}egb z{)Tt7<5Rj&Zr)4lQaLfwr%85^7T@PJ#Do}3_&p~$*}Dk+n~czT8c3H))Bdf|e&^M+ zd#fXqFB{u2F_BH&Fsc#{z(!|7Tkn@$hNmT3RQ*Dp^#{7!ba7$HsV_+~0$)BAaHU{} z$J_-0rGf7XOdRA{(=X~QN0pUoz)WhRPZS^CG65V;UE4)TE&1o`S;qF}HVpruRnMuP zn8ggNjYGK@bqSBZG>xjcKj7GF6U7=Plzc0n+jwC85{4DoGp!cwwqX12^(duMjA5h# zs=Ywa>qj(Cct{BvA2Q~9h>Kb}&e+C!{thQ5BKU07Fj@KYLZk^O3_r%$lpwl}m135D ztwllzDT}whq|B;t!-#sjk5!pdGn3){E~~TB+3H=?OUdf_*5_5$-nzDuF)cYJX3vAk z5%y$dPe?Xt;h)B8G?UUS{gu3!f-LsiYUo;m{P)DBwXNrw+R5N$PKqT0IF-3{-omyE z^m~rXN~+QF5Hr-LNjP#WlYgX{TgG(i7RjHXqZNX}@_d$Dcilp4r)&0euoGxD$ z(_}tp6EE|rwa?IeNg(r+H!V53XG=;y$u1iwW-&t(gd6=ZVqKP??BEiI6;$mbV0lKgejy3ZBZP!m|(3cN0sf}S^aeY@OGTZM2>YL?L-rmJ)2TP)F%qEUJDt(zHLfwD^tM535J_X3~%N^&PAQ-dAqNe`e}SmT3*Vhhv=~s|44~fbg4{DA zGX0ys##ubupAM?#TZU_u08<&%Ft{oKmu(p*IlC9dTMG354Lm*jSYKqJ<=A|R+T z^hwQ-@a){?sw~2V3nf`;ZJ=KM$xIeMc;(~v`uchmu;4m-?FX1E1R)H=rBwm(l$Ut6 z`Sz71(FUwuEMK4AS7F0L5f!VNqC{9Kv%&XzK(iFWsKw(`PrUCtNyTAvV)>oQEAJ-! zzocbQ1JOH0ivA*T)hmpemUll1Vu{q!6KFenvqrc{igx&bo!gSz*W9j0U#bXbqFLZ` zy%#o}pAKIt04A3_pI)lq18izb{YXMd@g2pqwjP6T+gq~nzBU>H3o)>LmmnsC=@@B^ zQ7C(&MM`HwrdeNzU4gpAbS3k?ByfvFbep|@$OxaMo z==ZovnoiB*FH-)uADOgF(Tg+OEu~dKe`pmx)Doi_6585L+8u3)Ef6cuSK9=z{-PQbdz7e(^P%5TAePphmxm?FqLS)=#aL`g(QD|c^TTT{jfe%>6 zm-AsE*#Zw%46(gr?Fp3d)|Hcm$9@>AZ?wLV7585l@5#C64kkg!;+N53$q&!iWRF+P zQgkCylMLFP<3^eY3LQd4Wrs{topIZvzX_;rGN2N$SsuRQK1(NZ6QiqEWqkCC5+314 zd-v)>V30X_B8_@1zN^7hK%6Ns49ay_jbU+bkX}8C4h`@XZKaP z((N`gPojP&gr)3@nc1u&Iig^d3DPjpq=0A6@}d609}STI$+Wt-d`#BpS>2PGJhyE3 z&SA;HnZJZf7=>~#gWAKad<>{l{!qd#8joLVR&LkJ7lcCjd92sy=_?6<&6x6Y%UM>t zDuT?!n@ek9`F=4wva1lyS%XD#1V+zk3?7hXRoT<+>%fx}bMoR+eGmCDdUvVP{Q;*_ za)QhJYgHN21W?FFS!*}<$tVBF28B`ouxjd3#x!=NXy^MgkJ7BenSVVYn0d5~Mqq6ATBtoB_y%Q+BBo95RTekaeXJOD_ZZFUEFQ;GnwVbUvmevwlpac z2x!pkXzJz%uY@v%=X4O|VIer}q1L8(;5S(QCnzN5~H`V&nD8m`&obE zpevGFFfZ1=P`vXcw&ndSv3@VEr`vs>FjpraWJ3+Np2eOIS|3F3x0%(H(0OI-i%A(cPW~RgV;lR z%PutOo>f^8n;K*3-moKy-@{ePu)nHl5VU7?y;D3O2oIDLS!J~NCZsLlC3|#bF6%eW zLpa^DIF<82vOjVux9e(Jj#413P{W-_?DAEjUG=+Mj}N1AJ1LqdK?v)!gDbe^73e2p zBIMhr6JaphGx_bbFG2xezBhI&DQPvs;S z&Bn3jBK6l53Eezfp(ceA**p6lR{;Y(O9t!Me&Q_|Kq@%c%8CHMfSG;~CHnZ`ael{l zuyK;z8O3_FdLI6K3^-BkG0*R?OoOk}FQ=?Frl)t=Mah*K<5PXDtGw`>!YicFWRGsUH zsmbyTlYIZXfEWYx8_VDs8^NnxK8-V0d#&WHPPxNA)BCBlP@X@m(@p1z77ye-*SrcQ zBBax_&#L!7DNNsxu#T!~x*1E%o^vX0WW={rMs649LyW^ypUU5n4*2bi@om>Ngq?qp zvoPD(cVQ`3E3-6{sSj^os#pB*OFqwGQ1_Ea4MTe^7aMh?dm|T`xN~xwE}0b>%Khg> zvtQ+A{9?4iW)b`E)i$DGF&aDsMZSZzXEGTP$eh|VMQY)e{A8l+`pzfojnXcW z){`W5p>TuN#nLGEr@~Xv9&xhC7H>u;zKVz^zn)9jI%y==HksIqCtB?-mw0(|zS8(! z;B!xe7Tgw>g23GeY5&L1?LUV$%=KfOjpgD0{JI{LiEH@3UBmy+ltdB1{y1U}A7JUJ zwHgVVu0>nelR>$4|Hp~$fAYe7$Qdw8zR14U8LIw1Wi3odc99QUUo1E+9f!~+ei9=Z zOuu{mCm(jq$60PiK{|btOI|`$#cBGtE{=MOKmQNjE0?*9nZ0xQ}E3fn7+tpoAFeXiHJ^} z9G|j#PZu5}Gf`m?!FCES!VQIb?BJ3+E;c;iIQXYIkS>hcby2uXLYV;NwRWKb=hgFN zlm2fITm)%dA&l(5*3|v(TFXxk$f1h9fmjT82d`G9bDDStAhCCFb}1Jm^+ zEdH$;t*SEh5p1&9F5OYqQOVbyed;zm%(^unrCpw_L6Q zLb&5ggw8K%*s)b?xlknBV-Ce)VeL{_)&EgP4%|yT`l5&b+Ac|?vBP&u0@3y^+T`mW z8}~m;4_Q+rP7mI2W=wv&g1UFV#9#tiVEE|3K}^m!@_ikm!473X-%YjuuD$(a05<~4 zSU~c6i9$Oghzh%efdzFxWnxvPl&3D14{NYb{WR?98$$YO22Ft|(teTYE<{OCzz_#E z)s$DoiAFaTWKyuwE7@-6t^V&a8Vs*r~OZV%(>U+yhNx9EqgCkkY3SHZZc^{(1QG}KAxT-z=K=g8N_YNqQpJ=2p zw8Fz9g@Y{xeb*}gn0K2{OP>Y2Xpv7-pGIHvZ(WrsVf9J&T=!wzl(vs&l0hVFaIuS^ z(R;ue1^eX$9C4P!lfBPPydqZWI2dIhDGTTEeJcCi+oCnAkKIS6T^CB9Oy5Cw!?j+0 zcciWb&5BgHIiGZtNHS>EY&7Ard8pK_)rvx$@HDn>Kz^>Ps|YAfi{8^pW9IKB4bGJ7 z(C#w9l@ePkA30||`1;GrdIGLC(4Hb+r=4_NDl~WUQaw`RwkJ&cdntGmX4=^Ya*w z^F5p8k1!{E{a>$e(BYiTr$dqt(iav*aR3+yU@20!|5O;D;B;6hbuniTE)K_m?M;^b z`rO&036-QlIxL%a4!Xi_#)v?M=A%z?O=k>Ib>#V)V>qrP4DZ9^!sbgWfKHY657RW0 z_#i{GQKNAjR+g+D4)H4uk^UkJPTHB0&-I!=EESZ@*y6Q^NTIl4#q^XK3x6BRw|RrC zo8NMTmlrasb+;)BPEQ$S&)-s|6Ui2fa0F2(>3Tv&k3JCZtA=hI_U2qX6IMkDiOXdw zrieTxLB5jk@lUR?3Vb);@oRf({btB9YHChldB4n$;>pumcY*Y|A0m_^Pgwct0+LQ- zCd_fmU)H*de`i<&|2gBJ|13>WYJ6irs!{76R3s@!t0#MZO9vHK4OE7x@m=4j!uI^- znBjEmU&J9v21@g^a5X**L`l6L2FIip+W{ac!;gX9KLa|wrGhXGAqBcg9nN>csL&6~ zLY#l|*&!)|&NF*xoH$8oxC1Es{}U@TN%;-635Grmy2%JHE!tHfUgvAx-R{5<(=mU4 z$gWb2gA`5lHE>lSSDHKvVWSJ*xs|XCS1IEoc2H=Y)rZWX%P< zu$obqS_E>{rV0la&7LC311RhBt~K+ZicBH3!HhrVKUc zd92bU5MPs85mk#^UN=vGd)DxNFYE6|s~X8=3tw1z5jGnU=|f$+RJ_~@=B6H(B!?x-?0dw*4_EzQt)%l zp_YA{Ll!H?cYpd3od;*lFm`{5V{X)#H}V)Bou>D4uFNhBrnsa$t|pkkfU7YmgVJel zP*(Y#8vtsJl<@T)3|}Jpya4zCb{BtDPP54)6})6hFj8hqu&WCch2%YsN@lzHhmL#& z9K{v!T8cx$ZxNI}jIV2Z5kF0t!%vMbu2FMb7&Md4Ynid63g?;ZSCs|iN*N1Q#V6|I zi$_`K&e@@BqDeEI$EXH228GwO)%X;stMFwTul4iZPZh+G2TN~B%Ip@a5n;xY?3c$7 z9es|B;nVBl4OJORJe$D#f}*!FUY=4+q(IoSe6f0g`+f~_RG=5|*k!;Skg|2x5Y15) zN3CiQlx`!+Y7V^tbFW6B|5}mAOmfszp~e+G#xUhE=(SxyKtiOIM825{wU;bF$Uf~H zF#F2dhg0l>#}KVLwBsf96QL{EV}Q;44NtRSe9e~h_DtY?^NBcwdFXG8BOtYp=xl-r z(@``Y>b-bC{HAEork5_fQPb;z0p3-Uombbw_g3ses3b`lFDrZ+TZone{qm76RWhwiB~D=x^urBOm-+K_)#F_sM^l2>U6 z<7*8P)_6=8Q4&X`A0j1{+P^-Iv`ZaItzXYZbkgwi2)ilG-hwqk`R#-CNmv)U903%& z`fTnSs|MFVyVxplna*xhLb*dHPCM=H5gz$=cq`gBl&Js23e%q$;fI54a4j9*xf5tD zahug@k4jcV6fnJr6)~6%O%_Ztn2%(zsHqj^1y#r-d>h9P+OXY~hIX&x$LymS6hbgO zGNaW^i99SbprTFOpK0P2rr0NxBvF~)u`(5~&MG?7E!LLHYM2zBip$XBX}d{^lcmx; zT*D9rv?xv^&ebQ>TMr(@kg>#ooZm4jAf?XiTj%2_d12yDB@kWhb z8SdW%NGgbE@+C#ZuOuA5LI^mVEMbvFk?+JGVT0yLm5?I0UCiFtm4#=>v{u4($9Xm` z*>!yVZrnTqbi;N8q}Uq(^K#ZaFw%7}d7dtNx`x0W)JR!#Sdtcp$PE%8K^pc&IG(;S zR&R%%p|le2hT6V}qsF_p-}nC^!!%7Av;?Ctg-{OQXJBG0^qwM<BQrzXTV)8NSAA(&;#xm7_QmA?e>7H=6%&N4k@IZg35kH3M-fP2z(39x+dA9Fw z{>hHDatki)d~G`%t5Y?o3R)3O;Rk4RUyd*+*ta4-oB*m;R&6A=t?GW`lDXtjq&ws> ze>jR!Cjgf)SRf0!$)XUxIWFr}2T#Jd#8$vX+qr!)z60s1^dz@$5zv315=LVOFb%PW zO(C#~x`4To<}fE;$no6eu9i0lQ~$_wb;JCWWt^Bv&=l2ss;Njmfh2<<)H_5KCpd3l z?AOvHv^Ae8=`D2LCBS%c8(MLiWiTHsgZhro_`e-1k?kxCBPbs+)ILTa?}N^kT`EuT z>sr(t{&K}JplKzEB2O0cLp{1y@i8EKUzsuOiERC8rK7prGyIdEXNXo&lcQU#oe0(7 zs9A*P9CWWSAY4zF_(2W+>vx6y=}>zm{fo;?zRG`MbS+aqB9aVD$ZVyu=-MCrc!$N| zC$@~S`^E27O(1SAArf%e+5n?Y9|i>_QIwXUiN0$Q%|mZ|hx~Y`J*g5vlNA#6e_H)4 z@h$QknR^ytQkjQSpU2-xSG}i)t`Q7`Tpw26Nu_l7K4OG0!3_=uKCKXAR`yT zx3KN1ch8=ETYRc~PwPD!$yj z*+_A8z8$+#PCeuO#z!`-{*hhCn2*E$W`ghUllHAP#QYPsRf@b0`vi>jzVr|SeW{^x za)+pzm+kukVoWlSysSmYFJZ>f5(nU(A0qo*5>m{abeFB}w_bVVpC+eWdnCTmcOU%lZ^q90AI}DIKRT$hF(6w?E6WgalG*cb zwxAB!QUuOX9G=S){>#Q$!KkQG_*2V$xckue!vREksvagK6kue^iw-k@gvlJwcUrzLlR;=DNWUq+3~c1-fb$~0aF=C(`I=J}G*?x~PtFU&j2 z$I0&#UMeec;pywavPIEJ(yc{?tTU(l@?P!bPXC^xXa@89uc?sTfs0O^P~(*RlsoOj z&6&d98b5IE$FM%0w2>a_07>_c5DoXYKXKf+MrRom*C=c)ea>_R`ri7!LdtfCGq7(z?)_C}GCy-xaid@g$52pzwXILcn0FHV;?5I+wy zWd!&zZuO6Onr6-v>z}$^M7baf0z<}Lc_1w&b<5n53p^7i?n=FyB)sA0n1lCV>N8PI z0NC84WbTAVZ8ykEllIm#1Xub!&{6AFMjw3MPK4vV-`(MpT_E=)FiaXk{?&Zc?4d>6 z$_5o{TLcF!T~SNH|lGQ<8AB`-ZBq1 zLb^`jhJK;<4Vk_XKRG~b=e{XKBkWNiYl`g;S3Pg(dcJK~h4I9F3DOqE_≫x&xT3 zChY*0@3x-4JWb!IlwTpmXp9s>xl(5irmK1!kcmzGub9kH(&SVRW+%ao9k>g>k-x#UnO&$5nmrb1~gy+0yT`=o? z#FN(dIQimp)9f>%A$K<+q=uhS58umLy2v{tl;lQjY@G2s zPV+UQkiW#K%wY}r6mCi*9!qFRgOf%jccA8nq!yr-;nmoM_HPlkENp}nwEhTwdV3}> zxbn%vYd`w6M#;<^ENRMiSlW8e?#86GwVzoZk_4`}l|&Y_|7&k~9Bu-UQbghGnl_vA zcVJ(dZW#q+&@yCY)`ik;>_^Pg*h+zFe}Y=yTK#HJ7!X7=Z>++Yv3OJL_;>uD@K+Q4 z&L`qCBcz}vWWcxsBb8i^dJ2?-Nbf6=b^}j8?4a(Rezm^Sn7{wzVgKRDtd9t_Le+=a z^Y5tP!#0rpIvp!+xpJB*iJA{`6N@FqYwr<6rE~_Xp!^`BqXbVxBa}oL z(&{`=a|5Fn!;n0xoDmmRC#qfQQ~x%Y&W55@I)?_xR?mB1BuW(;xU(>?RDfP+Sa{qK zqD@}vxI6UR`^NQ2m8I98yKp1-y211#Ge|t-`OY<2z?qFNlgZ~SVC;S^$kb z!;mF7aIJeGLyEKj1$L1hw-Q)oY&W(2`Z8pT40zWCjEQTWy#W;81qYIcr^J^ zHn=ri`EjX=k3WEyJNhbFkUQOuK>z4tSR|7vb4Nox5}C)n_PEvp@~#+88(X$6x7 zB=LRktR%op8dy{n3Q{>H{>c6a?vXNJS8gyml4f8Gs-RiNT6pMH2UQgQAs=sm*p2U)`8p zQQ3qIKm)*g6K|#{69r>F`m#CGu*^&R>=cHNMD%CYv)_W37FSJVfsyh3@2^STlTT~g z)9L^AhaG~ceUwGA&iBlMYv&h0HFWXg&i8 z7JUm?<8)=4m(SvPA}M>>8ece`{ndI;)0n$!ji&k*QLz-6NwMLDG~}ZJ5|qorgfK|Z zj!!{v3681@7&IZ)jYo7vKSzE$BeA8>XXdLs6Le*%V>UBxjeGlMqxY{fskUbOc>ORA zJrQ5dJ&{+AR>$@^E^oL+i@!NRlPPbjB_alwO?I^{uIzbCVCvRsxWnOjftN0iG z69hcGEjlYG!2B~GJJHH?~l;5L&oF0IX=9k`B9Gv!-L`tD%r0Xu$IqR=J zvxcj^P0Hxguy}gzqri$C`5h`KoVaxnzoDMpwPCC36_6 zW>j7OK7VDbqoqn->*Kl>X&&equE-)Q=5xTRtn?(80A*~yfQUcj>K5?*Hp(H)c<#B{ z=XR(muJl0R%j|CF5y9;m6WRG4kA~g{b3G|CplYB2nmgQy@M46u3Bi=1v3)(< zM*>vMgvT1_y8Sg(END_TvDOUR1%C1a`>(=ZK=C0t{58-J-z+^$S{-)KT({UCsagZv z6=e@u4QHrXAN0Rvt0z?IA>>ACq>tzi$0#2(6QLj5cIc2m{E}$e7Q(E@w@NfgAuN#M zp5j=kN|Qf;!@6Csf09iZIZ(BMCRI@TA@+rzl9wd-uR6PWEyZ<`@jS9Nr}$f|?w)*C zQpej-35~J$G%m&;cUe4C*@G?cB7Fkm8?pKMIX{<@_00G)quc0{NqLQu*TaL;E4?K${;r^VmJmpRo0Cm7|#=Y@_h%pikZ%P^{c%5hon$&~HL2hahhe2~h9Ua1; z|D@d%cjsus8Az<)qfd^Abq;IbD1vuMKW^11@j)Dpv*!Ib4H8(^N%E- zzi5=a`2Xs93#hh&=5H8xD-K0l+$rwvZUst_;!->m4GzI6UZ6mN7m90x6)hAC1xk_N zg%mFYw>N$M&pF@uUO6Y{3l|{gYV`hFsgLzQ7=ZU75vKOZzVqLs%ug3J!@0Cdm&LD2%5MV~7q9XSWd|lj#p6wH4b2@uhmr>YRc?>b3>;QRFWe`>3QDQrCc( zv9`n|!@0QN_ULoTP>a@!NfVLvXOQ%I5pu=12u1JDBGvW)b$oONe8!|A{%frQ2I=_a zmcNs}z4=xLU2?#zAq6W`{c*79h9RO6nuTtL#lh(#1mIHXQEhr4zVGVYc47#rdh?2b zh|$q*ZzB}TzJlsqK=&YcJ;O$2S_rW5`m6l6WnF`{078nm_CQURhwyq+F(i?S0RyA9 zO4-fa=|GZ~>L+)^b)5bHK{**9s>BD#6q7LoUfG(W4+)2izmzU_Wv2>zjkNjfn`PW z({CxD2Wi0Hv$Yf0TIXd9i9)3;fE2EB2?H`gK}QflUR0q(gXyN@zhqEku|`epYf2<| zGaJMQKO_w549GeCO6qVw0<%s8$?wF{wXqG>P*k;dDv+8<9 zgHMYCCuUuuL0A<%y?@uKy1m7dq%lj}JBbFq3Dr70>FJP8=V1g$drdHN8e8VMhw!W7+$}uJ(F-q5b2a?1Bxf z89Km_j=eY%wFXUfNBPzy9PL_E2^n)UQWMI_`%+u@1x2r_cfK#XWP;|deYcGh5Q>y% z_a_nn$Gi|yT7abUrBp|U%8(6e&Z~N(%e`MCZ&1$ToWy8L6o3$htzpj;M+s=6vL`Y^ z(Gqfug}LyP&e;L6pP(f7;ctec-s$d7Zkd>RY4l^lI!yibtU3ilC=l9V{CN#3$256Z zQHRXI_(1M&M;eIJy^f|n$}}spl0yqM(!&`QEiP$9!TiTd#HBmm-s8)9cM)!6K#QLk zXgWGPVES2#aZ=6;j1|VCBZXEC8smWPU7_~Ta!t5OphBteEWd>MJ5i#cGt5aY>D)}g$NhFIcjRBy^2YB9sH?Ld_=tIAs&-eW@kf0nnF_30P^p{V-?vVko zTU%Or|7$KFbXFHr&Z14sk(q>|gE__k^sqT03D3`^x5u`&{MpM}v2)IN_48J-#A6x- zECbqqOPYK5_v{ThLMF7+tH&H|ZuLMGPy&)CLjrk|lF>T1iYptviuBNXEZVSf2^mSK zZ)#3Z&)E$N44x)hK@x&iYld5wj|P?TYj}ClZjUSXpSwb^;Q4b>wO7ge zZrr0UPF>B_-cY+uB`0p;5CIgtm7@gA4>zcoqe*Xu|B83IOr-nVaK`|^O%ddn_$;2K z$VphlBjFw2_18t&z|=|<4S-MPAIuOvM9Z=Ca3@2s7=47epb3GbKnBVR zpLNd9$_MYVGz3Yv?Hoh9nm(W@=E5K7&|aAgu#TcL9!cIxhDojuYZD#y!%@&Ii6cRIrcx+@&i9(n ztBNZ_;_J(GFcq&(oU)9xUU2Edl(G}tnPiGW>6xKq(WeiwZKG*w_2rYT-U}4FoLkQ( zAfTfp{I$zKq`3514Z#N@>9>XRP0qJXga#iWS!B=14~aWvi~?;}m&jFo7UvTj5Rhen zeXbwRjVEG(rnr+CisAVT7|*2qlvQ{24EDxE;-z_8Q4sg1^QjkWbqa5}Kh>g+&SiZS z0QkZ`=%DSmI69VUdrSx*ert{wgB(&OBxMml#6F^$d~jQi7*2(S{dL=81pRF^@z<(4 zx@BMT=AgaO^gdXX3J1RGgByLEEF5+K@D5Z#Q-L_z@=nK zC`66vFF;3wiqm|5nq`UGOsjW`^IKkMB zJ{FTxPb5hlz3Zk*F|@9T0`+do0x*`v>t@eka~)I*REmj|37=zDa!_aO&>0QH+v)ve zw4ZLnxU`*^$2=G)Z1PkB>v23Yj3z(!_gpE?QVTsQi*sUCvW|c1cL-Sg2UND~MBW^M z{eP;^a2;@>u;4HiQnGp7FB)ADtaJj{j^F1!^Us1HmmRA(&@{;`0w*qZRxNI}+!up% zcUL_)j0EnTm8{ERUF-HW74LmDh__z&!7wRFpwvFGB!xUq^sOi_2fG=VgSat0rPgf| zqn6=^u^$FejCj(Ea&(Ig$5YzrXe=dLTKqsYA=fnXG}hOuIOHis`#)R>;T5HSnMRr+ z%OV1;1P|79QFoNTl)xSnU*E-hIs5#2V7$@%eP~7Y3P$6W4VG$w9j;@^eQjFjXT13x z_*2PtqK)bi1NwEH5Q!#LI6QLB3Cj_0qO^r@(k}HtZ^I%t@#LJNYl9J}Wv04MaT+$t z&d(M=5mK0VL-8|(u{I%=-+ir~#%A-ug~1tTl1RJ(g+bVoy}4ifijFmQ=&KUwhimrK zmmfZ2q&*6usW~r%J7rejG#%}#z*rJna14uj8{3}2$8@jk)a8zSc8!&HC)hm+K{s2M zeNiOZPk-wtwQ5nRBna$DC`|rPrqMl7VY)HcEvvP%vJZ=8vQa+P3BoZTT!UZfXmMw5 zUm)JyeKj^ddin{4G~>mvj?}rj!$wA`U^{s|4ws{|XzaaHN{_o9q-eMyXyp;{|D#(j)%+s9SYZ)Y$;lK#nXj&(>!4i8_HE0(@yz z+0Q=tfsRTVZ_zfITXr>j5s7>Bs9e%NUGx;C62(Av%X|+RA86diZ^QlJ`>EjRw!Wy| zMe{pdnWX_KQuuVhCCr7Wr}{&2-KI#uf2_1!LSWX*2};@7UUx23UOAL`6^dwyvnjvb z3DzhClz2g=QKp<6@{4j6bnz=;1?T(!Sas9XA+PE-a>RFa`hyTmH{B3kRZ(N+23@@~ zOQ8q=LMdm!!%TJk6lNQr=4rIa@x8MmjC!FO_MG)VnnG#^tv_XlGlc}GC;Q9qgz4t- z0G^y#BS{vyQ3=84(&wcRV7+>1_@el_h>%y=N8I(n0TFhYCs1Be4xJdi^^ z2J>sis+-ujEj;rN?E$*HTS=yhPGkuQ;}dg=Kad80oyxZ^UYSh2^KuIhC#xQ~k=ZX? z#EVciDrJc7;On=5KM4*qP*~q8-Zdrh1_R;~c;YiE)8FJ08?u?b5?h%2uPWjqW?3F7 z`J!S_)qU2B1bOciWk)I5sJm3UTW2#k>(u{B{LVip1Ry6pq+}i@RNID;w@Q<$qM>fu z%95xlU4ky>Eyk{Ep|5P~;bR3#8^oc2(+I6O-e0EIYkE%L|C*;F)#+- zllzP0&@kRlvkP_Q&OI6u4T3)&)(1}KLSaVBx-xZ7w{ao6A6IJj@DU!wUR$s}FZO_| zK;BQ5$1@_@lbjx$Ao+)UcNLngW^00hw;4NA$Mbl|=BC0;;}$-3uz{q+4^D7{NT;Vt zM;~oRl|kh5Lv;H<4#0}16^yjs<&iwl>8QK+IqV7kXXR}Wcz-)oPaYju(cdG33>OFC z>BF)vDV$%%g-a5pZ>)BkYpdizD5&ksv`Cd`iahz3Y=T&qYHpJ#U<-6BRw{i5aqlE! zVI8E5P#ff$Cl0g=1?9*7wO&0D7rf7wfq7Yjg&u(0QxNZ^Ao=patH!$sl(xWZseXlr z)NePG15iu5Mcp3%<5!aOzU~b6NO|Lfw_6?~4~VY}RYeVT=5dwBP=r&Is>OCYEcIlZ z2K|GDa)Xr`Jqj#mTBQSI^F4!3-~#59Z~hX7&0a|$2UvT=x+g2R*eP(?gH=rB!;*Qv zyu{x(3B>Gy@E9qI!_54Wbx7~~2=qWgvAr=mEltO*R0v-??LbQdS$|iO16hRHX+SPzxi>AcEhVMW|ftcR0-=ylu-JdmWsnVwt0`q6P&yeEU7gy z7D^)!WApOf1gv`_Dz0T5grI<#ByG$JngHnmqprJi=b2KlVJGFAI7j$3&#qEO35Jg7 z15S}Y*-mTEQOTG}NhAsC*kOsbzy933L5-qzp%Q$IiR%S{l0gsR7Mw`s8hGa;(oJB8 zG;O*n-Zz1lE|`o_ne{gT5UV$Qjy^%@f3~-a+*9lkzk|6g8 zks^p~VMHBP*AxY8|0w{At-3HTzW4jE?O-%;L9IM#~H@3zih$I9uro49^^6fDUHkjspcefhanu&t7wvY^)hr ztbNtPDvyMS$g%V!TyTjPO=CCIi2O(eS3BG^1ZB$ug#qi^R(=DSTJ9qZ)GtN(cTHg9 zJl8M(hWNwf0HpU?u&Ui=BY9%DrUpmqnaPm0FNp~Z^1#sp?02d`&F2pw6h7&g*vCeI zPUDD^mU^Tc!tjS@Jp7tUqWA*hk0 zdTcp@O-Xs>?wH;KMgz2B{CgC5lE72VL$vk5)WnhJFv~nU8U@TL_n1%*M9E(#&m6SK zd!KbF>9+6g-F1rT2-}m;$n4Sw4X?Xt0G9N|;*FNSMGqD32#smh}70rD*au zS%Wuq5~$gg86LFx2Eoi7Y1y4YtWgA{b*7bWeeBe`fE{*Z?xlk6ZHFn)C4qY+Qd_5A z-m1mi73y*4o)05KVwO?k+Da=UE7x;Q3~`N7?rxTqO#vJK*JlG_Ef1*f|Z z%MHVig7T?$f36lf!1U~mq{3={V;oSF`&qd8EJw-b@&hEpjNe7~0`K(A*O_7-Z2aL1 zye^n?y``Vt#Qodp_DUlQ2+!Ks?MsuM)<6}A&4=^`FC!Gq232*nSBl5~nIeE)@b2r! zP?fFt+!45~z(foF35rMx7eh$V+?VMVv{R1r5^8tiL^gl=H}>6Nvp!sBFqimW``a~K z>^iRt?D?_G<%SB@p5DkG$OI#%qnO@^ZbPPPw>PS%+Mda%Rg}Ln7elMQsDpf)F+@NV za6|A^j&XbB9ZI;~XuU#`nvJ;{H2*2zMZ0eP)b_=-sg}$NM8bhuVQfi5R>kS{bcY8p zLo((}BY^=X%~)u!PaKt-2Ur(JmsBo;5kU@VDc>g)vOec*ei=v^Q-?&0k+WrzgSW3E zj7ptTt%tNT_~ej*(<*(+4VGH!B4r=NYSgv9zj-M)=5JQtmZnFLi6#_CIXX(tUTCnL z!?8V}5^*LAz%uEtQ7Rp%=CR66auFbz{Iu{Xnfm9aIW|TEr}5&gE7k-dR36b?jPg zW{8=H_A{9I4Fj+na4UH1jR=S_*138)#ZPnB33&eE3GwS%OKSUeft0jQwKXC7oYftEZn zf3>g)E0}1*iGXCuZxh3bbj#LYrG*J$yTM^-x0aHuG5BTiNA-b-u({m<}R?O{UHyv@hjD zZIqnJ^9KQM4FV(RM{fXC&kw$z}6mW)^ zD{kNnhVr+li-y9%=43A$E1qk#)5oC2-XrS8-kgXnn>doh#7ff}ZcE=Y#Zimz1ZZb( zhStNyC0Wtj8%?;YPWUHZM?aq+_3Hw{xLfv;M1UMB@HGvCGL*9_Q_^jsx1-pS3L8Ece}MbaEi>31ONr53PQZ$hoq~Hp2p1FgcFW@F1@fCDHFA zOH*$ewHr2R?0mzPxt=h(0IPNJ#fN{IFpxd=)idLImQJ3=7bnQz#`rs(lyPUupN$H7 zPe9c9nq6-c);L-&J)ff)oT|A-aWMRh;&$Ds^Uv$KtwQh) zIn*Z?-RwN#@6r>-48)4*9BtWf9ED&aV{?Vw2`7>8tL^4YWdgZyMdgpk+hL6UvioGz#jw_kbyUoC0*fcAFW818iVsvHZ0voA+@j2Qx5da7F6f1V1y zI*KvKRi!Aheo689{V)0uv&K;6j+N<(!HXe{4E5~EjMEGQ%$+F;nK@W(G@L@3k5lXF z;3L_;VkVqQ`g8T^?t=uADVHRQP+CU>RF4w9nz0B|9w8I)ZACHX)FlBR;wetJ5|Ms< zgM%W(bqOMa<#ew^GSA$tQF6BlN0_0$^oe`oZRDcpLr^-#5JS?3oMX8o#E^UVb1)CY zJjK=b-lAnRA`Eo0Z2e{ye|y~Un}#fU0Y$tIf8GpIL1BxLu(&ClW=Ki@End+W1A7I5 z+-7aWSZC~%Yak4s`Hl4d<@BiN@P7f4`!Ajx)w=`FF;6la%tl8=CVeK6?#Hfn#pmNy z9G>uZ2fw>!hZd6~GGb=~tQY>j4}48*ijv_s>ge0zVUn;xUZfbw2agQE%tm5N|LB%agl3mG5;$37>iJEXrQ40 zl?VA=8e79q`()m6#iUWV;}___aKbn_=eNi>^LLTjwW42IW>!Du=8;DXaFCO-S$w&QiL% zz74p}XO3%^bl0caqz4;*G_i(X< z@A<1$SmjJxoB_RcM34I8lVl7t5D+;j^5`2 z^lGlOJxcQ=Cr%LI+xNAhg)!poA8`y_`j!~h5Wst5vNA}l$M!fYX*?-C9O{xXo&MLm zH_csMAe+rCkJ2+xmRZ?-S??~}wBeyure!ykX^(FDoH>kQ?U&R7qsALr0vYpcLL;){ z$9k)g3}0zL;na&)>^|WsJ3qDM&UZUUf|m3KGhaZ`&+;pcQr&Pq@Gp)wFS)`F6ctW1 z{)$_A#ze=-*@w!z+cic4aiQT{g_HAX^4!H((z%LB?yM1bPJzk0CStWR`L4fMb! z&GDbA?Dxh*KYbU|4rq20O7#oY0|SDixa~s+nWDo#aLRIMZK3eCrveYbBKcnWIkpLw zt$NAou;)wi$LuwgL%chAU>P-n?9b@B8i$h3uQsgc?sjBn5t)9m=tD{pB#pH0I6!4{ z?L`f+Lx{C)3=`#;T+~qmF*KhwQ61(Yo77}0+Vp`x@q!9b!-Or&zUi-V*!c;^dMLJ! zpsFC=_icRV}8&_1U7C z!tb{DNMk%8n)Ji?5&!ua10dh0JewmJY+q$~h+A8hItM8I1NIDs*$k1d7H;?v|4 zct`n_i^UAVWpPmgZ9nm&Ib6RCp+CIx(ZZ1~pj1F-j0?Un&J3Mcf^(l=WcrZjhz|2f zt^7zKEUj+%(>c++rMw#`JufQtL-pM+C6}LSuF$k2>^#)Qv101Nnc?-VGW6t?mu)Xw zU#MLzt6+nxV$pW(2kd3M5bE+`ocEad7w@gJFI@W*E61ATHFD2Xt@=OffHp=sx0=h+ z@VI?pNcfgr1~n)ACw(mOL|( z-u}-tTd~u?+L295`-cs(z+6F7Z0VtwurXuS5jX$h9u*~mO8jM``Wid;-8AaAbXi#Y{Go!)UYfByvSd1HX5n#A$fyvKfaDvP#xAer?YX6LXL74Eb z90KB{yI(w_@|ybf4cqJpHe>b^UE!~{#3Dc5+?05vSYu1;Dxkq%?{CsuQU!WrGK}+6 zTN%{94K_JfZlGfM`$ELjqvyqAqQcvaV;}Rbt?YZb*@HZF9=PwM$+BHR=2N&$&1S-9 znxn-(Ue0x#l`ryt|>k=q=NtdQR0n(UOHX_ieyDInks+PA)AknVixv+T5l=BbcBw!wBy9O z1g(Kz?X>>4WwBfCi|Y@)*eARNC0TVia8>%Ilm~ivV^gBmP=VAi}Ju=_ceJ$)>XJ}NR-co*9*Dx4O&v1P;&7^!R534YSJM0G4}2P z+J=uaMGso^&CKL!`GvwG%XVA8NlNJIf;Ef2K>+#8i>B?nF42zLf#@Y6-mP7q>x=tl z|IpsXI_#(?{Vn@zZt#*O?Q;|^$~#3=gtlQY^_nU@+~(x9U+1e@)NHxyHzEamn%R8; zcCQVeVg$WM|3I5OTBBA>;s-}BS1teiM^4H$F0(8 zte@A5)>BcJ2p}VDG{6lkWiDOKW0v=OnBw~RaG} zoyH)%(UX$yF3?pUDJTOpSQHW-sKM2v-pq@fblVHRep9v4Ww&o?I{)4gk< zs&c^@!2`wc%Y^z}873;Zu;X@VADaaIkr~j5^bG$rvzyijoT=ne0%m8MjU>dVH)rWW zt14UZGu?hc)s8#v4w(ft(zBY1`~<)1xlff~>7NqD z@SlaMF3zFkax$1Rg2gidtGw-AE>*-5Jp!4Zj3uXmRQ2X63TlUG6vwDWjuT7A8SFyK zye=bj{J`-~|DeohfF8nB3^am1}P^G=?Yh=pvu{XIO&EKi@ zne=IBDMGEBHiIMN(=zZzCo`Wu{!AG_Z+%5B2Tivm@w5iCDLhZ@@rvYJlwy znwdnuKmMsFp|YQK!zNtw17?J|w}fXh*1}bB$rwwIF%!aZRM$vRmwnj(EsxZy32M!A ztM-{AGezUDW|(P!ro`Ne$h?HraKn&jM3Z1X4|!s4N4m@DvcXg*8#L!e{4~@8;OVU( z0R1)9aX|eSugz@@N4au++8Jn0AEFxP4_hh0DA5KdRTrdp5i+Yvd>)IuipzOh1upKPV^J<9Y3F zWODkA=f9N-vo^Q)g1rqCkM5*T zliMi?08}5felb>8^Og#pj*lKI&Fof`Ba17_mqLsdjo;+cT2mD9$t2X58(8LrZU6B7 z+{gUN$Zfr(lq2{oIFGmVd0){=O6@-MQs7E7T^&(~$iC75_@4yV3Q`t9za9{zx*gTw z%_bsEOpAl|WB&Y)xH~x8Tqn+k4PlnZPRX(o$xY+12I`&-Qc+b@#8bwajcUm#uDs>3 zBC-}S#3~^4?}?Kkzo+jhM*KOsGx^b2*taVA|$4gOCDNgEm|X1 z%B03e9Ktyg#NL{gV&1d{g{MzxwS3OuIhjG*ia!G zrKIvLwr#vWk|nK_@I|XF;qYwcnR{F+k@6+&t%1PPg;`bkY7G*_qln)R`b4qs2&GAE z4j)c~0m+P>Jj;RUtwAyrZabZ+`4ZOYBS~f^ytE9H?l{}EY*-b3eeS*b zV25Jom=CPtEJ4AhKhP7kl5#fgzA`c}G$@V+dv$ph9nSAocS}l)CN187S&NeTB{q0g zXgO;a3rPEi@xDu`FNG=ED*=NW;T_{O+Gqp|L@P=l=0)H}9Ubyu-|NAK`)a@uL{QO? z7X&xL;AGK&2pT{}m56ZLkZszVr3-fC-7Re;TER1)FKh_VbvmM@y3d{8Y2O zXFsvh(i;8G&`Hy&JtE>efAQRu@gqJUcWv4Ev+mgX#)Ysnn0bAzGtNpQ+H1y(d^bf{cdL(7c3Esr8+B8FBM| zWba?nixWZ1OYp&>JlJzpfFu$e#Nx^zn^i95@*rhy5p$_W@U+EIQTgQ&9)34=`nXv|pk( zvy)Xy5i63c@h+GFJ@nbF{%<ap@(kd|PYr&#D1kb)mKmcek0Ma`Hg}$=!C)Rc zIgfwFuCzAOmmDK6G96{9SmHF7_w0v&FAmGYKn{aUmMv zasJP*cFs)QRFT0PofT9hqISiM{>ZHDTsK9vV4EK^8E#7?jmei@D7TJx1<0awnLiTW zVDaByVgC+>{N8JY57Di?m(vYqu}wqwKC-3orXQW=yii9-;yxk1I`@54 z#p1ob^GjY#QuggYwE;)%Ob`g>Qp!&7+LQvI#_$60))wwCbhGl)lKS==-4FszF*-`THP zn;08GpTaFB7f-(37mEM<`cwK$$1LWJoUQR&4(}0Pr6lvKbV;e7XZ$Pnti;yY1P)0L z-vv>T`Ux1PR%K#ejPd0NK0WPQFbkjFon*VH&J23iS-xuQ9X{fE79ds7`l~FFaf9~Y z+bnDY=zXRd-jy-vsnJDD^}lU}P{oIKT+n~=V?{soUBo{5KP&tn6uTz?YTX5`0Fn5A Ox3tvsRqK`QqW>3{!N9-( literal 0 HcmV?d00001 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md new file mode 100644 index 00000000000000..ffdce53289aef6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/README.md @@ -0,0 +1,80 @@ +# CUAV-7-Nano Flight Controller + +The CUAV-7-Nano flight controller produced by [CUAV](https://www.cuav.net). + +## Features + +- STM32H753 microcontroller +- 2 IMUs: IIM42652 and BMI088 +- builtin IST8310 magnetometer +- 2 barometers: BMP581 and ICP20100 +- microSD card slot +- USB-TypeC port +- 1 ETH network interface +- 5 UARTs plus USB +- 14 PWM outputs +- 3 I2C ports +- 3 CAN ports (two of which share a CAN bus and one is an independent CAN bus) +- Analog RSSI input +- 3.3V/5V configurable PWM ouput voltage + +## Pinout + +![CUAV-7-Nano_interface_definition.png](CUAV-7-Nano-pinout.png) + +## UART Mapping + +- SERIAL0 -> USB +- SERIAL1 -> UART7 (TELEM1) +- SERIAL2 -> UART5 (TELEM2) +- SERIAL3 -> USART1 (GPS&SAFETY) +- SERIAL4 -> UART8 (GPS2) +- SERIAL5 -> USART3 (FMU DEBUG) + +The TELEM1 and TELEM2 ports have RTS/CTS pins, the other UARTs do not have RTS/CTS. All have full DMA capability. + +## RC Input + +RC input is configured on the RCIN pin, at one end of the servo rail, marked RCIN in the above diagram. All ArduPilot supported unidirectional RC protocols can be input here including PPM. For bi-directional or half-duplex protocols, such as CRSF/ELRS a full UART will have to be used. + +## PWM Output + +The CUAV-7-Nano flight controller supports up to 14 PWM outputs. + +The 14 PWM outputs are in 6 groups: + +- PWM 1-4 in group1 (TIM5) +- PWM 5 and 6 in group2 (TIM4) +- PWM 7 and 8 in group3 (TIM1) +- PWM 9, 10 and 11 in group4 (TIM8) +- PWM 12 in group5 (TIM15) +- PWM 13 and 14 in group6 (TIM12) + +Channels within the same group need to use the same output rate. If any channel in a group uses DShot then all channels in the group need to use DShot. Outputs 1-4 support BDShot. + +First first 8 PWM outputs of CUAV-7-Nano flight controller support switching between 3.3V voltage and 5V voltage output. It can be switched to 5V by setting GPIO 80 high by setting up a Voltage-Level Translator to control it. + +## Battery Monitoring + +The board has a dedicated power monitor ports on 6 pin connectors(POWER A). The correct battery setting parameters are dependent on the type of power brick which is connected. + +## Compass + +The CUAV-7-Nano has an IST8310 builtin compass, but due to interference the board is usually used with an external I2C compass as part of a GPS/Compass combination. + +## Analog inputs + +The CUAV-7-Nano has 6 analog inputs. + +- ADC Pin9 -> Battery Voltage +- ADC Pin8 -> Battery Current Sensor +- ADC Pin5 -> Vdd 5V supply sense +- ADC Pin13 -> ADC 3.3V Sense +- ADC Pin12 -> ADC 6.6V Sense +- ADC Pin10 -> RSSI voltage monitoring + +## Loading Firmware + +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled "CUAV-7-Nano". + +The board comes pre-installed with an ArduPilot compatible bootloader, allowing the loading of *.apj firmware files with any ArduPilot compatible ground station. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm new file mode 100644 index 00000000000000..f46b1f17c1a3e4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/defaults.parm @@ -0,0 +1,2 @@ +INS_ACCEL_FILTER 10 +CAN_P1_DRIVER 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat new file mode 100644 index 00000000000000..62d0bae7107ef4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef-bl.dat @@ -0,0 +1,101 @@ +# hw definition file for processing by chibios_hwdef.py +# for CUAV-7-Nano board + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 7000 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 128 + +# flash size +FLASH_SIZE_KB 2048 + +env OPTIMIZE -Os + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 UART5 USART3 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# CS pins +PI9 ICM42652_CS CS +PH5 BMI088_A_CS CS +PG2 BMI088_G_CS CS +PD15 BMP581_CS CS +PG7 FRAM_CS CS + +# telem1 +PE8 UART7_TX UART7 +PF6 UART7_RX UART7 + +# telem2 +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 + +# debug uart +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# armed indication +PE7 nARMED OUTPUT HIGH + +# start peripheral power off +PG4 VDD_5V_PERIPH_EN OUTPUT HIGH +PG10 VDD_5V_HIPOWER_EN OUTPUT HIGH + +# LEDs +PE3 LED_RED OUTPUT LOW # red +PE4 LED_ACTIVITY OUTPUT LOW # green +PE5 LED_BOOTLOADER OUTPUT LOW # blue +define HAL_LED_ON 0 + +define HAL_USE_EMPTY_STORAGE 1 +define HAL_STORAGE_SIZE 16384 + +# enable DFU by default +ENABLE_DFU_BOOT 1 + +# support flashing from SD card: +# power enable pins +PC13 VDD_3V3_SD_CARD_EN OUTPUT HIGH + +# FATFS support: +define CH_CFG_USE_MEMCORE 1 +define CH_CFG_USE_HEAP 1 +define CH_CFG_USE_SEMAPHORES 0 +define CH_CFG_USE_MUTEXES 1 +define CH_CFG_USE_DYNAMIC 1 +define CH_CFG_USE_WAITEXIT 1 +define CH_CFG_USE_REGISTRY 1 + +# microSD support +PD6 SDMMC2_CK SDMMC2 +PD7 SDMMC2_CMD SDMMC2 +PB14 SDMMC2_D0 SDMMC2 +PB15 SDMMC2_D1 SDMMC2 +PG11 SDMMC2_D2 SDMMC2 +PB4 SDMMC2_D3 SDMMC2 +define FATFS_HAL_DEVICE SDCD2 + +DMA_PRIORITY SDMMC* USART6* ADC* UART* USART* SPI* TIM* + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat new file mode 100644 index 00000000000000..eea49e50f0d618 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-7-Nano/hwdef.dat @@ -0,0 +1,294 @@ +# hw definition file for processing by chibios_hwdef.py +# for CUAV-7-Nano board + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# ChibiOS system timer +STM32_ST_USE_TIMER 2 + +# board ID for firmware load +APJ_BOARD_ID 7000 + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# to be compatible with the px4 bootloader we need +# to use a different RAM_MAP +env USE_ALT_RAM_MAP 1 + +# flash size +FLASH_SIZE_KB 2048 + +# with 2M flash we can afford to optimize for speed +env OPTIMIZE -O2 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART3 OTG2 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# telem1 +PE8 UART7_TX UART7 +PF6 UART7_RX UART7 +PF8 UART7_RTS UART7 +PE10 UART7_CTS UART7 + +# telem2 +PC8 UART5_RTS UART5 +PC9 UART5_CTS UART5 +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 + +# GPS1 +PB6 USART1_TX USART1 +PB7 USART1_RX USART1 + +# GPS2 +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +# debug uart +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# uart6, RX only, RC input, if no IOMCU +PC7 USART6_RX USART6 + +# ethernet +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PG13 ETH_RMII_TXD0 ETH1 +PG12 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 +#PG15 ETH_POWER_EN ETH1 + +PG15 Ethernet_PWR_EN OUTPUT HIGH # disable power on ethernet + +define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_RMII + +# ADC +PA0 SCALED1_V3V3 ADC1 SCALE(2) +PA4 SCALED2_V3V3 ADC1 SCALE(2) +PB1 VDD_5V_SENS ADC1 SCALE(2) +PC0 RSSI_IN ADC1 SCALE(1) +PF12 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(3) + +# analog in +PC2 BATT_VOLTAGE_SENS ADC1 SCALE(1) ANALOG(9) +PB0 BATT_CURRENT_SENS ADC1 SCALE(1) ANALOG(8) + +# pin7 on AD&IO, analog 12 +PF3 ADC3_6V6 ADC3 SCALE(2) ANALOG(12) + +# pin6 on AD&IO, analog 13 +PC3 ADC1_3V3 ADC1 SCALE(1) ANALOG(13) + +define HAL_BATT_VOLT_PIN 9 +define HAL_BATT_CURR_PIN 8 +define HAL_BATT_VOLT_SCALE 10.1 +define HAL_BATT_CURR_SCALE 17.0 + +# SPI1 - IIM42652 +PA5 SPI1_SCK SPI1 +PB5 SPI1_MOSI SPI1 +PG9 SPI1_MISO SPI1 +PI9 IIM42652_CS CS +PF2 IIM42652_DRDY INPUT + +# SPI2 - BMI088 +PI1 SPI2_SCK SPI2 +PI2 SPI2_MISO SPI2 +PI3 SPI2_MOSI SPI2 +PH5 BMI088_A_CS CS +PG2 BMI088_G_CS CS +PG3 BMI088_DRDY_GYR INPUT +PA10 BMI088_DRDY_ACC INPUT + +# SPI4 - BMP581 +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE14 SPI4_MOSI SPI4 +PD15 BMP581_CS CS +PG1 BMP581_DRDY INPUT + +# SPI5 - FRAM +PF7 SPI5_SCK SPI5 +PH7 SPI5_MISO SPI5 +PF11 SPI5_MOSI SPI5 +PG7 FRAM_CS CS + +# SPI devices +SPIDEV bmi088_g SPI2 DEVID1 BMI088_G_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI2 DEVID2 BMI088_A_CS MODE3 10*MHZ 10*MHZ +SPIDEV iim42652 SPI1 DEVID1 IIM42652_CS MODE3 2*MHZ 8*MHZ +SPIDEV ramtron SPI5 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ +SPIDEV bmp581 SPI4 DEVID1 BMP581_CS MODE3 7.5*MHZ 12*MHZ + +# PWM output pins +PI0 TIM5_CH4 TIM5 PWM(1) GPIO(50) BIDIR +PH12 TIM5_CH3 TIM5 PWM(2) GPIO(51) +PH11 TIM5_CH2 TIM5 PWM(3) GPIO(52) BIDIR +PH10 TIM5_CH1 TIM5 PWM(4) GPIO(53) +PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) +PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) +PE11 TIM1_CH2 TIM1 PWM(7) GPIO(56) +PE9 TIM1_CH1 TIM1 PWM(8) GPIO(57) +PI6 TIM8_CH2 TIM8 PWM(9) GPIO(58) +PI7 TIM8_CH3 TIM8 PWM(10) GPIO(59) +PI5 TIM8_CH1 TIM8 PWM(11) GPIO(60) +PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) + +# we need to disable DMA on the last 2 FMU channels +# as timer 12 doesn't have a TIMn_UP DMA option +PH6 TIM12_CH1 TIM12 PWM(13) GPIO(62) NODMA +PH9 TIM12_CH2 TIM12 PWM(14) GPIO(63) NODMA + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# control for silent (no output) for CAN +PE2 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) +PI8 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(71) + +# I2C buses + +# I2C1, GPS+MAG +PB9 I2C1_SDA I2C1 +PB8 I2C1_SCL I2C1 + +# I2C2, GPS2+MAG +PF1 I2C2_SCL I2C2 +PF0 I2C2_SDA I2C2 + +# I2C3, IST8310 +PA8 I2C3_SCL I2C3 +PH8 I2C3_SDA I2C3 + +# I2C4, ICM20100 +PF14 I2C4_SCL I2C4 +PF15 I2C4_SDA I2C4 +PG5 DRDY1_ICP20100 INPUT + +# order of I2C buses +I2C_ORDER I2C3 I2C4 I2C1 I2C2 +define HAL_I2C_INTERNAL_MASK 3 + +# armed indication +PE7 nARMED OUTPUT HIGH + +# power enable pins +PC13 VDD_3V3_SD_CARD_EN OUTPUT HIGH +PI11 VDD_3V3_SENSORS1_EN OUTPUT HIGH +PF4 VDD_3V3_SENSORS2_EN OUTPUT HIGH +PH2 VDD_3V3_SENSORS3_EN OUTPUT HIGH + +# start peripheral power off, then enable after init +# this prevents a problem with radios that use RTS for +# bootloader hold +PG10 VDD_5V_HIPOWER_EN OUTPUT HIGH +PG4 VDD_5V_PERIPH_EN OUTPUT HIGH + +# power sensing +PE15 VDD_5V_PERIPH_nOC INPUT PULLUP +PF13 VDD_5V_HIPOWER_nOC INPUT PULLUP + +# Pin for PWM Voltage Selection, 0 means 3.3v, 1 means 5v, 3.3v default +PG8 PWM_VOLT_SEL OUTPUT LOW GPIO(80) +define HAL_GPIO_PWM_VOLT_PIN 80 +define HAL_GPIO_PWM_VOLT_3v3 0 + +# microSD support +PD6 SDMMC2_CK SDMMC2 +PD7 SDMMC2_CMD SDMMC2 +PB14 SDMMC2_D0 SDMMC2 +PB15 SDMMC2_D1 SDMMC2 +PG11 SDMMC2_D2 SDMMC2 +PB4 SDMMC2_D3 SDMMC2 +define FATFS_HAL_DEVICE SDCD2 + +# safety +PD10 LED_SAFETY OUTPUT +PF5 SAFETY_IN INPUT PULLDOWN + +# LEDs +PE3 LED_RED OUTPUT GPIO(90) LOW +PE4 LED_GREEN OUTPUT GPIO(91) LOW +PE5 LED_BLUE OUTPUT GPIO(92) LOW + +# setup for "pixracer" RGB LEDs +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 90 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 91 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 92 + +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +# ID pins +PG0 HW_VER_REV_DRIVE OUTPUT LOW +# PH3 HW_VER_SENS ADC3 SCALE(1) +# PH4 HW_REV_SENS ADC3 SCALE(1) + +# PWM output for buzzer +PF9 TIM14_CH1 TIM14 GPIO(77) ALARM + +# RC input +PC6 TIM3_CH1 TIM3 RCININT PULLDOWN LOW + +# other I2C devices +# 24LC64T eeprom 64Kbit, address 0x50 on I2C4 + +BARO BMP581 SPI:bmp581 +BARO ICP201XX I2C:1:0x63 + +# compass +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_SKIP_AUTO_INTERNAL_I2C_PROBE +define HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE +define AP_COMPASS_IST8310_DEFAULT_ROTATION ROTATION_ROLL_180_YAW_90 +COMPASS IST8310 I2C:0:0x0E false ROTATION_ROLL_90_YAW_90 +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 + +# IMUs +IMU Invensensev3 SPI:iim42652 ROTATION_ROLL_90_YAW_90 +IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_90 + +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# enable RAMTROM parameter storage +define HAL_STORAGE_SIZE 32768 +define HAL_WITH_RAMTRON 1 + +# allow to have have a dedicated safety switch pin +define HAL_HAVE_SAFETY_SWITCH 1 + +DMA_PRIORITY TIM5* SDMMC* USART6* ADC* UART* USART* SPI* TIM* + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +# enable DFU reboot for installing bootloader +# note that if firmware is build with --secure-bl then DFU is +# disabled +ENABLE_DFU_BOOT 1 From 3267b9116009abdc284b58fdf037c2dec0ab9c5f Mon Sep 17 00:00:00 2001 From: cuav-chen2 Date: Wed, 3 Jul 2024 10:29:53 +0800 Subject: [PATCH 57/77] bootloaders: Add CUAV-7-Nano bootloader --- Tools/bootloaders/CUAV-7-Nano_bl.bin | Bin 0 -> 40236 bytes Tools/bootloaders/CUAV-7-Nano_bl.hex | 2517 ++++++++++++++++++++++++++ 2 files changed, 2517 insertions(+) create mode 100644 Tools/bootloaders/CUAV-7-Nano_bl.bin create mode 100644 Tools/bootloaders/CUAV-7-Nano_bl.hex diff --git a/Tools/bootloaders/CUAV-7-Nano_bl.bin b/Tools/bootloaders/CUAV-7-Nano_bl.bin new file mode 100644 index 0000000000000000000000000000000000000000..60b99e1b3d72541dd578fe84e36f0e053e4636d9 GIT binary patch literal 40236 zcmb@ueSA|z_Aowk@6Ai!Q(B;Jz)jLZ+VB!uUMf`yX?iIwP+r7Elr^QWo6@2wpqABL zQeM^qt^!3DT3~@Hg1D3v>RPY{L04VZ{pM2GZ9&%}LTuy94QPZ>N1;Q1Jycn+y& z!LtUQeeiq$&$sZz&;HMNyMOzi^Zy%K3C#89k@^vM{x|rXd)ggU`7gNcd+L+{%szNN zfX5HdsN8?S{U^LRP+kVlf&T+s&!(T^2&w#O-haaX-{jr=5BUF^djAc~r_cYtqw~LM z@BbEtYC{4N2fSsEXjL&KT?zY_3)I#Gv$|vZG*XS?=zuYD46iA zX8U_$xW@xe(>I#!Og@;Ab)0i~ZrfRR63FI&zP>pd%?)8~$#ecxO!S3juZCh#Qcft?X0z_gO+) zlYBI-^h+kEFs5Rk#e6%df_kvX`e?$d^{4wpLEWdxuy)W^)Tau(E<-a6@=F;`A++Lk zfL2wkv{ISo4yF04L^O7k>#`Z?KWInUlR4=1^E)gdt!M%gzIURrkc&-RF#mM#<~wL& z9>2pvHE*(@+da-L357rhW)E^`LYoI*=YQ^{8l``dexWq8u-kdq7mlGPv!M?Icq|+4 z>I-zL`eLHZ9e7q`A1zc`)X*11YnRea@-oUiC#X<__At-}`4jt2%Ag^{V>kcF^H$uc{08Dm#<1dk81zAV|CV zdPFvyP!iG?CsUyAiv$X&UghmDnF_UEn&A{Got40&t`p+RX_)H z_|0RofmdZ{Y>=-o_H>JxjJ&p&?%ZX@*+U#V9pu5Zu<+~-Wj3^Y=b(k@rMd_wXc}72 zoOJ1+WdgKbWUWcu24f$%qv{)x0g^IH*%#TlW01QbDVZZK?XbkQQV$kdKThB}5SQtl z$Qd|=SIOPD6~~FU@^g+~YV=X+mA>#uoLTGTxDGGN?eUs9-s|C>^-@`d)^x9l3-J=! zv3MP`kvp>i3sMgj-s@8epA0BCrEpndI8S3#{sBw82WK3x%(`7Um2u-_ltO0E#er}a z63Rs^|Hgym5Bxp2}wOLvs1qDDxg*5#z=Ka4Y&{V90J}fwJe>bZc;VOx;>1h zwD{N`MaG1MKlae4mWxbZW`jwdr6Y4`dzecpWQa&VJg=bZS7kA|FlXV7%;8~k_4XK- z-j1{MwjxLsL8{0WG0#|@(O6`Ewvl47VCzlS7uk&Ev{A(u*^P4E!@YoMU6C!biRg~^ zR7ky7kJz1%R_Dh#XhV3*7hfYmt)SEd!{oE%cl22*AL*9a9J2`=ffpMVmKwd25#X?>zn+>V%`l|h$xLENGtZ;OldUCxZfg(Q7Rbegj7v^{;X^JB%u z&iNUSV!A^ap32SbsCWk7cR^ggw&xO?72{{4Zu3K!hW-g21bC4Cg18`kC^tB&TRF03 zv9rY{geWx&k0fDPpCc=^6oH;6y1VMsnS53!Y0=KmV+ZV5xY*;$iz%;d#M)E2(GJ=c z#6{#oA?PKfKOxJ<@mz&44Bm)*pqd!h>-nfU!FCjC(ni{Y^f8Vjd5dkZ-i4z5&D_Hr z&tVvqnLWv_{rQP@WCRJ`M8+vkq<7La$awo2DIe2A*J!iG7{6}8M;3{C;d&@ihlGJW z1kVIW4?UB@ky?rX+Yg>+;i-k^V|YmUwZZ>+kw{%JSZ^1kzlE})cT#5v*Z=lU+9Wdc z$N(~!g`NsjLunPq&iYZ5+MUF~e9HKv7z=YrCGPTqoI8;4m82T&yGH8FggUo8c&1_! zy8X|JFTPd@bZCSoW|~6J01ibJ626f}wlUfzxjkM+yDaw({63P~=`9eF)MifYXVh4T z5CL|l;usSCF1_s^4`Yv6;AaQ|O*p$FEbQ^#9er5H>>29{6T;{)Bdn&$QXq674{*)S z{nU#lzUB#*9Oem4-FH} z6LgX;|19?)H_9<4f4S`#P&BjavAQ5l8@qm1M=0pMo6aI(wsgu5BYJ0EU7SFxHg?Ua zi^@~`6!dO;m1X7~OWoVu&W$qz57rIKe>(?P=|2zhd%Og)Q;pK3JCp0^YCIj+&>}W| z3p$Y4EPFtA{JY*^i0*+WoRq`!5l~Pp^JQjNRNa^f(JdeNSt|5Iia?_+b1XS=R)gEZ z;oVpmCgN&IyD=Aojx6XnMt{`#JoVfh1Ba`ykS;2LvopKa)ZrS1Z?hu9OM3ReU6S|J zNhGWUF3#-w;v^dPHs=6dowLNXj+qqQ5?X^ZB0b{-njZE>jCHZd-mj=KbBs=LoSBY= zupTGp^=1N3)skZ8ZSSXItdmF&68<(od69mlmN~|NBsGDoZR|RAlC(+7ZN71m*^7iR zQ0vw`aK~l1{W9G5{e+@dWVi^aL#Fqpoc99InzzHfQpZSUaN=jD9oeQg&qfsRQNS;a)yU!t5Bfg2}ug< z?oR?u)YBJpO13#c!i*<}By8+S!lmGROB*24zp*Pxrs~Olr#xcAPfCHZepqKs3gI7p z3gB9c#Buin&tH&^-c)k)Z;=^?Z=L0Sb89;H+$|zAM79q779&{0e|SKT&O+VJ=?pi5 z%X;q7#`AWKiM1jjFmTg8)!NXF3U1n{mJu$XvErYN3SpyC;nyIf{j=YxWUY7V)47QN zt&|Sg8NWuI&LKat!Cto;nUq4YXllgzto3ccA;eAPzVRNhLv3v4(ydzS5!;vcMTuY9 zknr9B#R7hqwbk#&!3=GG2O&)Z!(o9ESx(bL(n#o+Ncv*Ev%c0pi&CUfEz>hmy<>J^ zmcqEl{j*F{W&rGS5nzXNYL3>0&q2b$0nAl4s;4}eG`uGoNr5&=$&G{*o-75fN}5!# zg*A+b!@lQXrExoUgKmX&<&WJnf34zX*|9F|7$qEm(KHLsJC)ofu7ZnB`9ch@fX9VT z6m&Tk&Eg1*0Hga9kE`puosXBbptMyUpuT#K<(o@}2~i&PXE75dct%gS&l5c%#)GH4 zS}%ChA!qu8NuCK4?(kXmVTo!VgEP6r;t45xXE%wA+2Lc)CmagEdz zL&~f5|MbRzB{akMfG53@e&iX;5$I530{@0D zgi`xLFm)_Cr6A4VF3_okL*fx=S&2s+8_M~;m}MXF26gy#I}l&qac3fZ;M-arv_W8s<*M`F3i_sP%d~J6O-)ikuS;_!k5I2A<%16!0a1 z9m>inkn@2zV2$bZG92{5T@L)Kgy#?NG{DmWPb|Wy3;u~bUe5d7sW7`oWDen~1S@Ul z=Oo|<_-m=Ge2xyW=^+8 z7Asz1eDylTAKse(pmnr$YX?Kem`ltV*7?QCS14#p`G-?bGSRxFgOb~sYJIU-^$KZ6 z^@kHsGS<3DZsj5K6zj41^R1+np*G|eR;%R})PFeq?=7qjky{A)!~XfaXli>9dYl9sFIvbSrgmtt^43%KcGpAXL<}tqX^r_a+gQH};VSv|YeS99lJJqWA5#D7o z-c{Cnu|7PAHRMNF7Y$-%euQy_ju%=kCuOX+RL%}c|G?iyw zWdwo!dh0sTL5``wa<%#T|2*0i}(V6-yUg&hosF{JP#X03HB!P0PPW$uovtu~ZG5j)j zw`H%hxGrGb@$Nq!+op@&S)x^+nX@8g^d0kQ^ZA9#em?e%58rBEu9@`r#j#eZ;@07o zkNTRuw?4hD{kv-V#pffgbbTAUEJx9PEvb}e= z&rSQ?i`g5FRsp>V-&&pGU=naoK`%DztuGDsEA*az?U*}P?$=PS##<}rfzfaDC9 z`?UhNzGdD6Ak%AgqsT}AE~C{6P=rSeaGcD}<-qUfAdm1!^#hN_TgwNrg}^A1u_d}< zeO@|Xdn$!K7$ZyJYIB0sk-rY;OC1}Pf_ zV}L;`8Lg{89@goD5?utNK&UN*aj#!7&$^<62}uPx-qt~fQUCp4oXeNaJ_Y<+=kJ*1n_n0pRxk3$l=x(JDA8wpMqK_W1LDbMy6Ur54;FIwb#0x zBj(O4vmU*(c^bhw)(SWoqKT-31mjeKQ#r~y-@Gnfeb6G~ol5X3hg*MPUK>AC#<_~% zRHlLi|J+Pwc91^6q#SR}ocFN#0V|9n%%FQ|%oxNK1|u}o=ShPU(lA>ABgky3)i~JO zwD~E6w2ilh93Yq`6513*<6`~hWZ$q_I2wiTefYpJ;{2}4m!s6o-HD1MY^pW z@4kBcfBuqGO_)X2O?S3Vu_Rfx?&9+*|yJ$oD^X*Iq@fTrz&Mzml zi`lUpRjlA5xf|$0=Lf`MP}RiIg*tb3G0w;?9M;vpF}s-dJ+v;nFxK;t-yEXysnOgV z=8_KllVY|?h=0d?e;zmAsuE)Rt=9X%k0k}3>_5aQeN4#P{+Ut7eQF%qYSv|Puxsrn z<;VPHwG>$1|GoEc$dkfMDwCu7utn*sh`s{0gQFp=82jTCN-mtc*!cn75w{@8sq1*? zqJJi(N*mLHwBWVrN#%&`k=386Lt3&=+}WcqCVBc*Ma4y{q`<^}EvM0$pv{lL=2;3* zOeZOD6whS2#aIdq;vNa!^=4fc%6&ER!Z z2vNO8J7dKeXS&f$qwRLH1MKQsQ11{t3gOaOvOgfZ17ruufb?!e-W0;(vj{2j6YPro z#LAk4%@(RvV^s)W6B}?gsY`HVf~}~Um+CUuS!>hRnP!C`G?KJJxY~mZot{i+i@=gP z;3Zvvx?j?GM5Kl{q@1%$Og3e>kY24*udaZ!6mYvSfpotLb#Q(hp~#Ii=&|^GoJs4{ zR!74q`^fwcI>VR=;iD^$TEh6p?4caKf0HGKi@FrsgP)x5qC{MB$BSpsel`6KUttO3 zs7vE@WH-+whvbc@JZWM0y_PNRSIQx;1m92j8LE?Cnn3tM+DZm~P60cbGBCU%ANEPZ zstX;nv7w)*;LSM<_A~%TEu{=J&w#gUs1)aGbZ~kySf=2Wd5eoT3oN~OsJ8sBq3;aB z5IY-W@aeg(S>sbUgL_trvQHtbOk{+&`@6hAjb77GWuZNPeeC^*a7K?;1D-dWscF#k z;F(#jT|LOW-$L^VE<72}=<+6z9CmL4?9I4-FU?V$Ct+9*PDH7_ykj6O*VmTF8$-OKlvy0AsAv$*MKMiSfo{jq{kq*f0nA zi0Bhq$a|N_%+MSh!X-2uK2`P>(qk4SJI5w)@f;;Qf488^^Gj{s11T}Z`g4~=&E&9i zH0;k&;XWH}zX&H?wEgFh8e_xuFnhLu=`cB^7e)`JuBrMj)6Y7m%(5%sYy#%;m@r!l zPYKm`NjlmCGk<>AJPb3sUCgMgwJ2K2Jj7EJ!Z;Bm4)Q-23*XYhP7G$_qM`Y3Y6!Ke z=k0=CMvFLWsadTzcOnL7jW@>#61}Bk0lLF~?=>~Co8SHxYb!08ZZ)S6b~dWPf4i&E zF;h5IZWDcsJ-Z#zw4z zxwLQr=o!8@ett%Im`e(@NObbZf&u}lW9AJ9f7pj25qI+SwIWwRX7;lm;;92Xo3p_a0Q@WjYX zWf#k!Cc3zqx{u1*VcG1?-iAtXAB|*f2P>*dzy0Y?o~FTzjorh-L!TNJn)ejc@P}cf zYsT3fi)amKrA57)J1Z?$>$`akNAPJHW|hMZoW+wVZVsl>M!0Jqqjj|Jd7oXl-f{b~dy&^I(#(X{lyin$GY}=o@<2E{l_nkdTr5`KRZfd&cQEIxe5p}bR zcGuT;qbDPp75)VLfnTD?IiuDp1>ORUf`m7m3M*o1u0_+)VK#Th^6 zJ4%p#;@Bml9)nG6YPd%MXU#XYA+S1x84tR>aX8~Q>&Wkhbzue{e?1N)Yu(}B zpwxUGsPH@~gew_891)~UOU#Ia@dk~Y>6w_iqdw0AUNan@O1DHm8(~2nHiKUGt+x@* z1Qa~*A(!-+E{2YoEq?y%lNqJstBb-B`4)`B#)b3DLq#TE7nIrGRsQwH4W#w-;5! z?0C+rNeKgg>`XmQseRf8XAA?`t*$>0l1}>; zffalqV}E@+)S2FJ5o9<~_WsbPOijo?7jG)XdUHA6#R#E&3eMSBThit@RH`evm{4Wu zxKvvXUMb@(p0Z-S4J1$Oz3#;+U&!rmE+vw)!=iyzy zZ>|J;gYB2=AU(rfbif?Q*t}8-d@lQ)&WL%7^6o6rAF!zSFkQ+V#y=M+h1bEGIMcI7 zoNMyIX;WOrwR&uv*b;fT#X5FHWU6u^}Ah`U_5_y>s;rI2nE3nNg0jXd}A4$@#_Cv_Pe1 z<}<#B6s{8R4f>X7rNDSe<02YIY3gHE+vLUzMH)4nXuT_6RLUqa5xJHyZoZxB-~U?O%xE58ykUDEoo*0%ui@?h!ZXel7VX#ItM6)9ot zn682KEa(CDE-N_9?|D7#jOogT#T^J^&n`P|Tz+nsotp3}L^VY4IBV@TGsI~;8k*g4 ze(~wP+nxI?i7iPUTheP@dIH5)74NW!EzQL>jX_FCTNj|MJ^UdwcN*#%aG0PaUq34# z^9wLq+r3Xh>Uoegu|a8PU>}iD&KScX0>!zA*o@mPxP@ZHroSlr_)T_Pbg7Xt#u?~ zF_%nWfqdM5sOU@C^EzJ2PNF!a2OB?!)Hk1t6)*#%5~B9w(2qLRYPGu$e8^+m@uJ#93sF=S zrUikHbH`+dKn~WsN;)0bw+!k>lKPE*=~RbWT_1L;Muv!?4y^BV8009Jcrn!UVJAIK zFGSH}vIQ?`mB>lwT|)IYASuu)P%W(=vm`zq>W?dM5WPXp9U_4TH;Tg=A+Dkc8;|;E z1#6FWzmYV9D^8LEt3lFk^EZld){R1+%r`MR$Tx+Zg3%+Zfe-lC)B3B%!O`9K+B+r7c}iyJ6oJ6U;Xu z)hCLn8C8~~X5bff#1ify7}Pw{AA$KhN8U*fzzN(t{9(?wy4K=u`M`^Gv*i3p!1_;5 zofl`EC^T*$)}pcOnzwVqj>a?gcPtc7={w8LcnQ5yAZ`E~_O)gwy$7cpTP)vuj(KV0 zQT{ZX8`iee7spu7S^m&`%j;;kxPjqlqmr+%Z!Z2c?l|l$kp2er%+WMIiM6K#re>H8 z0e&(k0B2eK7#ssLxicfnRM%;7;Fhv0n%*wLW1QsAjREq83Cipzz1@AZ^!TSkpT-s}l!TdDretLM(f&*JRmw9pMF2xF}! z+YVZ~n&C84DCxIvb0)^YE|DhQmCbmXTy_cT&&JP@YkUU0>Gb%#XsjP*1N_T!0JcIu zkroTD2WxCPKW+n_G=&HKuf#INgi~vJmgr~c)@rwidFgNHneGV37O{ZcBIc(r(r*z> z!?u9_+#=316~fzOc$*Jz8R>_Mw}|OFDezTqVKLJk>5u|n^sXq$0*?Vsx0N4RNefj{ zN*U7G%A>(HbxA0bP*0*LkRIk~c9?DJsVF!flLCo-%F*X<*H<`UuRqp|#v|cm#r-yg zFrd(vm$+=eja=zNmHP51mquU|$P)@DD+-wlNH5gu9X1CIF%=LQ$UBTo3Eu+olks19-UyR=MT9WW^mB zH_MOkIm=T%@H93v{zFhQzju##*2ETJW3A&6sMWZW?hWY~)4Q#+r~(-mc&z!1{}{l{ z=v8uiEx9e5lU5Wl{#Vfr*oB&U5yV3={{85E(d?80hF*;oYTiCnr0kzR#FPKO_w8-O(~9D&U_>j7u^x6R z(Y{}MX%ayB$UTi+(3rEK+r~0}(E1Mt8-E7&~QgEl+vDvbwl}`CXQZ)FK!)bbg zcg2dL!$ta)a{=emro|m^mSW!WEuJKGt*RsPKu>^vW)uic zT3_}&)OyZ@jhp#}Nn|WEz@d#@zpew@Oh;@kwyfH+vFoY2F(40f0bhF0UU*+q2XTpu zneK3yU#UG1owbqVp8 zt???*l(cUlebO72V!IWd^7C7C%44_8ceQq-bFg|mo{P{k?i zmE4@hCyM{<@~u8?+3Ut*PZZZSqGUxItRH>Y09vyJC$q#K`jRCH374EckcT@hBx($R{x zBmh^h_fm$_U>hJ|yOZ!D2G)eWGGsW;o7(=86Y4?6OFW+J0!`cmy7R9PD(WeBLmKCFm>EakIEgXmd71 zTNLmlC%Cqi7FJpIyJ6h!S*2Ecxtv&jeNrY@3%sO~wNUTFUJUUg%j>d}GCXarl45M6 zTRT=)fv;gSH-*Df$m$0?p1ARjwT{#vZT+zqya7k&e0Hiy;xq6$hy?SKU0pUZ^Wde|D8x|kZU}_cs}j#yb|KB{vFpVqTUpx8=pZe|3CS% z$6RbK5X|Zz#{L0<@ho6$9K`s6jImzE_+Hu{`M1G45pqch@x%X)SM6ujvvaPt5Qyp) z6E_eTBdr6^`yS73kgqU1(rFrK8^eXI-fYWXeWCDTnI1fA+g8I#O!|AUGi~c(VOQPs zwZ<$3O5MbtBR0@>7_o6l#VlYG2M#$<=r79Kz#48Zl>#62urA7AfSu#{da)_k|0f`- zp$lkbpx-1qe~%>_etUSM{R;=0==G3Z;s-(R53~)+JjlOV36jGk=Wqpgqz$f;=teYt zou{acNDwFYemO6c(B;VZgl9%MlNQZyg=qN<*za`$)gvLo4;kk2S;dLqQJG1fE|WFj z<_o{NX{6v(r@k!PGtoH9qZ3%HPJO@=m1gE2QBJ=?3vNY5$xboC*|{?N+3Xw>Z`rZ- z99s9h9;7fp6bE;{2qVfn-(rTD#n&AIOB^-gFU2$%=VIhaSu zM~TkM=v;Zk%Orp8P1+NT>pJp3H13oAR2h7$7>))48z63ttXj(@ z1;@IaoS9P*3+$y@N^VsD%N>am=Ar3bo<4wN9>sJyO@M}%m`zRLX ziKKKB2jvJ0oVY2cnWv`Vsb3PRLgC3;!o4rfJ$1Mw7i8c!Fs~ltZvkYx7C*HdYf1YGVWt9CMd>TQnTA8wov9NfN!2s=_sd zQivgv=zSHI1fHp;4n%W`n#8;+%a?pc`7$MQCEg`NnI3KY^=L90gii&qgW2zKa;v>f zmL?bph)TU9IW}3o^oTGTWVV#wT>x=>kJb^ouc@jphe1q|8E!f#$XS|UH$M+J!l?6L zT_$6cIyg={#r#wIV5Aa-LkhECuTa`=+tX!U=K(bgc>kVvM(HD`vRp%7XZLZ~>)G#C zfLuJo>r3#|T3^oa8yl9XZmyhqMGtzGHT{<{g4?>Bgwp;*0)s;0Vg`W-CInXAYxPl~haXall!HqDhw?TL`3^h{O}fQz0sXc)$-osk1_EOTw!o*=dH>Nz3`Bceqab+kQn=nB@f#v5&Ow9c|%EA~go3TKEWmRJck@2~m-|xM3lYeiORSQ0b2JPe=H^a1n=4 zo<@IE)ORg$Cw{{&i=)J);ShPM+htLD!2kIPc(Ehjf%x;uuxFSVu1Y0d?cclOPO$9K zI6Qf;TLbW_v~ZC^0_+l{K*j(WxpE$C5Lkm(M!mro2&$0XAQzf&Ea1pF>PR14grTkO zO;eFKYE!^IxTb?%O zB#5{s99oNWKPV1Ev@awKc6aHt7=j8qG^q$yS~K^KKO$;}hhC2MfpZLVK0zny~^)f)|m^v7nQ;7l<#d3-6@Tn16oYK(e?}m1! zrUG~R2Wy5w&Fy@!C%+i%39iE&t7|J62uM9QO6dQIvpr%vTYg_<_1gFfGT*)|p@ld) zW*zahblf8Ii^L0ta#KK(pYE%)G2kH|0K4fzJA?OtG=txdZZ1Rm=0dVg(T4mTjC=d` z><%hB4o)s$jV}l*C$n-JH^~S(^c40yW(?u;xR~-?up6Fa?1gs`G&63yCMKC`ZQx-j ztdaHWXuj5d!*vbhpUgM?ifeGPSOmWYQ!VWO9_xp*8N);THSpk5GEL+9VD{0w2Wh%d zMzr)8@J>#xaLPO-@+`}J!9ZZryK}g(@@*WRJVe15;|p$-!ld9yllYj*iN?v`>(GfK zO)y7b7x{vFqL$S{q_7cAk`wRnIRdL-Il#C;3Vhp7_(o10yiUdt#_vcAyw*3;5@~M}aMDdHEjlXzyjeh=IupKlBPNyo;#TXP1YZ`0l@q{!^guP~t zKr42RHUT9Q!8XG)dqC1z*f*ijqLm&_zw&)?wn?qeO-dJ|ZD{8m7dw&mOq>|OzYU&o zQWxzLypy3e3$-U2KjQJs54_bmic4r;qa_PYD@?Gb4Kt3D+X>U}5zXw)G{&vaZGn^; z;#3PdD%FqoR*GsNoDK(HxEi9#;p__FDaA3>%-ZxpM@KB2HlBlv?5N68w}* z!Q~c$BMfwkIu!k$;5y^oIB|HZNj^FH6mH-FE*e9m`Cd3#az-gRaw~6y2SF4wH|NFW z_yBi$GMAJ4X*+oM1*Y$F_!k?G!a2#d`uNn*Ex7`v*0f26j@Cy;Z_lBdNw2H9OYj<2mSNAIt@<_f|dJaY|FW73^5=GU#7& zUG0Wzux^4(BE~QkWKzq0xk0}|Uks85P8Nxd$u3%Pqm;H)nSjk{`(hgfA2{~l6D z&c+a|nfc&pQlB1UK-C-X44j+heZuQiKQOZULTt z4Rll(croAWRO5K?Ei4>3Wc!;}$zHvsU|Vh>_kK7Jcp07-;2G2KX*<%;$6`5p*FXS) zjXnHYSl%L-J7Ey33tFWn_-}!n@h+sp$L*lqJ_5T4d~Oim8`;j(5#}$#QXIMST8% zIKmVoL`Rv0E#N0%;QwRzS4Y+HboFFa%)YA9TsJ3}mF+7}tkje+@lIRWyp;_l~S_#u>xbPE3h^;uB~n#|0705L)*-~@83XtHe;C!0%G ze$XCC_U>!%2Bu2&%Egya@+mJZq(Z;Y&X~QCpn2V5%wDu_1fRJ`I|l;4mpNX+&*A93 z1A)hSfm?7JNHQ%gd>Z!3#K%H7@R_pUavZEKTRbhQO;=RO#a<~e3{LKk*)W_D4g}VU zC548Eo4J%l3x|fXApL^f(g@Pxx*bnz33^aJF zn|Yl4cQ2KE*{e=w!9pJm=_EK$9_gyL;AIN3-sxINbT{iC_QUQ@Bki|nGsj9ej!I%q9FDk8BV}n!31miMjE`^CVDT{ z&)^nvW*D6jP`aQ0CHOJT9MMScb)#n%TK@#GKhwDx(3ASR7rm;K-@q6+8eTaCUI-oD zNrB!N2)qn3*Vafh1}$ubacFT-Kj;cd))n9MF>uy%pl2~q6nbs6YcfY2yBqjcB9R-< z!QGM(ryNbn1wI+HAl$)+^f~a6oaMnbfKvhn+PMI_3w%@jCthXpN$7c< zS7>NDMYOyfxE&=?x?ulr0R_k~iw|-j7UZNE_Mt>4e+TkPbaL!TqLmX0$xe7W;q<p-(+)g=q546oKPIIp;h^qI~$_sV#wO5v@jc|97-n&y= z1D0MD@D}+?@!h~JSOvdu?2zsLxpi-WG~|Nb<3J*2!(7;C&@|u{XT7G0L=wbY0(&E4 zpCyWi=quf1k;<>C-vXz0wBHG_qu+ROx-+elkKst{*G=zduORhCLquz^j+&PY$`*+A zG}HOc%nYz?Anpj_gOHoR5$r;P?$VgN&GlpEz}w>ncPJM-YKq8=*K%*Tsw^?hQ$*^q ziJ{-uFY4Xeeaxb}s-74E_pg=3OoUVzn<4&e>^5$uonS_(J^ z;V2>C2EM|^=6~TUz-rb6aZ~% zk>x0Z*q~|E7&M+}L$qg@LCr@ctJp8ZkJ|9qe~M_qA&BZmV;8qE5TzECj99-{mE7l5 z_M*T8mOOs!+=332^guS_Ln9)6=h5Gw#(CI9_0_L)vAG_x#YB8l=iwiA=#ztKG?}DF zc5N((^o$&Yq3(ep=^)HQ;02|@Q#*#HZ3_Ev0W11=22KSZNUj23?c;W)H@xe9n>JZt zvlb3>?R^#rf9W@o*b7swSMDEtD}eM>(_l*80=NCaCp~!YKX|9q2{k_K$NJ%ORy<4h zQn$nGdbnM3wLZ+YwlO;IlGvir3K(C2|4Sl@4yI8wq_r`GdS0F1EVi&Zfub(Le>3C< z)8xAiXtJh>v`xNOpdS2Ig%o&yuzhvY=F$rg>#@MDX;8K<8$B0MzFG9?zOK$VtELJ3 zO!RhDE@*m+&XfPubg2B#(0@^qd=0%_X(=HoShKRTi%4v&3hp@~;S)&-U!i!vU87gz z*RE9L-zb7-rKX9*q{7{UTy*ArCTTiilkP+8#noYrn*56^=ndEy%}3?$wM6of(5KH> z3s`;^;lCMf@(0rl^og0AU5a!GtFEp{ur0GKTS5Gn)zJ25dxP<|q^|^5qUS%sg~<2` z&pu;y_uvnvgZLe@WjX{CRczo& z!wN$9r|h9~9)L5Aha^RIPM34ZQ#P?_y7|l%67@FOY$A8}zwXC1Hs}Gh>O#?l6*(otyPQuk z{8oU(ECu!^fvhMZHSt7cYdHA+DAc&^mC?6`>O+_pW4-=pVx~Yu^lenXDcjj1+J6$9oQT1 zLTm7G5>ZGAWBbW>2l6~CU^fF_OWL`_{y}Gm0#7YLuay!XI=L-F{+DMjS%HLc0}IX- zl@e*gDYLkX;?kWgH)>UNQ9;+sYp)hrZmXz2thfrElF({61CS0iep#x?kG6-IpKT1@ z-FX!3*4)O4u$#d9*fo?e2DC&lqGu>)ME{qi!BfkpBmqv+zw@fwq`QRfFj-VOAu zT@EL=m0Xa{C(u@)^GQxwKP;ll1=|8waUvf>E&n*RySgrw2vT%v5!{qaf_JQ+_u;XGi`7!32#-dWo z?JA1I#9dwSC7jI(18^eLm@fAz2U{0iohw{dwu0{`|Nvzk{(LeYsV7O_^~|Z{l0y<=$)>IMtYLkAl8b$$i;%PhYmf z{p-v|yl*N;#%EvO1#g`8D{pYLcK^_i*9YE``!P(QqsSSr3Zf2&bECPDo=EE%h(sf| zJ%kFZ?=1Wf>fv>#)H%kgcfi?Gk#+53^Yl(8*{=k}n zf8l-vaDU6=DMPpx0xn|Jn&2Ld{AHQs>{2s$Wj-syg0_#WKAI-Vo^6lL#x~j&H=F1U zDlZ0ZU!137hz80<<{!N;__z0RWLARi{Fux!W(jp zguxo|P($AvFV~2ZYuxFV+mQWB!{h3!Tt(f za;N!2_@aeV^S*bZ{yT_W{|45TuV4)%{Ud!Lv=e{lpVuh3>GrN-t!m1O1W&A>#&Hmn z=q&pZ*78|!R$Z$q5KJ^lo3TyKb7CSphEYvNs2frM z-uhU9RcG>9$`Mycj>4ZA$|^NiwgH|6APK*Qn{m^wsTm!cQ3(fbyz*nxETBLW12yT{&4a2!jpq0b8H!SGVR{P=N zvBgg(G+4qe#c^p|Klq1gi;dRWLIYQ8e>AZcY=ZFCy2Lmx9m<~XpTS+Xo2>nTP5n3Q zSyrDnSm#-Iy9sOKX83+j0M4g|a|AQ^@odv%%mi<7e-Yp<`T;KuzKMv|bgnKD@9GaM z>&N?MaBcRc-j@| zRTc~}+?yI9jyrvmZCR3~Fy4W*5N)1bZAo@tv}YH>H+_0w2TE3WQy{6yLy zM>iEFmD9yRZf*u{mdIb333Btl?f>@!|LT9%|Ihx@8x%%`d-H0uIQ8*Pl|B*@WdgE3{r{pv$(x`6>AOdy$Uiq9OGneE06kTKa@qUa^n&Qrz1R zb*C?)Yc6)udJN~Q^oi+mSxs+T&yxzy1D^p&cPb_;RJT1{Dz;2N-FNHIvE8w$3M(6D+=Q2b? zWC_{v=!+0UYpCH{fJFWpfi`4#4^H(nXJx+0?Bm{Dd0Cg1d>UiqJkrI^* zV}^KNWH<_O8ADXOPrr-2x3?Z99fyCl(9zxL>d419J(gn?CP!X&j}%vzS%>;mx9n*jSM685PNVV#Huct zPt{~VRH>~_UlJokMgcx*org*z-xs^FI=&pIB4Lk{g5N49g5Pb<`~rgSjUM{M4DRW+ zXrN11Bn8g&Jmkd>Z0uTAH^Xb4w{omC@4*6WjB^j?{xuTs!z!?`%UDO(WI>DL>gW?` zT&hgZVYy^9lpv#YC$|niIJ0YHU62DO20n)q6I%FF&u_g!F5LpYz1X8}MA}_mq&I{2 z7I;&ogj-z^(DzRM^tMOO)~LR}lAM2UnFMXz5QcZ^PxN%qGvV_vv=<@sU}{f!Bd#&R{qBpM*?PDa;ZbB! zMy7?_0|r}apxsMu0GOJ{o`C3Dg{*1m4X~d!(mb*4SkBSJz#JoKYAoEp0>3MX9{ig) zu?#*#6fnKbz_$`GKp=f~(fRgJ`MU`HU^9Sk0(3Xg-0#8eYnn*GnZCY^=rrP^7%Gzj z@GT)d*4f?^;fpSiQgLO2OtqdjnL1N&g}I!gHK0NgEZ%7rnh zcnqmudlacd?ZK5aY(rRv1$=|~8)D@D9UEd?!5rwv;)~*|aTJUwl2=kfPi6zXk>O%o zJH>}gCmL}IJBorEP$r3t-IL%EOL+zfpM&*37i|$AT0nBh7(#s4JDDJ}7@~w3kfJ#7 zb?Ki02oo6x^6dtcLb&n{>@?=0lxKjS1MDb*0RwgnXFNekSm53XUlpYV8_j}+zf-JW zkzSidw~R@HQv}#y>C*c6^X;QD=r%gf@=KL&(=RXn64pqNM=RX7o=(PQ$(0`@CP5yX zk@rYUG!OSf8SsV(VbW3;?9(B&XaH*5BR%IuGT*U3j20Reh19&feoDx#yv8QW@2Lj?x2ERv3G*zE{S@AGCx%-^B z9iq6+eps`d6vT$0n8}&Yn%4ZyP&+U>&El(u82h3P@~&9?Kv(+Ym0*z!-3#z zg0}7w4SHs9Uy#{E#t^XS20wyNaFar}Z6{y*o*6sl`4_ZlCn{B;sm~5HWO6iuV z6&qXGWlKn2EpcVo%j1YGg$5J`~E+F=JW8}FXy>Gzv~-3 z@Da)wOJj(nSy`$3Nt!}hJ56Y5s^m_wiB(uF>gLjzOouAnFcf?fwbiIqr97}{q(Vtl z>h_C0KMDMD4EGWKS# zYN}PMGHG?tD+Df#9rfDU(F$d^vcGu;=5B02*3I=YoGn7&#(}VX^#HXawn2~3omc9? z)m4>Q)uP?T)Bfr`pgxfK?7Cgf&8_vTcMMc#KDUmg)7!B6tOy<(7C2jN0^L-VmG*ur z@8p@OICF1_K$AQDJFrR!w+`w!yW^~YHv|j3U7g%PTvJ8fap1(f!Yia}H;ME{%(u-# z`D(%T?CP&qzvbV5ZsUE#xxW9L7V0u zzco`rNmMdR|JX}+%bi!Yp?x)(fHCTEC*x7=JaFp>cyGJ{?oe^BrF(Yy(>*)=Jg{`6 zPx}&vFEN6qxCSFXf$#Sw=rQu`Lf|g-4}Kw_s4tyQb?()%RCV6ONUE#UT}XAA`h-93 z{1~U>{ThZ{R%%~x=Gz+^E;t|i!wgtHI`VDw`S2fd$)jHbCb0o$K^4p$Fu9R=%hE6R z>KAl2PMDd~F(Ew*cvQ$Jm@~&{%bFAUn>X**>-J$jKIM#SJE|mZ0UY+MI5cVLEzRM6 z4c9_^xYV~%+Dsnb6{p$Z6wGb~U(givf7JOs>F7GKfmWB_Pp#(xUnp$975eGA_yqf) zInvg+^ck3U`gB0!I}#hrEhgN`hQOCW`N7+$*}waj{8PFvNI&C?*4E}&Yr<{$7<5Lo zemea}v5&66c(?n;T1BhzFSya(uT+B;bQ5;-CzVH?Lv70yQFh8^c^O`3jQv}D$4}#d z6OO!HN!E~vc1!8wyy?wUtFdM48sJVeDAOj~^xnhTJ?r26aoLIZmSXeE3ZB3akue|li=8Us$ZQ1Fw`fS>X$UCE3RWG|goYO#my zbnbvm2RczH&g3ySB`F<>EmGsBGN`=)ZM!=}v<7sa83?Wb?vm1m%XkcB;LUvsb+(4f zfV_CS5l6ngYLzSmx`%{-d04kl4+)*$4&zN&Me~ScH<=;Xr5baU%3^`)X0dpYvP4;t zu+urzLFYTku}9-vGUP!-kTvB z-N(ym+`MNX-9*V(@{TG(Q7GtbohpUvC)rqE zhX|LCyD`q1+@|rKPa#7mLswVcwPr6UA$Fj~1=#OXGj|(=R)S5?NKOC@n(nhRT1Dk+ zjI%xD*ZMd-7sGcx$heQp6C-*uC9;bfTXde&N^*lJ?<^M)4p^ZkQzhKDqLL2kN9sl=n+SlJ~@$Tjk2I2ZMyh_Tdr6BI!%jpX}O%SU&yac-+p<77UP z#$uOmkbjL*KOF7%i@-?kQNSVIaf7l&j+U>IlQ9FIRJJ7S3eR24#z~2Z3SgcrYDR8r zRC1M8TGVuTiV*mIPzZcJC?!4z9=bb|B=;=HWQm$3og!%wRSqHW$2Y1lm(`{?&p3C~ z%#uKq;Z&IjiNN>EC<*tFDV1?~N_nO5OT~+Ctw8*$DV90koqNieueNRP%k$IQ71nZsK(V$0SDONUN{%kW!G4 zF(s`v1EmV5h^l>e$VM{Pio$9BTbs)pP~PD)qMg~z2cU1j<0=aUKf*jPLtd1UQbJ?Pa1#qM^g=GjYR8Z#SUO`?z1iV*n4!R^XMCAwfcR>HzT zTjiIPkD)!!I{$iVB4nDG9i{}l^&;zuV8~ynZ&Vw5&4!F?AW^1N2+SVrP?H=lDPkh@ zsC2{?04`^0M?2m{D9u06fYu^q=6*oEgg_cNo+oZ@KO@db!?_logKH~UdWTm4Y<^8; z9VF-7?=CJJ*CtpYFL#L9!g-CpMcR!H;?09;t&X9j5hfr zX}xg;s2wDoFT1XxD2&rnne*?ozi!Z8D)Ac5K#vWKzPQqAFSpcl+2)()-T$ogg;#U*c5Hp$18 zqjGemtISg=mT#?el}F?Bp>)QkJdfjS=(Yt#C)CZ>D8R}$1OKxbSoBc2EnookLD+vn z?b2eq5Nk}PbjIY_#cRc1xR1g8Y{cCK_nwG*81BbI!nucUqxwR@+7J(H878`dTAMS#(b`(u33W%ry~C!n z9aVQ)O9JxZqv{^3uK1{02iP{@vw$I6wgcw@>3gs+qe!|zZX3n9HEa%3DFhrSElj0g z)7)Vy0u#`sQh@Lh|W106Y6IH@|MIIl9$gg z6p>sbNa>oX>(CXko+GU4N~tKElC|#G&5YOLtyu)Ss%h6gP~hWj#9w5ABP!oJ6ue7y zB{ADOjVqYh{q`A_W3LkJLJs7N=kb?)jY7_EAgv_}zIs9IhI-mpM zb=%Aya3lWfTiJv58=7=8CgTRk15+*(r%bVi_Xl$^Pv*6tvmf9)h2(L?I8)SdXwkGt zi^6gw!)XmgqrvY&XZ>NH2wy1FAh zOrY+NL%MVC-wv29K)*xd(%?}pxK#0@Y<+(436 zs#>yeQyvVy2z+cGB%Xi*5wzNB3CVC83^s-?UieNULE^p$85^WazHgifNpB?k&Ddau zuVU7?jxW?5kfgn%?hADfWODA=XD^wx!QDK)ig@h9uST^*2#5z$XJz^fHjjTX zFzh27@M41siHn%ekh5oO8s|ye@N#n~__t8Br&IBPUo)zBIoABdee4r(Jx z!L<|=L+d*RrH++3A#GPH?E?;p))V%;eg-*lI7i`|kY;R(#vJ`Sb`F*;0N*9~q|8l_ z_`zuh(C`!!(nuoM6-ae|$Z}OE_(+JLc80WyvSVu8oT(l5GRS`kiGnMTB7QA@4Xkc8 zYliRET3&UBx52g-sMM9v6GZqwz_>xd? zY3T2MtBvLoXqi=WvvsaDC*V(a>%c*6QSMIq%qKzr&j6&_Y;YwOC0&D8yCu<++ZqoZ zbydu2Jp~;!)6jArGRV*MNHN*ChbD1)ooCB-3n)aYCI@teK_4iGXK!_0rHa;mP1{Z= z_}wV=merOF$YPp18?3wRk88EsjJAmZBY3VPuWOpiR&_E-s`>k34Ok& zo71jd=-xYpbeJS)blCHK)=T)Vx>5~`p?yPTwc(w1{^Uf2al>2f3*AvYMP}j}DjEueNXy7#hR?e+9`#pqBVpS0Xj}b@XXsZ_*C50 zJSFZLoZVT#JzBAEjOcp`%sgjiidX7o_vXbtTkU@#Nb1T(Jby1xX>bVT{6uCgR1>4xb13^Ilh>DuvTLx`yrJd*a ze+|1t_$}GpTGjMY1GKS;vB0-a@R_dS1rWdDZSUn zY>*b&LUq#_e0o&GU5;fPj(0-ZnXx5v6|=fLC(e+Jp}zcB*S%ic4?~V(W@i@-1aE?l znQ`vR;2Q|aI78xaH=E&}Upx@JUM=Y_E6cP0LFH8kz-4-1sPB6|bV0s2bfMdw(lFH5 zJIC{V<0C3_>~!XJlmu4Bl=Q#*-}MIR?9I2Q`WlCZ`(lP-?Xia%herFx55?M`f39zZ zNj!T?e0frJVO!(mQxh5}H?W&ml>}m9Cc9&XMAykio)xP~hieLBI!Fc$zvvuZ{$KmK z{Y!rxBu?Wbzvu%$<>U@m(j%&n`4qTVTuD!zJeaZL6a@ z-yEitD(RQZkgwcIed2IVVAYE_tF=e7ls(KS6c{{hjeu_8-398`9(z+B$cQR>R((0sj1(0Etv^wf4p}?-RHG&>-ykwY%JaGNIJ}^R@|kQ@d7%X^q$R5=>)k* zH)Pe7N7WkyYbJdk-Q)hZG*v%JQ-vM>LY&{+YQ_w&9Zi$n>+^tRzN8@({Pk!$-n*<} zk6Z-^M)7D6=e^oQGOZ-g`023A@aeeEDtu1sgg^z|n}!vS8G z2BvI=gc?~fHVg*kA>caJ)7ql8#DUAP2-1&!+?Gv|wtzAXbvhz#d0DB~+Cg$ftfOo1 z0oEGrh{YFMoA=7!pdF`1RR1gTd}RuDg*0U_m_C$--T%jJx-K+pz1MGT5S6V?sTHr# z^>n_9xC18ug3_jXQ;4o67xNJmWuQe41tH!O(ykBpRm;W}3jb%HX_IcDl3l#khV#3) zViPolhHsxJ>7T4CQ^O3E|8bmWeDNtIxxcH-B$;BOT+s{ZUNCHR2C6|VJZv>WN8CQk zL?s^>p^389)_K<^)y7Z;#+`|Sx}|@4jne~}EB;?K-~2B%Q*C3d z|EadTsurC2iNJXz9!8&JJjINp=XA_a`wVy~b(oVOr4lsi&<_Y2^=^hcT9`&1V}m`T z3Uo&)E@s1BVeZq4)Z>g@1gb)2VrjZgvK^p!Lk<(%;6B#Eq9b9^JgXx9aVK7y*Tbyn zYQy;+drVEr#aYE3;zZ@f%f!mv`r(t4tfRpT-vs>=s0T-b4TC1nF|{_+i2pk>jat|X zg9a^3uZ5Xg@@UR7qeI5t*`c{5$IWn8H35SI`o9}|E{|8O#p$rjm+u*$ybAB$4$MZ< zc^Ib054*K*1^s$;B<|nzA%gGs55rlWujMS%%yOB%U9FYjy8N)+E@MZJt*^)i)MOYu zjWO2jGR(=5>`Ic6P2cni+!V*Ar@5cU992@&{W(|VUmho^H@b%yRnMSw;S%Cv>X2O?H+Rj%Q_)6KKW|v(|*0@L%-Hv7*9tW zbBN3;u)NH})Ne-EY7_MoXit&>F}jKF8kwPXb%z-gq5(c>n}XJ59#eC2(cUzv$2Ey( zKjS>2vPzh>;2kl+M4xSoR?sunx7D4HmteNXmR(qRVZaQTqtp*+BPY>!_Wscd>ce`h z7KfeacIwMacJv2Ij?0bq^Z-V7*aiOr8h^F`u1eVR+fgPUfdbZ8EXudmm#Zy#7;8s0 zgAD&#ynnY47lu~8tDDE7UAeT+Y4Vp7MfYNoCoz-TPy=JhpYWk;_9whQj{~zwUW`DUJHYQ&AMzrrPE+rhix>OSTI zwKLjlUXZSc2;u4DeQl~Smr}>}SKo&(S`Y`Gmg*(009%6ZT7Z~I81)5^Pv*jiFUSOa zVjv%N$n?I~V`Kr{y5~{@_MUr;?H^-?CF0FX?K`cI&aFX%MGHM`NNm^j)QG2)51O}D z7HwIsG!6y6XG0g-wlv2pok}gFicLy5dWNNhAsJz*Zd*e{&jZNeMo_#AN@v$TMEpT( zHy#J>sLxAj5SJf+CDm8VtI#9V$2V6_SH6|6JCsm>+miK&YBtSO06zfaCaQ5f-t0!B z*yD^d<9o4QOmVH)NlP68R*F?%LJWN`c3jDU?=^8i%5sbXG(>Yi^K#;`YcxA%p|`EL zsX!adtSvQDc;11b@B1Cy#~f73J2;PMO^o$X36J#BLnKWD{q#_h_hYQa(}sAhpYWbP z{3-NeVsG#2R30zPYmrsA*k?ox9rC#MZ z*cD2tq`x-KE6}8?JH+~BFye!@4 z-Ff9l{#PNx^9^u_ZUXnW=>9~FH&r+8@2Q^fbw{mI2hEGutI{^~Q*~XB$+)xN^~#iTtjnX9eF~D!Fg1e7SO(XppP(Km|^F%@rP#DDcsE%F#QWury3sLGYCb~~xy$7rRBusCI zK8ad(5%#7iQ_q#!tQWC^0v0fPB321oKs@#34@C_c`fS)2DmF?z4`P;|22BA`C_*>i zypuRdLyfzYXPlp%3h1X$iA-2?q%UeRq$i0&e_#^y6G9ry`_+x-9J5W}rQ0OpesZq5 z6m4Jggwqk&2f~|nHGbJ#pbW%zhIcRIvyHw4H`m;b(Y0_BmK-pS(b;f|nmcFoJ3(xh zH21907va`v?zGW0aO*XRnoeg)m#z#i~p&zU9!O^{Nhilw8S_yZ!#(PF*!X2*h&e0y-9T-z%GR+3i zPP|F|jd>nTJx)Cbt+YYvyDPrWeGMakorTR1`W-gvGb`3DXfS6c(S2x!Gf;~g51SDV z8#+DTO+6h7J~(7MKR))z`0PGEKGD5A`hEYS2JXW?S8bRn^l||bD^offhv--1p1{Ex!t zN5bd-OZWnuK>sm)BwR~Rd@G+?1?*s9SUvHrNVs825xy0|Z{Yqhhi+))0fxQ}-*ki@ zxlgDyp#87G4Yme)OyR;^uZDH3a|A*TM}LSp`oF;bcOUCm-LcM!Fe}2W2(w1QfG=U| zx*etKx=TxeFJa_A+!w{h+`vx2U5oNH8>QDG%?^a^K$vDDYzM-2B5WtZG#g<%5%w^` z9!8jEBkW;>?LpWcgzYgCUITXKFk}gbF%Ta&)4dz86x$LH_T=Ox=%IexPKlGWA8C^S zNhNnuvVapHY0g!iPK^4a(r|lmG2#7p$qS1+fm=+#+uY&YRaW5HRrwfVf$!+AzCA&{ z!jrIWQorLq$y?H2ot3Z}DOVFNgaa3nijk$ ziaX15iV;U;WhKZNNdKr9DSI*DRH&wm;)@sVDhK~$#gqzu?q1$m!O!|9K3TDRWm<6p zOlLXOw-@P?kRO=H#f`mbzZmcpL4#3A|K{?}GKVdp+)~`R(o&qT%2K=>X4k4L`?TU% zITp7Aq7fhTjVq3=B)-qsve-&7kSoR`3~RQO;F4&sBNX((W{&U-KMuGg?mvq(+z#L% z>@W)m0LOuP0s>EJjW(-;RR|`2<#?U(v*=cC1vMw4%_egfEN+HxaYQ5hyCy1Lyhox;VXyF zSR4y3`BHGn+u=iv@Od?!d9wpDQh+O>I%!WE4myWv7L0<1gYdiIu)L9Q*gG2USvV{! z681(b@oI&`(jsAvt;D-!Y+`|yNNFzuuBuNa%AVoi?h$>m!8#m#0=5zMcGxD^+h9k* z-U@p%><3||!QM1tO8yn>`(WP-`<@X)au)2nMq-jLg1sL0T-fViF9wB#(E;8fyqgI| zb0_eXuHlhj{=f*d#f=0f4~+!R42%X}9-wqAFykO&nN3o*xQE{kxx7PS>RX`A)-REE zixs-vVV)w%cKD3Z32O6+u8K^Q%WU~;jv$pZ5n;AIYkB>1vr@_@)1bxzaLygVp&S7YmHbo5^Q@jOhW?BsfO}OW{sUXNvSW7E%>_J z&}IKsw#Iim5_}C(z5d>r+k%=H@hKfuj({)esO*ZA=hDh^l>ydNF0KtzSQ34Gm>&Lt zHJ1bba2&k5@eL2)?wJM%bp4vI)&O_5V!N}xTsT3ukoE}m4D{$~c)u~;CE|sn8!JnF z38!<~KEg7Xe8jH=IqA@dM;u9Xe^;)Lyxl)c@3sMj$e@l>{(UUGo1 zMnCFw;s1cIevRW2$6QBteK~S~S1w%tZ$re>)Z*-DquSo{XS$cW2ZGB6>eu|d;KqU# z1@+~Azzc;vQZ~O`9Fmfc(I~|d<~Ax@#8^o`87o%>!4QQtG&V~}~Ln!ly@v>vQ5~e;|{uM^yr{nd> z$WO>d^Vuni-_V(xv zPvD&}+3lE}cuLpn5TSW3{V6A)Vd;_4IzLuF)cV^Hjl8??R2z-khyEGh3LgkotMzLx zE4UnTt?J9aA!VivII~v$#F{9HJwipTdJ<5Tb`}d= zuwmRR3&bDfo@HzBC9Nz5FeFHlSXPlxCfGV@o@rqmVVV!|NK5h$v~a;A;$|WO+gx?! z!`PE^0f)p)wi-MOo#v(+6l;RfF-5sQVGFQJ&w~D8IZ-tN!%Jz$=pKNtJWmTJ73nmN z6G#)uj>%+!$<3g7S5{~ctmNba5c9cP3MnO%^Z8*@^X>SDUb$3Wtv{uN-3rJi^6PdmRn^*nCs zO+9~`P}2Rk=+2Hk&Z%vnco1GLtnU$olJ2jBa@lmMGU@LwvF9z}#)74Cs^WrNH=CRU zi3U=20U#JG5CA7)n1Ax6!emA8O_c5Ke-KST&)Z^wI@b&>ll+$n-W}rn zMF4-TD}}r(NaN1aPmRemWb@k7#sc_oj;Ywmc0TP)P`LMHXIfiS!*89bZKy2+sWYa^ zJnLH3n564DqHh4L+%-y80rF);fug~f@pI+2B(#=40u5G_nrCI<-GTHO2{K{3s&EcH z@p`QLBg`$0rm1O~hv@Mi*tbj`@77gpge>_lhos&#@i}=_#WC!71a*TfRY(~PWkVBM z!Lc*?xuM|Oqv_BfDfQ-YtNR!vnpNA!6|Y)75;P61SGwfrViD_$Jw}wdiuAIqvF zEG;0OXQa6!!I?vP(9&qMqktD~fLXK+G!xii8QUa_;47_aO7iI3HOlA84f_u`V|^m_ zNrbo5%R7kLw?UtNfv;{&GN|gq)VL$T+`;-adzAgkv!G<{Iic^O7|24z>>7d2kTg&W zjs&L{T}$_HdBHGAz*IkUp6(R}_euirdBucvrUDVu<)F*jKwbm~GS zzf>q|Y?hRxN}V&hVw$&j?QU>gZb93j@mHhXOjx@~J|f3g#G~Fp+z5&X!}u97RWQfBjVRKl^kw$NG#?rah;D z@F^qEVH>8w4f`${Cb(B_3(-l81g&p?C#jfZ$C}fRoM96M%<3@B3EjOW;^p!kUM@zw zR>D#V!ftbN3@Fdcb54MF(0O{k01%BYMgdp#9h;V=QNW3$#lHDXRCgpfV;P7_*mR4uEzWM z>Y)FeU0pDjU3_sAn_YJM$~&tnZ(euDtg`aTTYu!g_?N40ue|B*e+$0p=Cbw5 ze-2%>G8~Sp-PoUQ0yF*&X3<1uJO%qnn6Wqx_a9&$LfpuBW;DUQ_MKdtTOtF<(yh?0Iv^WYS9s+_>iJ8MiKDqV+m98wD@9v!Y_v`t>*6dGpP8TW?u+ z=dE|FT4`NU;KFBG)7Ia-?yi;V?z;7|)H`QH0c`xs+QMb-{A;aND}$AtMGpMUyfp9f zdu>$vT(;sy=2}{qk3UyoVd27s3-j{}AA78%q-5#R>r0lF>?wKrnf!$%OP_o0 z`4{%S_|nU-{NdHt_BAx_KY$FcHy=FoM$4P6hu=DK^w`_^`R~5>{_zh!{L4o^U$~@& zr#er6`q{!SO1?yKCEtGc-BOf!{XhO0=snxlfA0IBIxsjiJTe;MqRwC%ugR86nuhk3 zu>`S;O%_#V5JPxQZs3e(@+_|B^SPN9^I80ExU2cq%q48(_v2TC-vj(X{2qdIxUKj- z!hgfJ@ml^Ue%tvD{yl%3Kf!n6cbTx8@8M5ztMCl3!|z%AevjYt{0018WMZo_76=Q3%Y>UUt`HnTK7MW?iZ2lyY$LvAsX*Vd zOxVVk3$^?P;YQ)-!Y?4Kzf33>cJP(LD&Zz!weSSKYmHEzakFrXaI3I~R|&TXPx1A_ z9r(Im3U>(`gu8`b3HJ#13M(`ITeweno^KTHhd9p~;n%_g!h^!Ayg;CDEEFCQej{uX zYK2FIKk<7+k&TF>Vn}>Nl-L8}X7M3$i`XDe6px4ks~79VW1>;~0I41pehyjh5>yzp%)Nsj`e`2Q7yz z?^@ooyl**f>9m}-sFndsx24B2Xc@9pBTX$#9n3z9#9p^FTN*8ISU#|DcGTjv2>LFV zZ{Yizr4RpyE!QM-mX%zUY!+(Z+LR1@zAiIkwj^f=e07GN-wC@qVtf$9KkH^a>^mUx|H1yoj>=o9+-eA9DFSA4J(bUIMx2FoJDXDLC}M1k$BiH)+)StmQqPO%a88GDT#V1Hl@ zY_j!D_DA*t3$oYQKDL+ri8ZtRtdZ5F*5Mct97Y&W+&G{Wg{8qwehGd($DtAN8{sz# z%o6H@u~L3x zE=!B}Qz*Y_KG1LB_%zBNbtzjG@n=x}=mo3_{ur1{${&-*Hb?xx1UdBM3RqplpG*12 z7qZtQ{yfS*!Hpd%^7r_d&=7sH+clL|8tW&~kpVCKV3>BibsfwSaZqU(VJ8 z_cz9>XEFfnUcw)uQbM6ff;t=?0=gZ4S{w%iP>VBB(33EFazyg+^Z8JPwJypr7H4+o z99o=_9l8sF(2zh;k$jx}IZ~g20Nq+aX?iS1k%$o+7-@>?%rgS&% z5`v2f*hI5T_(m?ksY_vIu1mpLDWwR4E=A9{L3h6hB5)ISTLDAL3_312$l6F2n*_55 zWlh3ag|<0Nlk$ItLIn|xkVKOd`9jzC7}Mm~9{s(t=qEgG(C)`Zqk*KT;;ErWoxnm%bWRGE0(W8h+t_Q-5$%s`)bJ zd^s~dRT1vfJ8%~!L-}f8SHpI|qm@a4=u~o7uDp2_EOtv7E`J$Pt|a9eW6W_$)|Axg z6wcU6^p4g-hWo8KEi;F`u{v}g6+wT?62mrQbTh#`fgw73f?EhlJyr(O_66gZrPa0%{Z9G*)8GF9 Date: Mon, 29 Jul 2024 12:58:38 +1000 Subject: [PATCH 58/77] Tools: environment_install: install Python packages one-at-a-time helps to work out which packages are causing problems --- Tools/environment_install/install-prereqs-ubuntu.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 76ea0c3098543c..c1aabcbabba8aa 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -414,7 +414,11 @@ if [ ${RELEASE_CODENAME} == 'bookworm' ] || $PIP install $PIP_USER_ARGUMENT -U attrdict3 fi -$PIP install $PIP_USER_ARGUMENT -U $PYTHON_PKGS +# install Python packages one-at-a-time so it is clear which package +# is causing problems: +for PACKAGE in $PYTHON_PKGS; do + $PIP install $PIP_USER_ARGUMENT -U $PACKAGE +done if [[ -z "${DO_AP_STM_ENV}" ]] && maybe_prompt_user "Install ArduPilot STM32 toolchain [N/y]?" ; then DO_AP_STM_ENV=1 From eadc7c0ecc2f1dc21b7554f207824ce4648b5eb9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 29 Jul 2024 13:39:23 +1000 Subject: [PATCH 59/77] Tools: install_prereqs_ubuntu.sh: pin setuptools to avoid problems on Focal focal: return Version(version) focal: File "/home/vagrant/.local/lib/python3.8/site-packages/packaging/version.py", line 202, in __init__ focal: raise InvalidVersion(f"Invalid version: '{version}'") focal: packaging.version.InvalidVersion: Invalid version: '1.0.3.linux-x86_64' --- Tools/environment_install/install-prereqs-ubuntu.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index c1aabcbabba8aa..5dece4504fb242 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -400,7 +400,11 @@ if [ -n "$PYTHON_VENV_PACKAGE" ]; then fi # try update packaging, setuptools and wheel before installing pip package that may need compilation -$PIP install $PIP_USER_ARGUMENT -U pip packaging setuptools wheel +SETUPTOOLS="setuptools" +if [ ${RELEASE_CODENAME} == 'focal' ]; then + SETUPTOOLS=setuptools==70.3.0 +fi +$PIP install $PIP_USER_ARGUMENT -U pip packaging $SETUPTOOLS wheel if [ "$GITHUB_ACTIONS" == "true" ]; then PIP_USER_ARGUMENT+=" --progress-bar off" From 1e6e291b527a6186e0548be6133642a0722a7a2a Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 21 May 2024 19:34:43 +0200 Subject: [PATCH 60/77] autotest: New tests Autotests for takeoffs have been added for Plane, covering AUTO and TAKEOFF mode takeoffs. An auxiliary `set_servo` method has been added to `vehicle_test_suite.py`. --- .../ArduPlane_Tests/TakeoffAuto1/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto2/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto3/catapult.txt | 13 + .../ArduPlane_Tests/TakeoffAuto4/catapult.txt | 13 + Tools/autotest/arduplane.py | 350 ++++++++++++++++++ Tools/autotest/vehicle_test_suite.py | 4 + 6 files changed, 406 insertions(+) create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt create mode 100644 Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto1/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto2/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto3/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt new file mode 100644 index 00000000000000..0aa9cd624db827 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/TakeoffAuto4/catapult.txt @@ -0,0 +1,13 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 +1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 100.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 100.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 100.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index cf319f97da59a4..45a8f9ddf97c0f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4371,6 +4371,349 @@ def LandingDrift(self): self.wait_disarmed(timeout=180) + def TakeoffAuto1(self): + '''Test the behaviour of an AUTO takeoff, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_TYPE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_THR_MAX": 80.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto2(self): + '''Test the behaviour of an AUTO takeoff, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX > THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Wait until we're midway through the climb. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + + # Ensure that by then the aircraft still goes at max allowed throttle. + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto3(self): + '''Test the behaviour of an AUTO takeoff, pt3. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")-1)) + + # Ensure that after that the aircraft does not go full throttle anymore. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")-10, operator.lt) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffAuto4(self): + '''Test the behaviour of an AUTO takeoff, pt4. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=1 + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 80.0, + "THR_MIN": 0.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MAX": 100.0, + "TKOFF_THR_MINACC": 3.0, + "TECS_PITCH_MAX": 35.0, + "TKOFF_THR_MAX_T": 3.0, + "PTCH_LIM_MAX_DEG": 35.0, + "RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item. + }) + + # Load and start mission. It contains a MAV_CMD_NAV_TAKEOFF item at 100m. + self.load_mission("catapult.txt", strict=True) + self.change_mode('AUTO') + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Ensure that TKOFF_THR_MAX_T is respected. + self.delay_sim_time(self.get_parameter("TKOFF_THR_MAX_T")-1) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Ensure that after that the aircraft still goes to maximum throttle. + test_alt = 50 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-10, operator.ge) + + # Wait for landing waypoint. + self.wait_current_waypoint(11, timeout=1200) + self.wait_disarmed(120) + + def TakeoffTakeoff1(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt1. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff2(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt2. + + Conditions: + - ARSPD_USE=1 + - TKOFF_MODE=1 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 1.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 80.0, + "TKOFF_ALT": 150.0, + "TKOFF_MODE": 1.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Check whether we've receded from max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MIN"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + + def TakeoffTakeoff3(self): + '''Test the behaviour of a takeoff in TAKEOFF mode, pt3. + + This is the same as case #1, but with disabled airspeed sensor. + + Conditions: + - ARSPD_USE=0 + - TKOFF_MODE=0 + - TKOFF_THR_MAX < THR_MAX + ''' + + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "ARSPD_USE": 0.0, + "THR_MAX": 100.0, + "TKOFF_LVL_ALT": 30.0, + "TKOFF_ALT": 100.0, + "TKOFF_MODE": 0.0, + "TKOFF_THR_MINACC": 3.0, + "TKOFF_THR_MAX": 80.0, + "TECS_PITCH_MAX": 35.0, + "PTCH_LIM_MAX_DEG": 35.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Throw the catapult. + self.set_servo(7, 2000) + + # Check whether we're at max throttle below TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")-10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*self.get_parameter("TKOFF_THR_MAX")) + + # Check whether we're still at max throttle past TKOFF_LVL_ALT. + test_alt = self.get_parameter("TKOFF_LVL_ALT")+10 + self.wait_altitude(test_alt, test_alt+2, relative=True) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX")), operator.le) + self.assert_servo_channel_value(3, 1000+10*(self.get_parameter("TKOFF_THR_MAX"))-1, operator.ge) + + # Wait for the takeoff to complete. + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -5712,6 +6055,13 @@ def tests(self): self.AHRS_ORIENTATION, self.AHRSTrim, self.LandingDrift, + self.TakeoffAuto1, + self.TakeoffAuto2, + self.TakeoffAuto3, + self.TakeoffAuto4, + self.TakeoffTakeoff1, + self.TakeoffTakeoff2, + self.TakeoffTakeoff3, self.ForcedDCM, self.DCMFallback, self.MAVFTP, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index f3458fee9d9575..19b9f946d92cbe 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -5571,6 +5571,10 @@ def set_rc(self, chan, pwm, timeout=20): """Setup a simulated RC control to a PWM value""" self.set_rc_from_map({chan: pwm}, timeout=timeout) + def set_servo(self, chan, pwm): + """Replicate the functionality of MAVProxy: servo set """ + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, p1=chan, p2=pwm) + def location_offset_ne(self, location, north, east): '''move location in metres''' print("old: %f %f" % (location.lat, location.lng)) From b163e139642bac450f45dfed9155009ff62ed0de Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 24 May 2024 11:38:42 +0200 Subject: [PATCH 61/77] AP_TECS: Fixes to reset state These concern the TAKEOFF flight stage and address #27147. * Logging metadata fixes * Disabled continuous TECS reset during takeoff * Fixed bug in reached_speed_takeoff calculation * Limited SPDWEIGHT=2 to only first part of takeoff climb * _post_TO_hgt_offset set to constant * Takeoff I-gain now defaults to 0 and is used * Use at least TRIM_THROTTLE during the climb * Updated description of TECS_TKOFF_IGAIN with new behaviour * Forced max throttle while below TKOFF_LVL_ALT * Added throttle constraints in no-airspeed mode Additionally, set_min_throttle() has been created, that allows one to set the minimum TECS throttle for the next update_pitch_throttle() loop. Additionally, the throttle limits system has been reworked. TECS will receive external throttle limits from Plane and will always take them into account and respect them. --- libraries/AP_TECS/AP_TECS.cpp | 148 +++++++++++++++++++--------- libraries/AP_TECS/AP_TECS.h | 20 ++++ libraries/AP_Vehicle/AP_FixedWing.h | 2 + 3 files changed, 126 insertions(+), 44 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 55fb024043b1da..9d1d00e460e712 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -222,7 +222,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: TKOFF_IGAIN // @DisplayName: Controller integrator during takeoff - // @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best + // @Description: This is the integrator gain on the control loop during takeoff. Increase to increase the rate at which speed and height offsets are trimmed out. // @Range: 0.0 0.5 // @Increment: 0.02 // @User: Advanced @@ -779,7 +779,6 @@ void AP_TECS::_update_throttle_with_airspeed(void) // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); } - _integTHR_state = integ_max; } else { _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); } @@ -827,7 +826,12 @@ void AP_TECS::_update_throttle_with_airspeed(void) #endif } - // Constrain throttle demand and record clipping + constrain_throttle(); + +} + +// Constrain throttle demand and record clipping +void AP_TECS::constrain_throttle() { if (_throttle_dem > _THRmaxf) { _thr_clip_status = clipStatus::MAX; _throttle_dem = _THRmaxf; @@ -843,9 +847,7 @@ float AP_TECS::_get_i_gain(void) { float i_gain = _integGain; if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!is_zero(_integGain_takeoff)) { - i_gain = _integGain_takeoff; - } + i_gain = _integGain_takeoff; } else if (_flags.is_doing_auto_land) { if (!is_zero(_integGain_land)) { i_gain = _integGain_land; @@ -893,18 +895,6 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi _throttle_dem = nomThr; } - if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { - const uint32_t now = AP_HAL::millis(); - if (_takeoff_start_ms == 0) { - _takeoff_start_ms = now; - } - const uint32_t dt = now - _takeoff_start_ms; - if (dt*0.001 < aparm.takeoff_throttle_max_t) { - _throttle_dem = _THRmaxf; - } - } else { - _takeoff_start_ms = 0; - } if (_flags.is_gliding) { _throttle_dem = 0.0f; return; @@ -918,6 +908,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); + + constrain_throttle(); } void AP_TECS::_detect_bad_descent(void) @@ -1088,17 +1080,17 @@ void AP_TECS::_update_pitch(void) // @Field: PEW: Potential energy weighting // @Field: KEW: Kinetic energy weighting // @Field: EBD: Energy balance demand - // @Field: EBE: Energy balance error + // @Field: EBE: Energy balance estimate // @Field: EBDD: Energy balance rate demand - // @Field: EBDE: Energy balance rate error + // @Field: EBDE: Energy balance rate estimate // @Field: EBDDT: Energy balance rate demand + Energy balance rate error*pitch_damping // @Field: Imin: Minimum integrator value // @Field: Imax: Maximum integrator value // @Field: I: Energy balance error integral // @Field: KI: Pitch demand kinetic energy integral - // @Field: pmin: Pitch min - // @Field: pmax: Pitch max - AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,pmin,pmax", + // @Field: tmin: Throttle min + // @Field: tmax: Throttle max + AP::logger().WriteStreaming("TEC2","TimeUS,PEW,KEW,EBD,EBE,EBDD,EBDE,EBDDT,Imin,Imax,I,KI,tmin,tmax", "Qfffffffffffff", AP_HAL::micros64(), (double)SPE_weighting, @@ -1112,8 +1104,8 @@ void AP_TECS::_update_pitch(void) (double)integSEBdot_max, (double)_integSEBdot, (double)_integKE, - (double)_PITCHminf, - (double)_PITCHmaxf); + (double)_THRminf, + (double)_THRmaxf); } #endif } @@ -1172,22 +1164,25 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _hgt_dem = hgt_afe; _hgt_dem_in_prev = hgt_afe; _hgt_dem_in_raw = hgt_afe; - _TAS_dem_adj = _TAS_dem; - _flags.reset = true; - _post_TO_hgt_offset = _climb_rate * _hgt_dem_tconst; _flags.underspeed = false; _flags.badDescent = false; - + _post_TO_hgt_offset = _climb_rate_limit * _hgt_dem_tconst; // Replacement prevents oscillating hgt_rate_dem. + _TAS_dem_adj = _TAS_dem; _max_climb_scaler = 1.0f; _max_sink_scaler = 1.0f; - _pitch_demand_lpf.reset(_ahrs.get_pitch()); _pitch_measured_lpf.reset(_ahrs.get_pitch()); + + if (!_flag_have_reset_after_takeoff) { + _flags.reset = true; + _flag_have_reset_after_takeoff = true; + } } if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; + _flag_have_reset_after_takeoff = false; } } @@ -1252,16 +1247,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _hgt_dem_in = _hgt_dem_in_raw; } - if (aparm.takeoff_throttle_max != 0 && - (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING)) { - _THRmaxf = aparm.takeoff_throttle_max * 0.01f; - } else { - _THRmaxf = aparm.throttle_max * 0.01f; - } - _THRminf = aparm.throttle_min * 0.01f; - - // min of 1% throttle range to prevent a numerical error - _THRmaxf = MAX(_THRmaxf, _THRminf+0.01); + // Update the throttle limits. + _update_throttle_limits(); // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use @@ -1312,9 +1299,6 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // note that this allows a flare pitch outside the normal TECS auto limits _PITCHmaxf = _land_pitch_max; } - - // and allow zero throttle - _THRminf = 0; } else if (_landing.is_on_approach()) { _PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min); _pitch_min_at_flare_entry = _PITCHminf; @@ -1339,7 +1323,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { - if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { + if (!_flags.reached_speed_takeoff && _TAS_state >= _TASmin && _TASmin > 0) { // we have reached our target speed in takeoff, allow for // normal throttle control _flags.reached_speed_takeoff = true; @@ -1440,3 +1424,79 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } #endif } + +// set minimum throttle override, [-1, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_min(const float thr_min) { + // Don't change the limit if it is already covered. + if (thr_min > _THRminf_ext) { + _THRminf_ext = thr_min; + } +} + +// set minimum throttle override, [0, -1] range +// it is applicable for one control cycle only +void AP_TECS::set_throttle_max(const float thr_max) { + // Don't change the limit if it is already covered. + if (thr_max < _THRmaxf_ext) { + _THRmaxf_ext = thr_max; + } +} + +void AP_TECS::_update_throttle_limits() { + + // Configure max throttle. + + // Read the maximum throttle limit. + _THRmaxf = aparm.throttle_max * 0.01f; + // If more max throttle is allowed during takeoff, use it. + if (aparm.takeoff_throttle_max*0.01f > _THRmaxf + && (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) + ) { + _THRmaxf = aparm.takeoff_throttle_max * 0.01f; + } + // In any case, constrain to the external safety limits. + _THRmaxf = MIN(_THRmaxf, _THRmaxf_ext); + + // Configure min throttle. + + // If less min throttle is allowed during takeoff, use it. + bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; + use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_mode == 1) && (aparm.takeoff_throttle_min != 0); + if ( use_takeoff_throttle ) { + _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f); + } + else { // Otherwise, during normal situations let regular limit. + _THRminf = aparm.throttle_min * 0.01f; + } + // Raise min to force max throttle for TKOFF_THR_MAX_T after a takeoff. + if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { + const uint32_t now = AP_HAL::millis(); + if (_takeoff_start_ms == 0) { + _takeoff_start_ms = now; + } + const uint32_t dt = now - _takeoff_start_ms; + if (dt*0.001 < aparm.takeoff_throttle_max_t) { + _THRminf = _THRmaxf; + } + } else { + _takeoff_start_ms = 0; + } + // If we are flaring, allow the throttle to go to 0. + if (_landing.is_flaring()) { + _THRminf = 0; + } + // In any case, constrain to the external safety limits. + _THRminf = MAX(_THRminf, _THRminf_ext); + + // Allow a minimum of 1% throttle range, primarily to prevent TECS numerical errors. + if (_THRmaxf < 1) { + _THRmaxf = MAX(_THRmaxf, _THRminf + 0.01f); + } else { + _THRminf = MIN(_THRminf, _THRmaxf - 0.01f); + } + + // Reset the external throttle limits. + _THRminf_ext = -1.0f; + _THRmaxf_ext = 1.0f; +} \ No newline at end of file diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index fcd3fe97f9bfe7..8ffff211fbf032 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -134,6 +134,14 @@ class AP_TECS { _pitch_max_limit = pitch_limit; } + // set minimum throttle override, [-1, -1] range + // it is applicable for one control cycle only + void set_throttle_min(const float thr_min); + + // set minimum throttle override, [0, -1] range + // it is applicable for one control cycle only + void set_throttle_max(const float thr_max); + // force use of synthetic airspeed for one loop void use_synthetic_airspeed(void) { _use_synthetic_airspeed_once = true; @@ -360,6 +368,9 @@ class AP_TECS { // Maximum and minimum floating point throttle limits float _THRmaxf; float _THRminf; + // Maximum and minimum throttle safety limits, set externally, typically by servos.cpp:apply_throttle_limits() + float _THRmaxf_ext = 1.0f; + float _THRminf_ext = -1.0f; // Maximum and minimum floating point pitch limits float _PITCHmaxf; @@ -419,6 +430,9 @@ class AP_TECS { // need to reset on next loop bool _need_reset; + // Checks if we reset at the beginning of takeoff. + bool _flag_have_reset_after_takeoff; + float _SKE_weighting; AP_Int8 _use_synthetic_airspeed; @@ -458,6 +472,9 @@ class AP_TECS { // Update Demanded Throttle Non-Airspeed void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg); + // Constrain throttle demand and record clipping + void constrain_throttle(); + // get integral gain which is flight_stage dependent float _get_i_gain(void); @@ -478,4 +495,7 @@ class AP_TECS { // current time constant float timeConstant(void) const; + + // Update the allowable throttle range. + void _update_throttle_limits(); }; diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index df3692a61b6552..ee51dcf1cdc125 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -11,6 +11,8 @@ struct AP_FixedWing { AP_Int8 throttle_slewrate; AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; + AP_Int8 takeoff_throttle_min; + AP_Int8 takeoff_mode; AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; From 773c91cec11cf61e6d97f3a80f120abb733a7488 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Fri, 24 May 2024 11:38:42 +0200 Subject: [PATCH 62/77] ArduPlane: Added minimum throttle during TAKEOFF mode This is a rework so that servos.cpp is responsible for setting the throttle limits under more circumstances and always notifies TECS when it does so. Additionally, the TAKEOFF mode has been improved with a new parameters TKOFF_MODE and TKOFF_THR_MIN that extend the throttle behaviour. --- ArduPlane/Attitude.cpp | 5 +++++ ArduPlane/Parameters.cpp | 18 ++++++++++++++++- ArduPlane/Parameters.h | 2 ++ ArduPlane/Plane.h | 1 + ArduPlane/altitude.cpp | 4 ++-- ArduPlane/commands_logic.cpp | 2 +- ArduPlane/mode_takeoff.cpp | 10 ++++----- ArduPlane/servos.cpp | 39 +++++++++++++++++++++++++++++------- ArduPlane/takeoff.cpp | 23 ++++++++++++++++++++- 9 files changed, 87 insertions(+), 17 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 0996a50b725ab4..788fe97dbada6d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -448,6 +448,10 @@ void Plane::stabilize() } +/* + * Set the throttle output. + * This is called by TECS-enabled flight modes, e.g. AUTO, GUIDED, etc. +*/ void Plane::calc_throttle() { if (aparm.throttle_cruise <= 1) { @@ -458,6 +462,7 @@ void Plane::calc_throttle() return; } + // Read the TECS throttle output and set it to the throttle channel. float commanded_throttle = TECS_controller.get_throttle_demand(); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 7ff30ad4277175..8c1c40e4f11443 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -142,12 +142,28 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MAX_T // @DisplayName: Takeoff throttle maximum time - // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff without an airspeed sensor. If an airspeed sensor is being used then the throttle is set to maximum until the takeoff airspeed is reached. + // @Description: This sets the time that maximum throttle will be forced during a fixed wing takeoff. // @Units: s // @Range: 0 10 // @Increment: 0.5 // @User: Standard ASCALAR(takeoff_throttle_max_t, "TKOFF_THR_MAX_T", 4), + + // @Param: TKOFF_THR_MIN + // @DisplayName: Takeoff minimum throttle + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_MODE=1. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), + + // @Param: TKOFF_MODE + // @DisplayName: Takeoff mode + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 0: During the takeoff, the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX). 1: During the takeoff TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. + // @Values: 0:Traditional,1:Throttle range + // @User: Advanced + ASCALAR(takeoff_mode, "TKOFF_MODE", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index de514b891532cc..f041ef76683773 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -357,6 +357,8 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + k_param_takeoff_throttle_min, + k_param_takeoff_mode, }; AP_Int16 format_version; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 443f2d56a1fb4b..10bca62c8a7b82 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1117,6 +1117,7 @@ class Plane : public AP_Vehicle { bool auto_takeoff_check(void); void takeoff_calc_roll(void); void takeoff_calc_pitch(void); + void takeoff_calc_throttle(const bool use_max_throttle=false); int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 6e32f619439d5b..5b845f38a86646 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -520,9 +520,9 @@ int32_t Plane::adjusted_altitude_cm(void) } /* - return home-relative altitude adjusted for ALT_OFFSET This is useful + return home-relative altitude adjusted for ALT_OFFSET. This is useful during long flights to account for barometer changes from the GCS, - or to adjust the flying height of a long mission + or to adjust the flying height of a long mission. */ int32_t Plane::adjusted_relative_altitude_cm(void) { diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 57969f17ce8a4b..a843901b34fb4b 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -376,7 +376,7 @@ void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd) next_WP_loc.lat = home.lat + 10; next_WP_loc.lng = home.lng + 10; auto_state.takeoff_speed_time_ms = 0; - auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction + auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction. auto_state.height_below_takeoff_to_level_off_cm = 0; // Flag also used to override "on the ground" throttle disable diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index 0854e512450a94..325e8cab2db2dc 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -22,11 +22,11 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Increment: 1 // @Units: m // @User: Standard - AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 5), + AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10), // @Param: LVL_PITCH // @DisplayName: Takeoff mode altitude initial pitch - // @Description: This is the target pitch for the initial climb to TKOFF_LVL_ALT + // @Description: This is the target pitch during the takeoff. // @Range: 0 30 // @Increment: 1 // @Units: deg @@ -149,11 +149,11 @@ void ModeTakeoff::update() if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) { //below TAKOFF_LVL_ALT - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 100.0); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); + plane.takeoff_calc_throttle(true); } else { - if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT ,or above and loitering + if ((altitude_cm >= alt * 100 - 200)) { //within 2m of TKOFF_ALT, or above and loitering #if AP_FENCE_ENABLED if (!plane.have_autoenabled_fences) { plane.fence.auto_enable_fence_after_takeoff(); @@ -164,7 +164,7 @@ void ModeTakeoff::update() plane.calc_nav_pitch(); plane.calc_throttle(); } else { // still climbing to TAKEOFF_ALT; may be loitering - plane.calc_throttle(); + plane.takeoff_calc_throttle(); plane.takeoff_calc_roll(); plane.takeoff_calc_pitch(); } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cc4d4dde104e38..6ac1baba77c53c 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -499,47 +499,72 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) #endif // #if AP_BATTERY_WATT_MAX_ENABLED /* - Apply min/max limits to throttle + Apply min/max safety limits to throttle. */ float Plane::apply_throttle_limits(float throttle_in) { - // convert 0 to 100% (or -100 to +100) into PWM + // Pull the base throttle limits. + // These are usually set to map the ESC operating range. int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); #if AP_ICENGINE_ENABLED - // apply idle governor + // Apply idle governor. g2.ice_control.update_idle_governor(min_throttle); #endif + // If reverse thrust is enabled not allowed right now, the minimum throttle must not fall below 0. if (min_throttle < 0 && !allow_reverse_thrust()) { // reverse thrust is available but inhibited. min_throttle = 0; } - const bool use_takeoff_throttle_max = + // Query the conditions where TKOFF_THR_MAX applies. + const bool use_takeoff_throttle = #if HAL_QUADPLANE_ENABLED quadplane.in_transition() || #endif (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) || (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING); - if (use_takeoff_throttle_max) { + if (use_takeoff_throttle) { if (aparm.takeoff_throttle_max != 0) { + // Replace max throttle with the takeoff max throttle setting. + // This is typically done to protect against long intervals of large power draw. + // Or (in contrast) to give some extra throttle during the initial climb. max_throttle = aparm.takeoff_throttle_max.get(); } + // Do not allow min throttle to go below a lower threshold. + // This is typically done to protect against premature stalls close to the ground. + if (aparm.takeoff_mode.get() == 0 || !ahrs.using_airspeed_sensor()) { + // Use a constant max throttle throughout the takeoff or when airspeed readings are not available. + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); + } else if (aparm.takeoff_mode.get() == 1) { // Use a throttle range through the takeoff. + if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1. + min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); + } + } } else if (landing.is_flaring()) { + // Allow throttle cutoff when flaring. + // This is to allow the aircraft to bleed speed faster and land with a shut off thruster. min_throttle = 0; } - // compensate for battery voltage drop + // Compensate the limits for battery voltage drop. + // This relaxes the limits when the battery is getting depleted. g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED - // apply watt limiter + // Ensure that the power draw limits are not exceeded. throttle_watt_limiter(min_throttle, max_throttle); #endif + // Do a sanity check on them. Constrain down if necessary. + min_throttle = MIN(min_throttle, max_throttle); + + // Let TECS know about the updated throttle limits. + TECS_controller.set_throttle_min(0.01f*min_throttle); + TECS_controller.set_throttle_max(0.01f*max_throttle); return constrain_float(throttle_in, min_throttle, max_throttle); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index faa220412fd914..d6e8ddc4f533d4 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -219,7 +219,28 @@ void Plane::takeoff_calc_pitch(void) } /* - * get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off + * Set the throttle limits to run at during a takeoff. + */ +void Plane::takeoff_calc_throttle(const bool use_max_throttle) { + // This setting will take effect at the next run of TECS::update_pitch_throttle(). + + // Set the maximum throttle limit. + if (aparm.takeoff_throttle_max != 0) { + TECS_controller.set_throttle_max(0.01f*aparm.takeoff_throttle_max); + } + + // Set the minimum throttle limit. + if (aparm.takeoff_mode==0 || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); + } else { // TKOFF_MODE == 1, allow for a throttle range. + if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. + TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_min); + } + } + calc_throttle(); +} + +/* get the pitch min used during takeoff. This matches the mission pitch until near the end where it allows it to levels off */ int16_t Plane::get_takeoff_pitch_min_cd(void) { From c213ee2ef831ac0e6c37fc11e0c825f19f7f1592 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:28:59 +0200 Subject: [PATCH 63/77] Plane: Converted paramter TKOFF_MODE into TKOFF_OPTIONS --- ArduPlane/Parameters.cpp | 12 ++++++------ ArduPlane/Parameters.h | 2 +- ArduPlane/servos.cpp | 5 +++-- ArduPlane/takeoff.cpp | 3 ++- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 8c1c40e4f11443..cc6bc6e6ee626b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -151,19 +151,19 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TKOFF_THR_MIN // @DisplayName: Takeoff minimum throttle - // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_MODE=1. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. + // @Description: The minimum throttle to use in takeoffs in AUTO and TAKEOFF flight modes, when TKOFF_OPTIONS bit 0 is set. Also, the minimum throttle to use in a quadpane forward transition. This can be useful to ensure faster takeoffs or transitions on aircraft where the normal throttle control leads to a slow takeoff or transition. It is used when it is larger than THR_MIN, otherwise THR_MIN is used instead. // @Units: % // @Range: 0 100 // @Increment: 1 // @User: Standard ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 60), - // @Param: TKOFF_MODE - // @DisplayName: Takeoff mode - // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. 0: During the takeoff, the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX). 1: During the takeoff TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. - // @Values: 0:Traditional,1:Throttle range + // @Param: TKOFF_OPTIONS + // @DisplayName: Takeoff options + // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. + // @Bitmask: 0: When unset the maximum allowed throttle is always used (THR_MAX or TKOFF_THR_MAX) during takeoff. When set TECS is allowed to operate between a minimum (THR_MIN or TKOFF_THR_MIN) and a maximum (THR_MAX or TKOFF_THR_MAX) limit. Applicable only when using an airspeed sensor. // @User: Advanced - ASCALAR(takeoff_mode, "TKOFF_MODE", 0), + ASCALAR(takeoff_options, "TKOFF_OPTIONS", 0), // @Param: TKOFF_TDRAG_ELEV // @DisplayName: Takeoff tail dragger elevator diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f041ef76683773..c06fcbafc0a077 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -358,7 +358,7 @@ class Parameters { k_param_takeoff_throttle_max_t, k_param_autotune_options, k_param_takeoff_throttle_min, - k_param_takeoff_mode, + k_param_takeoff_options, }; AP_Int16 format_version; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 6ac1baba77c53c..ab3dd86ae25080 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -536,10 +536,11 @@ float Plane::apply_throttle_limits(float throttle_in) } // Do not allow min throttle to go below a lower threshold. // This is typically done to protect against premature stalls close to the ground. - if (aparm.takeoff_mode.get() == 0 || !ahrs.using_airspeed_sensor()) { + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + if (!use_throttle_range || !ahrs.using_airspeed_sensor()) { // Use a constant max throttle throughout the takeoff or when airspeed readings are not available. min_throttle = MAX(min_throttle, aparm.takeoff_throttle_max.get()); - } else if (aparm.takeoff_mode.get() == 1) { // Use a throttle range through the takeoff. + } else if (use_throttle_range) { // Use a throttle range through the takeoff. if (aparm.takeoff_throttle_min.get() != 0) { // This is enabled by TKOFF_MODE==1. min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get()); } diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index d6e8ddc4f533d4..d45651fbfe8853 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -230,7 +230,8 @@ void Plane::takeoff_calc_throttle(const bool use_max_throttle) { } // Set the minimum throttle limit. - if (aparm.takeoff_mode==0 || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + if (!use_throttle_range || !ahrs.using_airspeed_sensor() || use_max_throttle) { // Traditional takeoff throttle limit. TECS_controller.set_throttle_min(0.01f*aparm.takeoff_throttle_max); } else { // TKOFF_MODE == 1, allow for a throttle range. if (aparm.takeoff_throttle_min != 0) { // Override THR_MIN. From e227187e72a1c277576df57e1e413a7b2bbce187 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:29:22 +0200 Subject: [PATCH 64/77] AP_TECS: Converted parameter TKOFF_MODE to TKOFF_OPTIONS --- libraries/AP_TECS/AP_TECS.cpp | 3 ++- libraries/AP_Vehicle/AP_FixedWing.h | 7 ++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 9d1d00e460e712..e3843a02aeabcf 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1462,7 +1462,8 @@ void AP_TECS::_update_throttle_limits() { // If less min throttle is allowed during takeoff, use it. bool use_takeoff_throttle = _flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING; - use_takeoff_throttle = use_takeoff_throttle && (aparm.takeoff_mode == 1) && (aparm.takeoff_throttle_min != 0); + const bool use_throttle_range = (aparm.takeoff_options & (uint32_t)AP_FixedWing::TakeoffOption::THROTTLE_RANGE); + use_takeoff_throttle = use_takeoff_throttle && (use_throttle_range == 1) && (aparm.takeoff_throttle_min != 0); if ( use_takeoff_throttle ) { _THRminf = MIN(_THRminf, aparm.takeoff_throttle_min * 0.01f); } diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index ee51dcf1cdc125..722c53dcaf9af8 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -12,7 +12,7 @@ struct AP_FixedWing { AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; AP_Int8 takeoff_throttle_min; - AP_Int8 takeoff_mode; + AP_Int32 takeoff_options; AP_Int16 airspeed_min; AP_Int16 airspeed_max; AP_Float airspeed_cruise; @@ -51,4 +51,9 @@ struct AP_FixedWing { LAND = 4, ABORT_LANDING = 7 }; + + // Bitfields of TKOFF_OPTIONS + enum class TakeoffOption { + THROTTLE_RANGE = (1U << 0), // Unset: Max throttle. Set: Throttle range. + }; }; From b5c91a16908804290efa6ca3efaf4a448041424e Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 25 Jul 2024 21:29:35 +0200 Subject: [PATCH 65/77] autotest: Converted parameter TKOFF_MODE to TKOFF_OPTIONS --- Tools/autotest/arduplane.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 45a8f9ddf97c0f..2af157ab9c8ce2 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4376,7 +4376,7 @@ def TakeoffAuto1(self): Conditions: - ARSPD_USE=1 - - TKOFF_TYPE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4421,7 +4421,7 @@ def TakeoffAuto2(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX > THR_MAX ''' @@ -4466,7 +4466,7 @@ def TakeoffAuto3(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 ''' self.customise_SITL_commandline( @@ -4478,7 +4478,7 @@ def TakeoffAuto3(self): "ARSPD_USE": 1.0, "THR_MAX": 80.0, "THR_MIN": 0.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MAX": 100.0, "TKOFF_THR_MINACC": 3.0, "TECS_PITCH_MAX": 35.0, @@ -4515,7 +4515,7 @@ def TakeoffAuto4(self): Conditions: - ARSPD_USE=0 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 ''' self.customise_SITL_commandline( @@ -4527,7 +4527,7 @@ def TakeoffAuto4(self): "ARSPD_USE": 0.0, "THR_MAX": 80.0, "THR_MIN": 0.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MAX": 100.0, "TKOFF_THR_MINACC": 3.0, "TECS_PITCH_MAX": 35.0, @@ -4566,7 +4566,7 @@ def TakeoffTakeoff1(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4580,7 +4580,7 @@ def TakeoffTakeoff1(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 30.0, "TKOFF_ALT": 100.0, - "TKOFF_MODE": 0.0, + "TKOFF_OPTIONS": 0.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0, @@ -4616,7 +4616,7 @@ def TakeoffTakeoff2(self): Conditions: - ARSPD_USE=1 - - TKOFF_MODE=1 + - TKOFF_OPTIONS[0]=1 - TKOFF_THR_MAX < THR_MAX ''' @@ -4630,7 +4630,7 @@ def TakeoffTakeoff2(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 80.0, "TKOFF_ALT": 150.0, - "TKOFF_MODE": 1.0, + "TKOFF_OPTIONS": 1.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0, @@ -4669,7 +4669,7 @@ def TakeoffTakeoff3(self): Conditions: - ARSPD_USE=0 - - TKOFF_MODE=0 + - TKOFF_OPTIONS[0]=0 - TKOFF_THR_MAX < THR_MAX ''' @@ -4683,7 +4683,7 @@ def TakeoffTakeoff3(self): "THR_MAX": 100.0, "TKOFF_LVL_ALT": 30.0, "TKOFF_ALT": 100.0, - "TKOFF_MODE": 0.0, + "TKOFF_OPTIONS": 0.0, "TKOFF_THR_MINACC": 3.0, "TKOFF_THR_MAX": 80.0, "TECS_PITCH_MAX": 35.0, From d4ca5fe8687963e65a83044a4858782f818d1f7c Mon Sep 17 00:00:00 2001 From: BLash Date: Wed, 17 Jul 2024 15:30:47 -0500 Subject: [PATCH 66/77] AP_ExternalAHRS_VectorNav: Split IMU and EKF message In AHRS Mode, split the single message to an IMU packet and an AHRS packet; in EKF Mode, split the two messages into an IMU message, an EKF packet, and a GNSS packet. Simplify message header definition to consolidate and eliminate the need for static asserts Update healthy logic and use to represent new packet structure Replace EAH3 message with messages per-packet Add Ypr as configured output in the EKF message --- .../AP_ExternalAHRS_VectorNav.cpp | 648 +++++++++--------- .../AP_ExternalAHRS_VectorNav.h | 17 +- 2 files changed, 340 insertions(+), 325 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index aef7fe26528a97..aeefe9ff54d45b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -16,6 +16,7 @@ support for serial connected AHRS systems */ +#include #define ALLOW_DOUBLE_MATH_FUNCTIONS #include "AP_ExternalAHRS_config.h" @@ -38,136 +39,138 @@ extern const AP_HAL::HAL &hal; -/* - header for 50Hz INS data - assumes the following config for VN-300: - $VNWRG,75,3,8,34,072E,0106,0612*0C - - 0x34: Groups 3,5,6 - Group 3 (IMU): - 0x072E: - UncompMag - UncompAccel - UncompGyro - Pres - Mag - Accel - AngularRate - Group 5 (Attitude): - 0x0106: - YawPitchRoll - Quaternion - YprU - Group 6 (INS): - 0x0612: - PosLLa - VelNed - PosU - VelU +/* +TYPE::VN_AHRS configures 2 packets: high-rate IMU and mid-rate EKF +Header for IMU packet + $VNWRG,75,3,16,01,0721*D415 + Common group (Group 1) + TimeStartup + AngularRate + Accel + Imu + MagPres +Header for EKF packet + $VNWRG,76,3,16,11,0001,0106*B36B + Common group (Group 1) + TimeStartup + Attitude group (Group 4) + Ypr + Quaternion + YprU */ -static const uint8_t vn_ins_pkt1_header[] { 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 }; -#define VN_INS_PKT1_LENGTH 170 // includes header and CRC -struct PACKED VN_INS_packet1 { - float uncompMag[3]; +struct PACKED VN_IMU_packet { + static constexpr uint8_t header[]{0x01, 0x21, 0x07}; + uint64_t timeStartup; + float gyro[3]; + float accel[3]; float uncompAccel[3]; float uncompAngRate[3]; - float pressure; float mag[3]; - float accel[3]; - float gyro[3]; + float temp; + float pressure; +}; +constexpr uint8_t VN_IMU_packet::header[]; +constexpr uint8_t VN_IMU_LENGTH = sizeof(VN_IMU_packet) + sizeof(VN_IMU_packet::header) + 1 + 2; // Includes sync byte and CRC + +struct PACKED VN_AHRS_ekf_packet { + static constexpr uint8_t header[]{0x11, 0x01, 0x00, 0x06, 0x01}; + uint64_t timeStartup; float ypr[3]; float quaternion[4]; float yprU[3]; - double positionLLA[3]; - float velNED[3]; - float posU; - float velU; }; - -// check packet size for 4 groups -static_assert(sizeof(VN_INS_packet1)+2+3*2+2 == VN_INS_PKT1_LENGTH, "incorrect VN_INS_packet1 length"); +constexpr uint8_t VN_AHRS_ekf_packet::header[]; +constexpr uint8_t VN_AHRS_EKF_LENGTH = sizeof(VN_AHRS_ekf_packet) + sizeof(VN_AHRS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC /* - header for 5Hz GNSS data - assumes the following VN-300 config: - $VNWRG,76,3,80,4E,0002,0010,20B8,0018*63 - - 0x4E: Groups 2,3,4,7 - Group 2 (Time): - 0x0002: - TimeGps - Group 3 (IMU): - 0x0010: - Temp - Group 4 (GPS1): - 0x20B8: - NumSats - Fix - PosLLa - VelNed - DOP - Group 7 (GPS2): - 0x0018: - NumSats - Fix +TYPE::VN_INS configures 3 packets: high-rate IMU, mid-rate EKF, and 5Hz GNSS +Header for IMU packet + $VNWRG,75,3,16,01,0721*D415 + Common group (Group 1) + TimeStartup + AngularRate + Accel + Imu + MagPres +Header for EKF packet + $VNWRG,76,3,16,31,0001,0106,0613*097A + Common group (Group 1) + TimeStartup + Attitude group (Group 4) + Ypr + Quaternion + YprU + Ins group (Group 5) + InsStatus + PosLla + VelNed + PosU + VelU +Header for GNSS packet + $VNWRG,77,1,160,49,0003,26B8,0018*4FD9 + Common group (Group 1) + TimeStartup + TimeGps + Gnss1 group (Group 3) + NumSats + GnssFix + GnssPosLla + GnssVelNed + PosU1 + VelU1 + GnssDop + Gnss2 group (Group 6) + NumSats + GnssFix */ -static const uint8_t vn_ins_pkt2_header[] { 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x00 }; -#define VN_INS_PKT2_LENGTH 92 // includes header and CRC -struct PACKED VN_INS_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; +union Ins_Status { + uint16_t _value; + struct { + uint16_t mode : 2; + uint16_t gnssFix : 1; + uint16_t resv1 : 2; + uint16_t imuErr : 1; + uint16_t magPresErr : 1; + uint16_t gnssErr : 1; + uint16_t resv2 : 1; + uint16_t gnssHeadingIns : 2; + }; }; -// check packet size for 4 groups -static_assert(sizeof(VN_INS_packet2)+2+4*2+2 == VN_INS_PKT2_LENGTH, "incorrect VN_INS_packet2 length"); - -/* -header for 50Hz IMU data, used in TYPE::VN_AHRS only - assumes the following VN-100 config: - $VNWRG,75,3,80,14,073E,0004*66 - - Alternate first packet for VN-100 - 0x14: Groups 3, 5 - Group 3 (IMU): - 0x073E: - UncompMag - UncompAccel - UncompGyro - Temp - Pres - Mag - Accel - Gyro - Group 5 (Attitude): - 0x0004: - Quaternion -*/ -static const uint8_t vn_ahrs_pkt1_header[] { 0x14, 0x3E, 0x07, 0x04, 0x00 }; -#define VN_AHRS_PKT1_LENGTH 104 // includes header and CRC - -struct PACKED VN_AHRS_packet1 { - float uncompMag[3]; - float uncompAccel[3]; - float uncompAngRate[3]; - float temp; - float pressure; - float mag[3]; - float accel[3]; - float gyro[3]; +struct PACKED VN_INS_ekf_packet { + static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06}; + uint64_t timeStartup; + float ypr[3]; float quaternion[4]; + float yprU[3]; + uint16_t insStatus; + double posLla[3]; + float velNed[3]; + float posU; + float velU; }; - -static_assert(sizeof(VN_AHRS_packet1)+2+2*2+2 == VN_AHRS_PKT1_LENGTH, "incorrect VN_AHRS_packet1 length"); +constexpr uint8_t VN_INS_ekf_packet::header[]; +constexpr uint8_t VN_INS_EKF_LENGTH = sizeof(VN_INS_ekf_packet) + sizeof(VN_INS_ekf_packet::header) + 1 + 2; // Includes sync byte and CRC + +struct PACKED VN_INS_gnss_packet { + static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00}; + uint64_t timeStartup; + uint64_t timeGps; + uint8_t numSats1; + uint8_t fix1; + double posLla1[3]; + float velNed1[3]; + float posU1[3]; + float velU1; + float dop1[7]; + uint8_t numSats2; + uint8_t fix2; +}; +constexpr uint8_t VN_INS_gnss_packet::header[]; +constexpr uint8_t VN_INS_GNSS_LENGTH = sizeof(VN_INS_gnss_packet) + sizeof(VN_INS_gnss_packet::header) + 1 + 2; // Includes sync byte and CRC // constructor AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, @@ -183,12 +186,13 @@ AP_ExternalAHRS_VectorNav::AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *_frontend, baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); - bufsize = MAX(MAX(VN_INS_PKT1_LENGTH, VN_INS_PKT2_LENGTH), VN_AHRS_PKT1_LENGTH); + bufsize = MAX(MAX(MAX(VN_IMU_LENGTH, VN_INS_EKF_LENGTH), VN_INS_GNSS_LENGTH), VN_AHRS_EKF_LENGTH); + pktbuf = NEW_NOTHROW uint8_t[bufsize]; - last_ins_pkt1 = NEW_NOTHROW VN_INS_packet1; - last_ins_pkt2 = NEW_NOTHROW VN_INS_packet2; + latest_ins_ekf_packet = NEW_NOTHROW VN_INS_ekf_packet; + latest_ins_gnss_packet = NEW_NOTHROW VN_INS_gnss_packet; - if (!pktbuf || !last_ins_pkt1 || !last_ins_pkt2) { + if (!pktbuf || !latest_ins_ekf_packet) { AP_BoardConfig::allocation_error("VectorNav ExternalAHRS"); } @@ -226,48 +230,57 @@ bool AP_ExternalAHRS_VectorNav::check_uart() bool match_header1 = false; bool match_header2 = false; bool match_header3 = false; + bool match_header4 = false; if (pktbuf[0] != SYNC_BYTE) { goto reset; } - if (type == TYPE::VN_INS) { - match_header1 = (0 == memcmp(&pktbuf[1], vn_ins_pkt1_header, MIN(sizeof(vn_ins_pkt1_header), unsigned(pktoffset-1)))); - match_header2 = (0 == memcmp(&pktbuf[1], vn_ins_pkt2_header, MIN(sizeof(vn_ins_pkt2_header), unsigned(pktoffset-1)))); + + match_header1 = (0 == memcmp(&pktbuf[1], VN_IMU_packet::header, MIN(sizeof(VN_IMU_packet::header), unsigned(pktoffset - 1)))); + if (type == TYPE::VN_AHRS) { + match_header2 = (0 == memcmp(&pktbuf[1], VN_AHRS_ekf_packet::header, MIN(sizeof(VN_AHRS_ekf_packet::header), unsigned(pktoffset - 1)))); } else { - match_header3 = (0 == memcmp(&pktbuf[1], vn_ahrs_pkt1_header, MIN(sizeof(vn_ahrs_pkt1_header), unsigned(pktoffset-1)))); + match_header3 = (0 == memcmp(&pktbuf[1], VN_INS_ekf_packet::header, MIN(sizeof(VN_INS_ekf_packet::header), unsigned(pktoffset - 1)))); + match_header4 = (0 == memcmp(&pktbuf[1], VN_INS_gnss_packet::header, MIN(sizeof(VN_INS_gnss_packet::header), unsigned(pktoffset - 1)))); } - if (!match_header1 && !match_header2 && !match_header3) { + if (!match_header1 && !match_header2 && !match_header3 && !match_header4) { goto reset; } - if (match_header1 && pktoffset >= VN_INS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT1_LENGTH-1, 0); + if (match_header1 && pktoffset >= VN_IMU_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_IMU_LENGTH - 1, 0); + if (crc == 0) { + process_imu_packet(&pktbuf[sizeof(VN_IMU_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_IMU_LENGTH], pktoffset - VN_IMU_LENGTH); + pktoffset -= VN_IMU_LENGTH; + } else { + goto reset; + } + } else if (match_header2 && pktoffset >= VN_AHRS_EKF_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_EKF_LENGTH - 1, 0); if (crc == 0) { - // got pkt1 - process_ins_packet1(&pktbuf[sizeof(vn_ins_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT1_LENGTH], pktoffset-VN_INS_PKT1_LENGTH); - pktoffset -= VN_INS_PKT1_LENGTH; + process_ahrs_ekf_packet(&pktbuf[sizeof(VN_AHRS_ekf_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_AHRS_EKF_LENGTH], pktoffset - VN_AHRS_EKF_LENGTH); + pktoffset -= VN_AHRS_EKF_LENGTH; } else { goto reset; } - } else if (match_header2 && pktoffset >= VN_INS_PKT2_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_PKT2_LENGTH-1, 0); + } else if (match_header3 && pktoffset >= VN_INS_EKF_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_EKF_LENGTH - 1, 0); if (crc == 0) { - // got pkt2 - process_ins_packet2(&pktbuf[sizeof(vn_ins_pkt2_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_INS_PKT2_LENGTH], pktoffset-VN_INS_PKT2_LENGTH); - pktoffset -= VN_INS_PKT2_LENGTH; + process_ins_ekf_packet(&pktbuf[sizeof(VN_INS_ekf_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_INS_EKF_LENGTH], pktoffset - VN_INS_EKF_LENGTH); + pktoffset -= VN_INS_EKF_LENGTH; } else { goto reset; } - } else if (match_header3 && pktoffset >= VN_AHRS_PKT1_LENGTH) { - uint16_t crc = crc16_ccitt(&pktbuf[1], VN_AHRS_PKT1_LENGTH-1, 0); + } else if (match_header4 && pktoffset >= VN_INS_GNSS_LENGTH) { + uint16_t crc = crc16_ccitt(&pktbuf[1], VN_INS_GNSS_LENGTH - 1, 0); if (crc == 0) { - // got AHRS pkt - process_ahrs_packet(&pktbuf[sizeof(vn_ahrs_pkt1_header)+1]); - memmove(&pktbuf[0], &pktbuf[VN_AHRS_PKT1_LENGTH], pktoffset-VN_AHRS_PKT1_LENGTH); - pktoffset -= VN_AHRS_PKT1_LENGTH; + process_ins_gnss_packet(&pktbuf[sizeof(VN_INS_gnss_packet::header) + 1]); + memmove(&pktbuf[0], &pktbuf[VN_INS_GNSS_LENGTH], pktoffset - VN_INS_GNSS_LENGTH); + pktoffset -= VN_INS_GNSS_LENGTH; } else { goto reset; } @@ -431,8 +444,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { // VN-1X0 type = TYPE::VN_AHRS; - // This assumes unit is still configured at its default rate of 800hz - run_command("VNWRG,75,3,%u,14,073E,0004", unsigned(800 / get_rate())); + // These assumes unit is still configured at its default rate of 800hz + run_command("VNWRG,75,3,%u,01,0721", unsigned(800 / get_rate())); + run_command("VNWRG,76,3,16,11,0001,0106"); } else { // Default to setup for sensors other than VN-100 or VN-110 // This assumes unit is still configured at its default IMU rate of 400hz for VN-300, 800hz for others @@ -443,8 +457,9 @@ void AP_ExternalAHRS_VectorNav::initialize() { if (strncmp(model_name, "VN-3", 4) == 0) { has_dual_gnss = true; } - run_command("VNWRG,75,3,%u,34,072E,0106,0612", unsigned(imu_rate / get_rate())); - run_command("VNWRG,76,3,%u,4E,0002,0010,20B8,0018", unsigned(imu_rate / 5)); + run_command("VNWRG,75,3,%u,01,0721", unsigned(imu_rate / get_rate())); + run_command("VNWRG,76,3,%u,31,0001,0106,0613", unsigned(imu_rate / 50)); + run_command("VNWRG,77,3,%u,49,0003,26B8,0018", unsigned(imu_rate / 5)); } // Resume asynchronous communications @@ -469,129 +484,10 @@ const char* AP_ExternalAHRS_VectorNav::get_name() const return nullptr; } -/* - process INS mode INS packet - */ -void AP_ExternalAHRS_VectorNav::process_ins_packet1(const uint8_t *b) -{ - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)b; - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; - - last_pkt1_ms = AP_HAL::millis(); - *last_ins_pkt1 = pkt1; - - const bool use_uncomp = option_is_set(AP_ExternalAHRS::OPTIONS::VN_UNCOMP_IMU); - - { - WITH_SEMAPHORE(state.sem); - if (use_uncomp) { - state.accel = Vector3f{pkt1.uncompAccel[0], pkt1.uncompAccel[1], pkt1.uncompAccel[2]}; - state.gyro = Vector3f{pkt1.uncompAngRate[0], pkt1.uncompAngRate[1], pkt1.uncompAngRate[2]}; - } else { - state.accel = Vector3f{pkt1.accel[0], pkt1.accel[1], pkt1.accel[2]}; - state.gyro = Vector3f{pkt1.gyro[0], pkt1.gyro[1], pkt1.gyro[2]}; - } - - state.quat = Quaternion{pkt1.quaternion[3], pkt1.quaternion[0], pkt1.quaternion[1], pkt1.quaternion[2]}; - state.have_quaternion = true; - - state.velocity = Vector3f{pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2]}; - state.have_velocity = true; - - state.location = Location{int32_t(pkt1.positionLLA[0] * 1.0e7), - int32_t(pkt1.positionLLA[1] * 1.0e7), - int32_t(pkt1.positionLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; - state.last_location_update_us = AP_HAL::micros(); - state.have_location = true; - } - -#if AP_BARO_EXTERNALAHRS_ENABLED - { - AP_ExternalAHRS::baro_data_message_t baro; - baro.instance = 0; - baro.pressure_pa = pkt1.pressure*1e3; - baro.temperature = pkt2.temp; - - AP::baro().handle_external(baro); - } -#endif - -#if AP_COMPASS_EXTERNALAHRS_ENABLED - { - AP_ExternalAHRS::mag_data_message_t mag; - mag.field = Vector3f{pkt1.mag[0], pkt1.mag[1], pkt1.mag[2]}; - mag.field *= 1000; // to mGauss - - AP::compass().handle_external(mag); - } -#endif - - { - AP_ExternalAHRS::ins_data_message_t ins; - - ins.accel = state.accel; - ins.gyro = state.gyro; - ins.temperature = pkt2.temp; - - AP::ins().handle_external(ins); - } -} - -/* - process INS mode GNSS packet - */ -void AP_ExternalAHRS_VectorNav::process_ins_packet2(const uint8_t *b) -{ - const struct VN_INS_packet2 &pkt2 = *(struct VN_INS_packet2 *)b; - const struct VN_INS_packet1 &pkt1 = *last_ins_pkt1; - - last_pkt2_ms = AP_HAL::millis(); - *last_ins_pkt2 = pkt2; - - AP_ExternalAHRS::gps_data_message_t gps; - - // get ToW in milliseconds - gps.gps_week = pkt2.timeGPS / (AP_MSEC_PER_WEEK * 1000000ULL); - gps.ms_tow = (pkt2.timeGPS / 1000000ULL) % (60*60*24*7*1000ULL); - gps.fix_type = pkt2.GPS1Fix; - gps.satellites_in_view = pkt2.numGPS1Sats; - - gps.horizontal_pos_accuracy = pkt1.posU; - gps.vertical_pos_accuracy = pkt1.posU; - gps.horizontal_vel_accuracy = pkt1.velU; - - gps.hdop = pkt2.GPS1DOP[4]; - gps.vdop = pkt2.GPS1DOP[3]; - - gps.latitude = pkt2.GPS1posLLA[0] * 1.0e7; - gps.longitude = pkt2.GPS1posLLA[1] * 1.0e7; - gps.msl_altitude = pkt2.GPS1posLLA[2] * 1.0e2; - - gps.ned_vel_north = pkt2.GPS1velNED[0]; - gps.ned_vel_east = pkt2.GPS1velNED[1]; - gps.ned_vel_down = pkt2.GPS1velNED[2]; - - if (gps.fix_type >= 3 && !state.have_origin) { - WITH_SEMAPHORE(state.sem); - state.origin = Location{int32_t(pkt2.GPS1posLLA[0] * 1.0e7), - int32_t(pkt2.GPS1posLLA[1] * 1.0e7), - int32_t(pkt2.GPS1posLLA[2] * 1.0e2), - Location::AltFrame::ABSOLUTE}; - state.have_origin = true; - } - uint8_t instance; - if (AP::gps().get_first_external_instance(instance)) { - AP::gps().handle_external(gps, instance); - } -} - -/* - process AHRS mode AHRS packet - */ -void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) +// process INS mode INS packet +void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b) { - const struct VN_AHRS_packet1 &pkt = *(struct VN_AHRS_packet1 *)b; + const struct VN_IMU_packet &pkt = *(struct VN_IMU_packet *)b; last_pkt1_ms = AP_HAL::millis(); @@ -606,16 +502,13 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) state.accel = Vector3f{pkt.accel[0], pkt.accel[1], pkt.accel[2]}; state.gyro = Vector3f{pkt.gyro[0], pkt.gyro[1], pkt.gyro[2]}; } - - state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; - state.have_quaternion = true; } #if AP_BARO_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::baro_data_message_t baro; baro.instance = 0; - baro.pressure_pa = pkt.pressure*1e3; + baro.pressure_pa = pkt.pressure * 1e3; baro.temperature = pkt.temp; AP::baro().handle_external(baro); @@ -625,11 +518,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) #if AP_COMPASS_EXTERNALAHRS_ENABLED { AP_ExternalAHRS::mag_data_message_t mag; - if (use_uncomp) { - mag.field = Vector3f{pkt.uncompMag[0], pkt.uncompMag[1], pkt.uncompMag[2]}; - } else { - mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; - } + mag.field = Vector3f{pkt.mag[0], pkt.mag[1], pkt.mag[2]}; mag.field *= 1000; // to mGauss AP::compass().handle_external(mag); @@ -647,8 +536,8 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) } #if HAL_LOGGING_ENABLED - // @LoggerMessage: EAH3 - // @Description: External AHRS data + // @LoggerMessage: EAHI + // @Description: External AHRS IMU data // @Field: TimeUS: Time since system startup // @Field: Temp: Temprature // @Field: Pres: Pressure @@ -661,25 +550,160 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_packet(const uint8_t *b) // @Field: GX: Rotation rate X-axis // @Field: GY: Rotation rate Y-axis // @Field: GZ: Rotation rate Z-axis - // @Field: Q1: Attitude quaternion 1 - // @Field: Q2: Attitude quaternion 2 - // @Field: Q3: Attitude quaternion 3 - // @Field: Q4: Attitude quaternion 4 - AP::logger().WriteStreaming("EAH3", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ,Q1,Q2,Q3,Q4", - "sdPGGGoooEEE----", "F000000000000000", - "Qfffffffffffffff", + AP::logger().WriteStreaming("EAHIM", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ", + "sdPGGGoooEEE", "F00000000000", + "Qfffffffffff", AP_HAL::micros64(), pkt.temp, pkt.pressure*1e3, - use_uncomp ? pkt.uncompMag[0] : pkt.mag[0], - use_uncomp ? pkt.uncompMag[1] : pkt.mag[1], - use_uncomp ? pkt.uncompMag[2] : pkt.mag[2], + pkt.mag[0], pkt.mag[1], pkt.mag[2], state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1], state.gyro[2], state.quat[0], state.quat[1], state.quat[2], state.quat[3]); #endif // HAL_LOGGING_ENABLED } +// process AHRS mode AHRS packet +void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) { + const struct VN_AHRS_ekf_packet &pkt = *(struct VN_AHRS_ekf_packet *)b; + + last_pkt2_ms = AP_HAL::millis(); + + state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; + state.have_quaternion = true; + +#if HAL_LOGGING_ENABLED + // @LoggerMessage: EAHA + // @Description: External AHRS Attitude data + // @Field: TimeUS: Time since system startup + // @Field: Q1: Attitude quaternion 1 + // @Field: Q2: Attitude quaternion 2 + // @Field: Q3: Attitude quaternion 3 + // @Field: Q4: Attitude quaternion 4 + // @Field: Yaw: Yaw + // @Field: Pitch: Pitch + // @Field: Roll: Roll + // @Field: YU: Yaw unceratainty + // @Field: PU: Pitch uncertainty + // @Field: RU: Roll uncertainty + + AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + "s----dddddd", "F0000000000", + "Qffffffffff", + AP_HAL::micros64(), + state.quat[0], state.quat[1], state.quat[2], state.quat[3], + pkt.ypr[0], pkt.ypr[1], pkt.ypr[2], + pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]); +#endif // HAL_LOGGING_ENABLED +} + +// process INS mode EKF packet +void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { + const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b; + + last_pkt2_ms = AP_HAL::millis(); + latest_ins_ekf_packet = &pkt; + + state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; + state.have_quaternion = true; + + state.velocity = Vector3f{pkt.velNed[0], pkt.velNed[1], pkt.velNed[2]}; + state.have_velocity = true; + + state.location = Location{int32_t(pkt.posLla[0] * 1.0e7), int32_t(pkt.posLla[1] * 1.0e7), int32_t(pkt.posLla[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.last_location_update_us = AP_HAL::micros(); + state.have_location = true; + +#if HAL_LOGGING_ENABLED + auto now = AP_HAL::micros64(); + + // @LoggerMessage: EAHA + // @Description: External AHRS Attitude data + // @Field: TimeUS: Time since system startup + // @Field: Q1: Attitude quaternion 1 + // @Field: Q2: Attitude quaternion 2 + // @Field: Q3: Attitude quaternion 3 + // @Field: Q4: Attitude quaternion 4 + // @Field: Yaw: Yaw + // @Field: Pitch: Pitch + // @Field: Roll: Roll + // @Field: YU: Yaw unceratainty + // @Field: PU: Pitch uncertainty + // @Field: RU: Roll uncertainty + + AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + "s----dddddd", "F0000000000", + "Qffffffffff", + now, + state.quat[0], state.quat[1], state.quat[2], state.quat[3], + pkt.ypr[0], pkt.ypr[1], pkt.ypr[2], + pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]); + + // @LoggerMessage: EAHK + // @Description: External AHRS INS Kalman Filter data + // @Field: TimeUS: Time since system startup + // @Field: InsStatus: VectorNav INS health status + // @Field: Lat: Latitude + // @Field: Lon: Longitude + // @Field: Alt: Altitude + // @Field: VelN: Velocity Northing + // @Field: VelE: Velocity Easting + // @Field: VelD: Velocity Downing + // @Field: PosU: Filter estimated position uncertainty + // @Field: VelU: Filter estimated Velocity uncertainty + + AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", + "s-ddmnnndn", "F00000000", + "QHdddfffff", + now, + pkt.insStatus, + pkt.posLla[0], pkt.posLla[1], pkt.posLla[2], + pkt.velNed[0], pkt.velNed[1], pkt.velNed[2], + pkt.posU, pkt.velU); +#endif // HAL_LOGGING_ENABLED +} + +// process INS mode GNSS packet +void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { + const struct VN_INS_gnss_packet &pkt = *(struct VN_INS_gnss_packet *)b; + AP_ExternalAHRS::gps_data_message_t gps; + + + last_pkt3_ms = AP_HAL::millis(); + latest_ins_gnss_packet = &pkt; + + // get ToW in milliseconds + gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); + gps.ms_tow = (pkt.timeGps / 1000000ULL) % (60 * 60 * 24 * 7 * 1000ULL); + gps.fix_type = pkt.fix1; + gps.satellites_in_view = pkt.numSats1; + + gps.horizontal_pos_accuracy = pkt.posU1[0]; + gps.vertical_pos_accuracy = pkt.posU1[2]; + gps.horizontal_vel_accuracy = pkt.velU1; + + gps.hdop = pkt.dop1[4]; + gps.vdop = pkt.dop1[3]; + + gps.latitude = pkt.posLla1[0] * 1.0e7; + gps.longitude = pkt.posLla1[1] * 1.0e7; + gps.msl_altitude = pkt.posLla1[2] * 1.0e2; + + gps.ned_vel_north = pkt.velNed1[0]; + gps.ned_vel_east = pkt.velNed1[1]; + gps.ned_vel_down = pkt.velNed1[2]; + + if (!state.have_origin && gps.fix_type >= 3) { + WITH_SEMAPHORE(state.sem); + state.origin = Location{int32_t(pkt.posLla1[0] * 1.0e7), int32_t(pkt.posLla1[1] * 1.0e7), + int32_t(pkt.posLla1[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps, instance); + } +} // get serial port number for the uart int8_t AP_ExternalAHRS_VectorNav::get_port(void) const @@ -694,10 +718,7 @@ int8_t AP_ExternalAHRS_VectorNav::get_port(void) const bool AP_ExternalAHRS_VectorNav::healthy(void) const { const uint32_t now = AP_HAL::millis(); - if (type == TYPE::VN_AHRS) { - return (now - last_pkt1_ms < 40); - } - return (now - last_pkt1_ms < 40 && now - last_pkt2_ms < 500); + return (now - last_pkt1_ms < 40) && (now - last_pkt2_ms < 500) && (type == TYPE::VN_AHRS ? true: now - last_pkt3_ms < 1000); } bool AP_ExternalAHRS_VectorNav::initialised(void) const @@ -705,10 +726,7 @@ bool AP_ExternalAHRS_VectorNav::initialised(void) const if (!setup_complete) { return false; } - if (type == TYPE::VN_AHRS) { - return last_pkt1_ms != 0; - } - return last_pkt1_ms != 0 && last_pkt2_ms != 0; + return last_pkt1_ms != UINT32_MAX && last_pkt2_ms != UINT32_MAX && (type == TYPE::VN_AHRS ? true : last_pkt3_ms != UINT32_MAX); } bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const @@ -722,11 +740,11 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure return false; } if (type == TYPE::VN_INS) { - if (last_ins_pkt2->GPS1Fix < 3) { + if (latest_ins_gnss_packet->fix1 < 3) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS1 lock"); return false; } - if (has_dual_gnss && (last_ins_pkt2->GPS2Fix < 3)) { + if (has_dual_gnss && (latest_ins_gnss_packet->fix2 < 3)) { hal.util->snprintf(failure_msg, failure_msg_len, "VectorNav no GPS2 lock"); return false; } @@ -741,37 +759,31 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const { memset(&status, 0, sizeof(status)); - if (type == TYPE::VN_INS) { - if (last_ins_pkt1 && last_ins_pkt2) { - status.flags.initalized = true; - } - if (healthy() && last_ins_pkt2) { + status.flags.initalized = initialised(); + if (healthy()) { + if (type == TYPE::VN_AHRS) { status.flags.attitude = true; - status.flags.vert_vel = true; - status.flags.vert_pos = true; - - const struct VN_INS_packet2 &pkt2 = *last_ins_pkt2; - if (pkt2.GPS1Fix >= 3) { - status.flags.horiz_vel = true; - status.flags.horiz_pos_rel = true; - status.flags.horiz_pos_abs = true; + } else { + status.flags.attitude = true; + if (latest_ins_ekf_packet) { + status.flags.vert_vel = true; + status.flags.vert_pos = true; + + status.flags.horiz_vel = true; + status.flags.horiz_pos_rel = true; + status.flags.horiz_pos_abs = true; status.flags.pred_horiz_pos_rel = true; status.flags.pred_horiz_pos_abs = true; - status.flags.using_gps = true; + status.flags.using_gps = true; } } - } else { - status.flags.initalized = initialised(); - if (healthy()) { - status.flags.attitude = true; - } } } // send an EKF_STATUS message to GCS void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const { - if (!last_ins_pkt1) { + if (!latest_ins_ekf_packet) { return; } // prepare flags @@ -813,13 +825,13 @@ void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const } // send message - const struct VN_INS_packet1 &pkt1 = *(struct VN_INS_packet1 *)last_ins_pkt1; + const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)latest_ins_ekf_packet; const float vel_gate = 5; const float pos_gate = 5; const float hgt_gate = 5; const float mag_var = 0; mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, + pkt.velU / vel_gate, pkt.posU / pos_gate, pkt.posU / hgt_gate, mag_var, 0, 0); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index db73138befe348..a70bec210dc29e 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -63,20 +63,23 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { void initialize(); - void process_ins_packet1(const uint8_t *b); - void process_ins_packet2(const uint8_t *b); - void process_ahrs_packet(const uint8_t *b); void run_command(const char *fmt, ...); + void process_imu_packet(const uint8_t *b); + void process_ahrs_ekf_packet(const uint8_t *b); + void process_ins_ekf_packet(const uint8_t *b); + void process_ins_gnss_packet(const uint8_t *b); uint8_t *pktbuf; uint16_t pktoffset; uint16_t bufsize; - struct VN_INS_packet1 *last_ins_pkt1; - struct VN_INS_packet2 *last_ins_pkt2; + struct VN_imu_packet const *latest_imu_packet = nullptr; + struct VN_INS_ekf_packet const *latest_ins_ekf_packet = nullptr; + struct VN_INS_gnss_packet const *latest_ins_gnss_packet = nullptr; - uint32_t last_pkt1_ms; - uint32_t last_pkt2_ms; + uint32_t last_pkt1_ms = UINT32_MAX; + uint32_t last_pkt2_ms = UINT32_MAX; + uint32_t last_pkt3_ms = UINT32_MAX; enum class TYPE { VN_INS, // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0 From 50980ee03ef6efe361dd4acbec24ae744cf09b9d Mon Sep 17 00:00:00 2001 From: BLash Date: Thu, 18 Jul 2024 19:27:08 -0500 Subject: [PATCH 67/77] AP_ExternalAHRS_VectorNav: Update SIM to match new message definitions Redefine messages to 3 INS packets, and send as appropriate Resolve SITL failures due to too-long logger message headers --- .../AP_ExternalAHRS_VectorNav.cpp | 14 +- .../AP_ExternalAHRS_VectorNav.h | 6 +- libraries/SITL/SIM_VectorNav.cpp | 207 +++++++++++------- libraries/SITL/SIM_VectorNav.h | 11 +- 4 files changed, 149 insertions(+), 89 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index aeefe9ff54d45b..3e58710799d031 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -551,7 +551,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b) // @Field: GY: Rotation rate Y-axis // @Field: GZ: Rotation rate Z-axis - AP::logger().WriteStreaming("EAHIM", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ", + AP::logger().WriteStreaming("EAHI", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ", "sdPGGGoooEEE", "F00000000000", "Qfffffffffff", AP_HAL::micros64(), @@ -587,7 +587,7 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) { // @Field: PU: Pitch uncertainty // @Field: RU: Roll uncertainty - AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", "s----dddddd", "F0000000000", "Qffffffffff", AP_HAL::micros64(), @@ -602,7 +602,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { const struct VN_INS_ekf_packet &pkt = *(struct VN_INS_ekf_packet *)b; last_pkt2_ms = AP_HAL::millis(); - latest_ins_ekf_packet = &pkt; + *latest_ins_ekf_packet = pkt; state.quat = Quaternion{pkt.quaternion[3], pkt.quaternion[0], pkt.quaternion[1], pkt.quaternion[2]}; state.have_quaternion = true; @@ -631,7 +631,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { // @Field: PU: Pitch uncertainty // @Field: RU: Roll uncertainty - AP::logger().WriteStreaming("EAHAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", "s----dddddd", "F0000000000", "Qffffffffff", now, @@ -652,8 +652,8 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { // @Field: PosU: Filter estimated position uncertainty // @Field: VelU: Filter estimated Velocity uncertainty - AP::logger().WriteStreaming("EAHKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", - "s-ddmnnndn", "F00000000", + AP::logger().WriteStreaming("EAHK", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU", + "s-ddmnnndn", "F000000000", "QHdddfffff", now, pkt.insStatus, @@ -670,7 +670,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_gnss_packet(const uint8_t *b) { last_pkt3_ms = AP_HAL::millis(); - latest_ins_gnss_packet = &pkt; + *latest_ins_gnss_packet = pkt; // get ToW in milliseconds gps.gps_week = pkt.timeGps / (AP_MSEC_PER_WEEK * 1000000ULL); diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index a70bec210dc29e..456090d205f00a 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -73,9 +73,9 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { uint16_t pktoffset; uint16_t bufsize; - struct VN_imu_packet const *latest_imu_packet = nullptr; - struct VN_INS_ekf_packet const *latest_ins_ekf_packet = nullptr; - struct VN_INS_gnss_packet const *latest_ins_gnss_packet = nullptr; + struct VN_imu_packet *latest_imu_packet = nullptr; + struct VN_INS_ekf_packet *latest_ins_ekf_packet = nullptr; + struct VN_INS_gnss_packet *latest_ins_gnss_packet = nullptr; uint32_t last_pkt1_ms = UINT32_MAX; uint32_t last_pkt2_ms = UINT32_MAX; diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index b3f9ecf6a4d89f..acaa07edaa4371 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -29,37 +29,49 @@ VectorNav::VectorNav() : { } -struct PACKED VN_INS_packet1 { - float uncompMag[3]; + +struct PACKED VN_IMU_packet_sim { + static constexpr uint8_t header[]{0x01, 0x21, 0x07}; + uint64_t timeStartup; + float gyro[3]; + float accel[3]; float uncompAccel[3]; float uncompAngRate[3]; - float pressure; float mag[3]; - float accel[3]; - float gyro[3]; + float temp; + float pressure; +}; +constexpr uint8_t VN_IMU_packet_sim::header[]; + +struct PACKED VN_INS_ekf_packet_sim { + static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06}; + uint64_t timeStartup; float ypr[3]; float quaternion[4]; float yprU[3]; - double positionLLA[3]; - float velNED[3]; + uint16_t insStatus; + double posLla[3]; + float velNed[3]; float posU; float velU; }; - -struct PACKED VN_INS_packet2 { - uint64_t timeGPS; - float temp; - uint8_t numGPS1Sats; - uint8_t GPS1Fix; - double GPS1posLLA[3]; - float GPS1velNED[3]; - float GPS1DOP[7]; - uint8_t numGPS2Sats; - uint8_t GPS2Fix; +constexpr uint8_t VN_INS_ekf_packet_sim::header[]; + +struct PACKED VN_INS_gnss_packet_sim { + static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00}; + uint64_t timeStartup; + uint64_t timeGps; + uint8_t numSats1; + uint8_t fix1; + double posLla1[3]; + float velNed1[3]; + float posU1[3]; + float velU1; + float dop1[7]; + uint8_t numSats2; + uint8_t fix2; }; - -#define VN_PKT1_HEADER { 0xFA, 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 } -#define VN_PKT2_HEADER { 0xFA, 0x4E, 0x02, 0x00, 0x10, 0x00, 0xB8, 0x20, 0x18, 0x00 } +constexpr uint8_t VN_INS_gnss_packet_sim::header[]; /* get timeval using simulation time @@ -80,36 +92,62 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -void VectorNav::send_packet1(void) +void VectorNav::send_imu_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet1 pkt {}; + struct VN_IMU_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; + + + const float gyro_noise = 0.05; + + pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); + pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); + pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + + pkt.accel[0] = fdm.xAccel; + pkt.accel[1] = fdm.yAccel; + pkt.accel[2] = fdm.zAccel; pkt.uncompAccel[0] = fdm.xAccel; pkt.uncompAccel[1] = fdm.yAccel; pkt.uncompAccel[2] = fdm.zAccel; - const float gyro_noise = 0.05; + pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float()); pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float()); pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float()); - const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); - pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; - pkt.mag[0] = fdm.bodyMagField.x*0.001; pkt.mag[1] = fdm.bodyMagField.y*0.001; pkt.mag[2] = fdm.bodyMagField.z*0.001; - pkt.uncompMag[0] = pkt.mag[0]; - pkt.uncompMag[1] = pkt.mag[1]; - pkt.uncompMag[2] = pkt.mag[2]; - pkt.accel[0] = fdm.xAccel; - pkt.accel[1] = fdm.yAccel; - pkt.accel[2] = fdm.zAccel; - pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise); - pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise); - pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise); + pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); + + const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)&sync_byte, 1); + write_to_autopilot((char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header)); + write_to_autopilot((char *)&pkt, sizeof(pkt)); + + uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0); + crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; + swab(&crc, &crc2, 2); + + write_to_autopilot((char *)&crc2, sizeof(crc2)); +} + +void VectorNav::send_ins_ekf_packet(void) +{ + const auto &fdm = _sitl->state; + + struct VN_INS_ekf_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; pkt.ypr[0] = fdm.yawDeg; pkt.ypr[1] = fdm.pitchDeg; @@ -120,58 +158,74 @@ void VectorNav::send_packet1(void) pkt.quaternion[2] = fdm.quaternion.q4; pkt.quaternion[3] = fdm.quaternion.q1; - pkt.positionLLA[0] = fdm.latitude; - pkt.positionLLA[1] = fdm.longitude; - pkt.positionLLA[2] = fdm.altitude; - pkt.velNED[0] = fdm.speedN; - pkt.velNED[1] = fdm.speedE; - pkt.velNED[2] = fdm.speedD; + pkt.yprU[0] = 0.03; + pkt.yprU[1] = 0.03; + pkt.yprU[2] = 0.15; + + pkt.insStatus = 0x0306; + + pkt.posLla[0] = fdm.latitude; + pkt.posLla[1] = fdm.longitude; + pkt.posLla[2] = fdm.altitude; + pkt.velNed[0] = fdm.speedN; + pkt.velNed[1] = fdm.speedE; + pkt.velNed[2] = fdm.speedD; pkt.posU = 0.5; pkt.velU = 0.25; - const uint8_t header[] VN_PKT1_HEADER; - - write_to_autopilot((char *)&header, sizeof(header)); + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)&sync_byte, 1); + write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header)); write_to_autopilot((char *)&pkt, sizeof(pkt)); - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); + uint16_t crc2; swab(&crc, &crc2, 2); write_to_autopilot((char *)&crc2, sizeof(crc2)); } -void VectorNav::send_packet2(void) +void VectorNav::send_ins_gnss_packet(void) { const auto &fdm = _sitl->state; - struct VN_INS_packet2 pkt {}; + struct VN_INS_gnss_packet_sim pkt {}; + + pkt.timeStartup = AP_HAL::micros() * 1e3; struct timeval tv; simulation_timeval(&tv); - pkt.timeGPS = tv.tv_usec * 1000ULL; - - pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude); - pkt.numGPS1Sats = 19; - pkt.GPS1Fix = 3; - pkt.GPS1posLLA[0] = fdm.latitude; - pkt.GPS1posLLA[1] = fdm.longitude; - pkt.GPS1posLLA[2] = fdm.altitude; - pkt.GPS1velNED[0] = fdm.speedN; - pkt.GPS1velNED[1] = fdm.speedE; - pkt.GPS1velNED[2] = fdm.speedD; - // pkt.GPS1DOP = - pkt.numGPS2Sats = 18; - pkt.GPS2Fix = 3; - - const uint8_t header[] VN_PKT2_HEADER; - - write_to_autopilot((char *)&header, sizeof(header)); + pkt.timeGps = tv.tv_usec * 1000ULL; + + pkt.numSats1 = 19; + pkt.fix1 = 3; + pkt.posLla1[0] = fdm.latitude; + pkt.posLla1[1] = fdm.longitude; + pkt.posLla1[2] = fdm.altitude; + pkt.velNed1[0] = fdm.speedN; + pkt.velNed1[1] = fdm.speedE; + pkt.velNed1[2] = fdm.speedD; + + pkt.posU1[0] = 1; + pkt.posU1[0] = 1; + pkt.posU1[0] = 1.5; + + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + pkt.velNed1[0] = 0.05; + // pkt.dop1 = + pkt.numSats2 = 18; + pkt.fix2 = 3; + + const uint8_t sync_byte = 0xFA; + write_to_autopilot((char *)&sync_byte, 1); + write_to_autopilot((char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header)); write_to_autopilot((char *)&pkt, sizeof(pkt)); - uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0); + uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); uint16_t crc2; @@ -203,14 +257,19 @@ void VectorNav::update(void) } uint32_t now = AP_HAL::micros(); - if (now - last_pkt1_us >= 20000) { - last_pkt1_us = now; - send_packet1(); + if (now - last_imu_pkt_us >= 20000) { + last_imu_pkt_us = now; + send_imu_packet(); } - - if (now - last_pkt2_us >= 200000) { - last_pkt2_us = now; - send_packet2(); + + if (now - last_ekf_pkt_us >= 20000) { + last_ekf_pkt_us = now; + send_ins_ekf_packet(); + } + + if (now - last_gnss_pkt_us >= 200000) { + last_gnss_pkt_us = now; + send_ins_gnss_packet(); } char receive_buf[50]; diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index 589539e5d9a8f3..77fde25625733a 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -41,12 +41,13 @@ class VectorNav : public SerialDevice { void update(void); private: - uint32_t last_pkt1_us; - uint32_t last_pkt2_us; - uint32_t last_type_us; + uint32_t last_imu_pkt_us; + uint32_t last_ekf_pkt_us; + uint32_t last_gnss_pkt_us; - void send_packet1(); - void send_packet2(); + void send_imu_packet(); + void send_ins_ekf_packet(); + void send_ins_gnss_packet(); void nmea_printf(const char *fmt, ...); }; From 1d1bb5724eb96e91f335263c22bd4ef91c3b9818 Mon Sep 17 00:00:00 2001 From: BLash Date: Thu, 18 Jul 2024 21:02:40 -0500 Subject: [PATCH 68/77] AP_ExternalAHRS_VectorNav: Address review comments Remove unnecessary header Switch parameters to default initialization Change pointer casting to prevent a const_cast, and remove doubled sync byte when echoing ASCII messages Fix Valgrind report by preventing use of nmea_printf on buffers which may not be null-terminated --- .../AP_ExternalAHRS_VectorNav.cpp | 1 - .../AP_ExternalAHRS_VectorNav.h | 12 +++++----- libraries/SITL/SIM_VectorNav.cpp | 24 +++++++++---------- 3 files changed, 18 insertions(+), 19 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 3e58710799d031..38f44ed1501118 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -16,7 +16,6 @@ support for serial connected AHRS systems */ -#include #define ALLOW_DOUBLE_MATH_FUNCTIONS #include "AP_ExternalAHRS_config.h" diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 456090d205f00a..9c1639640a1e00 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -73,13 +73,13 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { uint16_t pktoffset; uint16_t bufsize; - struct VN_imu_packet *latest_imu_packet = nullptr; - struct VN_INS_ekf_packet *latest_ins_ekf_packet = nullptr; - struct VN_INS_gnss_packet *latest_ins_gnss_packet = nullptr; + struct VN_imu_packet *latest_imu_packet; + struct VN_INS_ekf_packet *latest_ins_ekf_packet; + struct VN_INS_gnss_packet *latest_ins_gnss_packet; - uint32_t last_pkt1_ms = UINT32_MAX; - uint32_t last_pkt2_ms = UINT32_MAX; - uint32_t last_pkt3_ms = UINT32_MAX; + uint32_t last_pkt1_ms; + uint32_t last_pkt2_ms; + uint32_t last_pkt3_ms; enum class TYPE { VN_INS, // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0 diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp index acaa07edaa4371..ad61f0101077e3 100644 --- a/libraries/SITL/SIM_VectorNav.cpp +++ b/libraries/SITL/SIM_VectorNav.cpp @@ -129,16 +129,16 @@ void VectorNav::send_imu_packet(void) pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01; const uint8_t sync_byte = 0xFA; - write_to_autopilot((char *)&sync_byte, 1); - write_to_autopilot((char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header)); - write_to_autopilot((char *)&pkt, sizeof(pkt)); + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); uint16_t crc2; swab(&crc, &crc2, 2); - write_to_autopilot((char *)&crc2, sizeof(crc2)); + write_to_autopilot((const char *)&crc2, sizeof(crc2)); } void VectorNav::send_ins_ekf_packet(void) @@ -174,9 +174,9 @@ void VectorNav::send_ins_ekf_packet(void) pkt.velU = 0.25; const uint8_t sync_byte = 0xFA; - write_to_autopilot((char *)&sync_byte, 1); - write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header)); - write_to_autopilot((char *)&pkt, sizeof(pkt)); + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); @@ -184,7 +184,7 @@ void VectorNav::send_ins_ekf_packet(void) uint16_t crc2; swab(&crc, &crc2, 2); - write_to_autopilot((char *)&crc2, sizeof(crc2)); + write_to_autopilot((const char *)&crc2, sizeof(crc2)); } void VectorNav::send_ins_gnss_packet(void) @@ -221,9 +221,9 @@ void VectorNav::send_ins_gnss_packet(void) pkt.fix2 = 3; const uint8_t sync_byte = 0xFA; - write_to_autopilot((char *)&sync_byte, 1); - write_to_autopilot((char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header)); - write_to_autopilot((char *)&pkt, sizeof(pkt)); + write_to_autopilot((const char *)&sync_byte, 1); + write_to_autopilot((const char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header)); + write_to_autopilot((const char *)&pkt, sizeof(pkt)); uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0); crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc); @@ -231,7 +231,7 @@ void VectorNav::send_ins_gnss_packet(void) uint16_t crc2; swab(&crc, &crc2, 2); - write_to_autopilot((char *)&crc2, sizeof(crc2)); + write_to_autopilot((const char *)&crc2, sizeof(crc2)); } void VectorNav::nmea_printf(const char *fmt, ...) From d7ecf5fbc5a69d8253bc2e9faf5a45ecd08b677d Mon Sep 17 00:00:00 2001 From: BLash Date: Fri, 19 Jul 2024 19:30:07 -0500 Subject: [PATCH 69/77] AP_ExternalAHRS_VectorNav: Consolidate EAH3 definition to single method Resolve SITL failures due to multiply defined logger message by pulling the call to a single method --- .../AP_ExternalAHRS_VectorNav.cpp | 95 ++++++++++--------- .../AP_ExternalAHRS_VectorNav.h | 4 + 2 files changed, 56 insertions(+), 43 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 38f44ed1501118..aa358f8fbde636 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -483,6 +483,44 @@ const char* AP_ExternalAHRS_VectorNav::get_name() const return nullptr; } +// Input data struct for EAHA logging message, used by both AHRS mode and INS mode +struct AP_ExternalAHRS_VectorNav::EAHA { + uint64_t timeUs; + float quat[4]; + float ypr[3]; + float yprU[3]; +}; + + +void AP_ExternalAHRS_VectorNav::write_eaha(const EAHA& data_to_log) const { + +#if HAL_LOGGING_ENABLED + // @LoggerMessage: EAHA + // @Description: External AHRS Attitude data + // @Field: TimeUS: Time since system startup + // @Field: Q1: Attitude quaternion 1 + // @Field: Q2: Attitude quaternion 2 + // @Field: Q3: Attitude quaternion 3 + // @Field: Q4: Attitude quaternion 4 + // @Field: Yaw: Yaw + // @Field: Pitch: Pitch + // @Field: Roll: Roll + // @Field: YU: Yaw unceratainty + // @Field: PU: Pitch uncertainty + // @Field: RU: Roll uncertainty + + AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", + "s----dddddd", "F0000000000", + "Qffffffffff", + data_to_log.timeUs, + data_to_log.quat[0], data_to_log.quat[1], data_to_log.quat[2], data_to_log.quat[3], + data_to_log.ypr[0], data_to_log.ypr[1], data_to_log.ypr[2], + data_to_log.yprU[0], data_to_log.yprU[1], data_to_log.yprU[2]); +#endif +} + + + // process INS mode INS packet void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b) { @@ -572,27 +610,13 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) { state.have_quaternion = true; #if HAL_LOGGING_ENABLED - // @LoggerMessage: EAHA - // @Description: External AHRS Attitude data - // @Field: TimeUS: Time since system startup - // @Field: Q1: Attitude quaternion 1 - // @Field: Q2: Attitude quaternion 2 - // @Field: Q3: Attitude quaternion 3 - // @Field: Q4: Attitude quaternion 4 - // @Field: Yaw: Yaw - // @Field: Pitch: Pitch - // @Field: Roll: Roll - // @Field: YU: Yaw unceratainty - // @Field: PU: Pitch uncertainty - // @Field: RU: Roll uncertainty + EAHA data_to_log; + data_to_log.timeUs = AP_HAL::micros64(); + memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion)); + memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr)); + memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU)); - AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", - "s----dddddd", "F0000000000", - "Qffffffffff", - AP_HAL::micros64(), - state.quat[0], state.quat[1], state.quat[2], state.quat[3], - pkt.ypr[0], pkt.ypr[1], pkt.ypr[2], - pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]); + write_eaha(data_to_log); #endif // HAL_LOGGING_ENABLED } @@ -614,29 +638,13 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { state.have_location = true; #if HAL_LOGGING_ENABLED - auto now = AP_HAL::micros64(); - - // @LoggerMessage: EAHA - // @Description: External AHRS Attitude data - // @Field: TimeUS: Time since system startup - // @Field: Q1: Attitude quaternion 1 - // @Field: Q2: Attitude quaternion 2 - // @Field: Q3: Attitude quaternion 3 - // @Field: Q4: Attitude quaternion 4 - // @Field: Yaw: Yaw - // @Field: Pitch: Pitch - // @Field: Roll: Roll - // @Field: YU: Yaw unceratainty - // @Field: PU: Pitch uncertainty - // @Field: RU: Roll uncertainty - - AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU", - "s----dddddd", "F0000000000", - "Qffffffffff", - now, - state.quat[0], state.quat[1], state.quat[2], state.quat[3], - pkt.ypr[0], pkt.ypr[1], pkt.ypr[2], - pkt.yprU[0], pkt.yprU[1], pkt.yprU[2]); + EAHA data_to_log; + auto now = AP_HAL::micros64(); + data_to_log.timeUs = now; + memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion)); + memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr)); + memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU)); + write_eaha(data_to_log); // @LoggerMessage: EAHK // @Description: External AHRS INS Kalman Filter data @@ -660,6 +668,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) { pkt.velNed[0], pkt.velNed[1], pkt.velNed[2], pkt.posU, pkt.velU); #endif // HAL_LOGGING_ENABLED + } // process INS mode GNSS packet diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 9c1639640a1e00..8e1a1c5ec1917c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -64,11 +64,15 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { void initialize(); void run_command(const char *fmt, ...); + + struct EAHA; + void write_eaha(const EAHA& data_to_log) const; void process_imu_packet(const uint8_t *b); void process_ahrs_ekf_packet(const uint8_t *b); void process_ins_ekf_packet(const uint8_t *b); void process_ins_gnss_packet(const uint8_t *b); + uint8_t *pktbuf; uint16_t pktoffset; uint16_t bufsize; From 9f99d3c934405c93a7c5c746eb2c5c5a1cc9e118 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 27 Jul 2024 09:32:54 +0900 Subject: [PATCH 70/77] Tracker: 4.5.5-beta2 release notes --- AntennaTracker/ReleaseNotes.txt | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index 558ecfa5b57951..89982976f1caa0 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,5 +1,14 @@ Antenna Tracker Release Notes: ------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 From b752f8d4cfcf913978b53da90d64fd01061ddafd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 27 Jul 2024 09:32:32 +0900 Subject: [PATCH 71/77] Rover: 4.5.5-beta2 release notes --- Rover/ReleaseNotes.txt | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index bfccbf5df6fba8..250c049f4caec3 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,5 +1,14 @@ Rover Release Notes: --------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 From 8f51eafc825b84dcb2508d0a28ed0c254e7725ff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 27 Jul 2024 09:28:55 +0900 Subject: [PATCH 72/77] Copter: 4.5.5-beta2 release notes --- ArduCopter/ReleaseNotes.txt | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 3e1504a0626b57..84d8c9f384f65e 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,14 @@ ArduPilot Copter Release Notes: -------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 From 2806f8b63b1343505614ed9e5f542f9fd426925f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 27 Jul 2024 09:32:04 +0900 Subject: [PATCH 73/77] Plane: 4.5.5-beta2 release notes --- ArduPlane/ReleaseNotes.txt | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 277136de6841ac..5d2677d74a85a5 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,5 +1,14 @@ ArduPilot Plane Release Notes: ------------------------------- +------------------------------------------------------------------ +Release 4.5.5-beta2 27 July 2024 + +Changes from 4.5.5-beta1 + +1) Board specific enhancements and bug fixes + +- CubeRed's second core disabled at boot to avoid spurious writes to RAM +- CubeRed bootloader's dual endpoint update method fixed +------------------------------------------------------------------ Release 4.5.5-beta1 1st July 2024 Changes from 4.5.4 From 6dae7603462e6ab9f1f3fed3409c0b19906f6969 Mon Sep 17 00:00:00 2001 From: arynrosh Date: Sun, 28 Jul 2024 13:13:22 +0530 Subject: [PATCH 74/77] Tools: added name to GIT_Success.txt --- Tools/GIT_Test/GIT_Success.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 3ef569921c66cc..91e375800da1b6 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -197,4 +197,5 @@ Kei Kabutomori Hiroaki Kawasaki Kazuhide Juta Ryoichi Shiohama -Masaki Shibuya \ No newline at end of file +Masaki Shibuya +Aryan Roshan trying his first commit \ No newline at end of file From d7bc9a420fec5f476196036ced4051a9e49dd186 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 09:46:20 +1000 Subject: [PATCH 75/77] autotest: tracker; tidy GPSForYaw test --- Tools/autotest/antennatracker.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index b186711637b6bd..3864f4c512e035 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -177,8 +177,6 @@ def disabled_tests(self): def GPSForYaw(self): '''Moving baseline GPS yaw''' - self.context_push() - self.load_default_params_file("tracker-gps-for-yaw.parm") self.reboot_sitl() @@ -195,10 +193,6 @@ def GPSForYaw(self): raise NotAchievedException("GPS_RAW not tracking simstate yaw") self.progress(f"yaw match ({gps_raw_hdg} vs {sim_hdg}") - self.context_pop() - - self.reboot_sitl() - def tests(self): '''return list of all tests''' ret = super(AutoTestTracker, self).tests() From 2245399ff9374682ae1fc6e093e8b5224041a9db Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 27 Jul 2024 09:48:59 +1000 Subject: [PATCH 76/77] autotest: tidy Rover autotests autotest: tidy WheelEncoders test autotest: rover: tidy PolyFenceObjectAvoidance code autotest: add load_fence_using_mavwp Also corrects the mission item types being returned by the mission-item-to-misison-item-int converter autotest: fix Rover PolyFenceAvoidanceBendyRulerEasier test these tests weren't being run becausethey were disabled based on support for loading fence items from QGC files. autotest: fix Rover PolyFenceAvoidanceBendyRuler test --- .../rover-path-planning-fence.txt | 0 .../rover-path-planning-mission.txt | 0 .../rover-path-bendyruler-fence.txt | 0 .../rover-path-bendyruler-mission-easier.txt | 0 .../rover-path-bendyruler-fence.txt | 11 + Tools/autotest/rover.py | 284 +++++++----------- Tools/autotest/vehicle_test_suite.py | 44 ++- 7 files changed, 148 insertions(+), 191 deletions(-) rename Tools/autotest/ArduRover_Tests/{PolyFenceObjectAvoidance => PolyFenceObjectAvoidanceAuto}/rover-path-planning-fence.txt (100%) rename Tools/autotest/ArduRover_Tests/{PolyFenceObjectAvoidance => PolyFenceObjectAvoidanceAuto}/rover-path-planning-mission.txt (100%) rename Tools/autotest/ArduRover_Tests/{PolyFenceObjectAvoidanceBendyRulerEasier => PolyFenceObjectAvoidanceBendyRulerEasierAuto}/rover-path-bendyruler-fence.txt (100%) rename Tools/autotest/ArduRover_Tests/{PolyFenceObjectAvoidanceBendyRulerEasier => PolyFenceObjectAvoidanceBendyRulerEasierAuto}/rover-path-bendyruler-mission-easier.txt (100%) create mode 100644 Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidance/rover-path-planning-mission.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceAuto/rover-path-planning-mission.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-fence.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-fence.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt similarity index 100% rename from Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasier/rover-path-bendyruler-mission-easier.txt rename to Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierAuto/rover-path-bendyruler-mission-easier.txt diff --git a/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt new file mode 100644 index 00000000000000..ace97c223c4a40 --- /dev/null +++ b/Tools/autotest/ArduRover_Tests/PolyFenceObjectAvoidanceBendyRulerEasierGuided/rover-path-bendyruler-fence.txt @@ -0,0 +1,11 @@ +QGC WPL 110 +0 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071766 -105.230202 0.000000 0 +1 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.230247 0.000000 0 +2 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071014 -105.228821 0.000000 0 +3 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071609 -105.228867 0.000000 0 +4 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.071602 -105.228172 0.000000 0 +5 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070858 -105.227982 0.000000 0 +6 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.070789 -105.226219 0.000000 0 +7 0 0 5001 8.000000 0.000000 0.000000 0.000000 40.072453 -105.226379 0.000000 0 +8 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071609 -105.228172 0.000000 0 +9 0 0 5004 20.000000 0.000000 0.000000 0.000000 40.071625 -105.227982 0.000000 0 diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 9f4c8e08fa90f5..46f1f68b59ba45 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -4794,20 +4794,20 @@ def MotorTest(self): self.wait_servo_channel_value(3, self.get_parameter("RC3_TRIM", 5), timeout=10) self.wait_disarmed() - def test_poly_fence_object_avoidance_guided(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceGuided(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - guided mode''' if not self.mavproxy_can_do_mision_item_protocols(): return self.test_poly_fence_object_avoidance_guided_pathfinding( target_system=target_system, target_component=target_component) - return - # twosquares is currently disabled because of the requirement to have an inclusion fence (which it doesn't have ATM) - # self.test_poly_fence_object_avoidance_guided_two_squares( - # target_system=target_system, - # target_component=target_component) + self.test_poly_fence_object_avoidance_guided_two_squares( + target_system=target_system, + target_component=target_component) - def test_poly_fence_object_avoidance_auto(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceAuto(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - auto mode''' mavproxy = self.start_mavproxy() self.load_fence_using_mavproxy(mavproxy, "rover-path-planning-fence.txt") self.stop_mavproxy(mavproxy) @@ -4823,8 +4823,6 @@ def test_poly_fence_object_avoidance_auto(self, target_system=1, target_componen self.wait_ready_to_arm() self.arm_vehicle() self.set_parameter("FENCE_ENABLE", 1) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") # target_loc is copied from the mission file target_loc = mavutil.location(40.073799, -105.229156) self.wait_location(target_loc, height_accuracy=None, timeout=300) @@ -4872,41 +4870,32 @@ def test_poly_fence_object_avoidance_guided_pathfinding(self, target_system=1, t def WheelEncoders(self): '''make sure wheel encoders are generally working''' - ex = None - try: - self.set_parameters({ - "WENC_TYPE": 10, - "EK3_ENABLE": 1, - "AHRS_EKF_TYPE": 3, - }) - self.reboot_sitl() - self.change_mode("LOITER") - self.wait_ready_to_arm() - self.change_mode("MANUAL") - self.arm_vehicle() - self.set_rc(3, 1600) + self.set_parameters({ + "WENC_TYPE": 10, + "EK3_ENABLE": 1, + "AHRS_EKF_TYPE": 3, + }) + self.reboot_sitl() + self.change_mode("LOITER") + self.wait_ready_to_arm() + self.change_mode("MANUAL") + self.arm_vehicle() + self.set_rc(3, 1600) - m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5) + m = self.assert_receive_message('WHEEL_DISTANCE', timeout=5) - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > 10: - break - dist_home = self.distance_to_home(use_cached_home=True) - m = self.mav.messages.get("WHEEL_DISTANCE") - delta = abs(m.distance[0] - dist_home) - self.progress("dist-home=%f wheel-distance=%f delta=%f" % - (dist_home, m.distance[0], delta)) - if delta > 5: - raise NotAchievedException("wheel distance incorrect") - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - self.disarm_vehicle() - ex = e - self.reboot_sitl() - if ex is not None: - raise ex + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > 10: + break + dist_home = self.distance_to_home(use_cached_home=True) + m = self.mav.messages.get("WHEEL_DISTANCE") + delta = abs(m.distance[0] - dist_home) + self.progress("dist-home=%f wheel-distance=%f delta=%f" % + (dist_home, m.distance[0], delta)) + if delta > 5: + raise NotAchievedException("wheel distance incorrect") + self.disarm_vehicle() def test_poly_fence_object_avoidance_guided_two_squares(self, target_system=1, target_component=1): self.start_subtest("Ensure we can steer around obstacles in guided mode") @@ -5018,159 +5007,86 @@ def PolyFenceAvoidance(self, target_system=1, target_component=1): self.disarm_vehicle() - def test_poly_fence_object_avoidance_guided_bendy_ruler(self, target_system=1, target_component=1): - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.load_fence("rover-path-bendyruler-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071060, -105.227734, 0, 0) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - # FIXME: we don't get within WP_RADIUS of our target?! - self.wait_location(target_loc, timeout=300, accuracy=15) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() + def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1): + '''PolyFence object avoidance tests - bendy ruler''' + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) self.reboot_sitl() - if ex is not None: - raise ex - - def PolyFenceObjectAvoidanceBendyRulerEasier(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests - easier bendy ruler test''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.test_poly_fence_object_avoidance_auto_bendy_ruler_easier( - target_system=target_system, target_component=target_component) - self.test_poly_fence_object_avoidance_guided_bendy_ruler_easier( - target_system=target_system, target_component=target_component) + self.set_parameters({ + "OA_BR_LOOKAHEAD": 50, + }) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071060, -105.227734, 1584, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() - def test_poly_fence_object_avoidance_guided_bendy_ruler_easier(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceBendyRulerEasierGuided(self, target_system=1, target_component=1): '''finish-line issue means we can't complete the harder one. This test can go away once we've nailed that one. The only difference here is the target point. ''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - self.load_fence("rover-path-bendyruler-fence.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('GUIDED') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071260, -105.227000, 0, 0) - self.send_guided_mission_item(target_loc, - target_system=target_system, - target_component=target_component) - # FIXME: we don't get within WP_RADIUS of our target?! - self.wait_location(target_loc, timeout=300, accuracy=15) - self.do_RTL(timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) self.reboot_sitl() - if ex is not None: - raise ex + self.set_parameters({ + "OA_BR_LOOKAHEAD": 60, + }) + self.change_mode('GUIDED') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071260, -105.227000, 1584, 0) + self.send_guided_mission_item(target_loc, + target_system=target_system, + target_component=target_component) + # FIXME: we don't get within WP_RADIUS of our target?! + self.wait_location(target_loc, timeout=300, accuracy=15) + self.do_RTL(timeout=300) + self.disarm_vehicle() - def test_poly_fence_object_avoidance_auto_bendy_ruler_easier(self, target_system=1, target_component=1): + def PolyFenceObjectAvoidanceBendyRulerEasierAuto(self, target_system=1, target_component=1): '''finish-line issue means we can't complete the harder one. This test can go away once we've nailed that one. The only difference here is the target point. ''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - - self.load_fence("rover-path-bendyruler-fence.txt") + self.load_fence_using_mavwp("rover-path-bendyruler-fence.txt") self.load_mission("rover-path-bendyruler-mission-easier.txt") - self.context_push() - ex = None - try: - self.set_parameters({ - "AVOID_ENABLE": 3, - "OA_TYPE": 1, - "OA_LOOKAHEAD": 50, - }) - self.reboot_sitl() - self.change_mode('AUTO') - self.wait_ready_to_arm() - self.arm_vehicle() - self.set_parameters({ - "FENCE_ENABLE": 1, - "WP_RADIUS": 5, - }) - if self.mavproxy is not None: - self.mavproxy.send("fence list\n") - target_loc = mavutil.location(40.071260, -105.227000, 0, 0) - # target_loc is copied from the mission file - self.wait_location(target_loc, timeout=300) - # mission has RTL as last item - self.wait_distance_to_home(3, 7, timeout=300) - self.disarm_vehicle() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - self.disarm_vehicle() - self.reboot_sitl() - if ex is not None: - raise ex - def PolyFenceObjectAvoidance(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests''' - self.test_poly_fence_object_avoidance_auto( - target_system=target_system, - target_component=target_component) - self.test_poly_fence_object_avoidance_guided( - target_system=target_system, - target_component=target_component) - - def PolyFenceObjectAvoidanceBendyRuler(self, target_system=1, target_component=1): - '''PolyFence object avoidance tests - bendy ruler''' - if not self.mavproxy_can_do_mision_item_protocols(): - return - # bendy Ruler isn't as flexible as Dijkstra for planning, so - # it gets a simpler test: - self.test_poly_fence_object_avoidance_guided_bendy_ruler( - target_system=target_system, - target_component=target_component, - ) + self.set_parameters({ + "AVOID_ENABLE": 3, + "OA_TYPE": 1, # BendyRuler + "FENCE_ENABLE": 1, + "WP_RADIUS": 5, + }) + self.reboot_sitl() + self.set_parameters({ + "OA_BR_LOOKAHEAD": 60, + }) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + target_loc = mavutil.location(40.071260, -105.227000, 1584, 0) + # target_loc is copied from the mission file + self.wait_location(target_loc, timeout=300) + # mission has RTL as last item + self.wait_distance_to_home(3, 7, timeout=300) + self.disarm_vehicle() def test_scripting_simple_loop(self): self.start_subtest("Scripting simple loop") @@ -6861,10 +6777,12 @@ def tests(self): self.SkidSteer, self.PolyFence, self.PolyFenceAvoidance, - self.PolyFenceObjectAvoidance, + self.PolyFenceObjectAvoidanceAuto, + self.PolyFenceObjectAvoidanceGuided, self.PolyFenceObjectAvoidanceBendyRuler, self.SendToComponents, - self.PolyFenceObjectAvoidanceBendyRulerEasier, + self.PolyFenceObjectAvoidanceBendyRulerEasierGuided, + self.PolyFenceObjectAvoidanceBendyRulerEasierAuto, self.SlewRate, self.Scripting, self.ScriptingSteeringAndThrottle, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 19b9f946d92cbe..67cdbf54e4cdb0 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2112,6 +2112,14 @@ def load_fence(self, filename): (mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, locs), ]) + def load_fence_using_mavwp(self, filename): + filepath = os.path.join(testdir, self.current_test_name_directory, filename) + if not os.path.exists(filepath): + filepath = self.generic_mission_filepath_for_filename(filename) + self.progress("Loading fence from (%s)" % str(filepath)) + items = self.mission_item_protocol_items_from_filepath(mavwp.MissionItemProtocol_Fence, filepath) + self.check_fence_upload_download(items) + def send_reboot_command(self): self.mav.mav.command_long_send(self.sysid_thismav(), 1, @@ -5124,7 +5132,7 @@ def load_mission(self, filename, strict=True): os.path.join(testdir, self.current_test_name_directory, filename), strict=strict) - def wp_to_mission_item_int(self, wp): + def wp_to_mission_item_int(self, wp, mission_type): '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as MISSION_ITEM_INT to give cm level accuracy Swiped from mavproxy_wp.py @@ -5145,19 +5153,35 @@ def wp_to_mission_item_int(self, wp): wp.param4, int(wp.x*1.0e7), int(wp.y*1.0e7), - wp.z) + wp.z, + mission_type, + ) return wp_int - def mission_from_filepath(self, filepath, target_system=1, target_component=1): + def mission_item_protocol_items_from_filepath(self, + loaderclass, + filepath, + target_system=1, + target_component=1, + ): '''returns a list of mission-item-ints from filepath''' - self.progress("filepath: %s" % filepath) - self.progress("Loading mission (%s)" % os.path.basename(filepath)) - wploader = mavwp.MAVWPLoader( + # self.progress("filepath: %s" % filepath) + self.progress("Loading {loaderclass.itemstype()} (%s)" % os.path.basename(filepath)) + wploader = loaderclass( target_system=target_system, target_component=target_component ) wploader.load(filepath) - return [self.wp_to_mission_item_int(x) for x in wploader.wpoints] + return [self.wp_to_mission_item_int(x, wploader.mav_mission_type()) for x in wploader.wpoints] # noqa:502 + + def mission_from_filepath(self, filepath, target_system=1, target_component=1): + '''returns a list of mission-item-ints from filepath''' + return self.mission_item_protocol_items_from_filepath( + mavwp.MAVWPLoader, + filepath, + target_system=target_system, + target_component=target_component, + ) def sitl_home_string_from_mission(self, filename): '''return a string of the form "lat,lng,yaw,alt" from the home @@ -5178,6 +5202,10 @@ def get_home_tuple_from_mission(self, filename): os.path.join(testdir, self.current_test_name_directory, filename) ) + def get_home_location_from_mission(self, filename): + (home_lat, home_lon, home_alt, heading) = self.get_home_tuple_from_mission("rover-path-planning-mission.txt") + return mavutil.location(home_lat, home_lon) + def get_home_tuple_from_mission_filepath(self, filepath): '''gets item 0 from the mission file, returns a tuple suitable for passing to customise_SITL_commandline as --home. Yaw will be @@ -9078,7 +9106,7 @@ def upload_using_mission_protocol(self, mission_type, items): raise NotAchievedException("received request for item from wrong mission type") if items[m.seq].mission_type != mission_type: - raise NotAchievedException("supplied item not of correct mission type") + raise NotAchievedException(f"supplied item not of correct mission type (want={mission_type} got={items[m.seq].mission_type}") # noqa:501 if items[m.seq].target_system != target_system: raise NotAchievedException("supplied item not of correct target system") if items[m.seq].target_component != target_component: From f60ecbfe4e53a0dc27e376b6dd18bdcf745d53b2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 29 Jul 2024 08:59:17 +1000 Subject: [PATCH 77/77] autotest: disable PolyFenceObjectAvoidanceBendyRuler as it is unreliable ATM --- Tools/autotest/rover.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 46f1f68b59ba45..ad4deeb0f3f0da 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6825,6 +6825,7 @@ def disabled_tests(self): return { "SlewRate": "got timing report failure on CI", "MAV_CMD_NAV_SET_YAW_SPEED": "compiled out of code by default", + "PolyFenceObjectAvoidanceBendyRuler": "unreliable", } def rc_defaults(self):