From 24de88f85cf248eeec868037b5605c41c6788481 Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Tue, 20 Feb 2024 23:28:48 +0530 Subject: [PATCH 01/49] AP_DDS: Add IMU publisher * Using NED frame --- libraries/AP_DDS/AP_DDS_Client.cpp | 60 +++++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 7 +++ libraries/AP_DDS/AP_DDS_Frames.h | 1 + libraries/AP_DDS/AP_DDS_Topic_Table.h | 12 ++++ libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl | 48 ++++++++++++++++ libraries/AP_DDS/dds_xrce_profile.xml | 23 ++++++++ 6 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index b743ed8aa01510..871ef0dfaafa08 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1,13 +1,13 @@ #include #if AP_DDS_ENABLED - #include #include #include #include #include +#include #include #include #include @@ -32,6 +32,7 @@ static constexpr uint8_t ENABLED_BY_DEFAULT = 1; static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5; static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33; static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; @@ -417,6 +418,42 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) } } +void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg) +{ + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID); + + auto &imu = AP::ins(); + auto &ahrs = AP::ahrs(); + + WITH_SEMAPHORE(ahrs.get_semaphore()); + + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + msg.orientation.x = orientation[0]; + msg.orientation.y = orientation[1]; + msg.orientation.z = orientation[2]; + msg.orientation.w = orientation[3]; + } + msg.orientation_covariance[0] = -1; + + uint8_t accel_index = ahrs.get_primary_accel_index(); + uint8_t gyro_index = ahrs.get_primary_gyro_index(); + const Vector3f accel_data = imu.get_accel(accel_index); + const Vector3f gyro_data = imu.get_gyro(gyro_index); + + // Populate the message fields + msg.linear_acceleration.x = accel_data.x; + msg.linear_acceleration.y = accel_data.y; + msg.linear_acceleration.z = accel_data.z; + + msg.angular_velocity.x = gyro_data.x; + msg.angular_velocity.y = gyro_data.y; + msg.angular_velocity.z = gyro_data.z; + msg.angular_velocity_covariance[0] = -1; + msg.linear_acceleration_covariance[0] = -1; +} + void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { update_topic(msg.clock); @@ -958,6 +995,21 @@ void AP_DDS_Client::write_tx_local_velocity_topic() } } +void AP_DDS_Client::write_imu_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(&imu_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::IMU_PUB)].dw_id, &ub, topic_size); + const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} + void AP_DDS_Client::write_geo_pose_topic() { WITH_SEMAPHORE(csem); @@ -1023,6 +1075,12 @@ void AP_DDS_Client::update() write_tx_local_velocity_topic(); } + if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) { + update_topic(imu_topic); + last_imu_time_ms = cur_time_ms; + write_imu_topic(); + } + if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { update_topic(geo_pose_topic); last_geo_pose_time_ms = cur_time_ms; diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 3751a697e8c1bd..65ce8fd5126fc6 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -11,6 +11,7 @@ #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" +#include "sensor_msgs/msg/Imu.h" #include "sensor_msgs/msg/Joy.h" #include "geometry_msgs/msg/PoseStamped.h" #include "geometry_msgs/msg/TwistStamped.h" @@ -62,6 +63,7 @@ class AP_DDS_Client geometry_msgs_msg_TwistStamped tx_local_velocity_topic; sensor_msgs_msg_BatteryState battery_state_topic; sensor_msgs_msg_NavSatFix nav_sat_fix_topic; + sensor_msgs_msg_Imu imu_topic; rosgraph_msgs_msg_Clock clock_topic; // incoming joystick data static sensor_msgs_msg_Joy rx_joy_topic; @@ -88,6 +90,7 @@ class AP_DDS_Client static void update_topic(geometry_msgs_msg_PoseStamped& msg); static void update_topic(geometry_msgs_msg_TwistStamped& msg); static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg); + static void update_topic(sensor_msgs_msg_Imu& msg); static void update_topic(rosgraph_msgs_msg_Clock& msg); // subscription callback function @@ -112,6 +115,8 @@ class AP_DDS_Client uint64_t last_nav_sat_fix_time_ms; // The last ms timestamp AP_DDS wrote a BatteryState message uint64_t last_battery_state_time_ms; + // The last ms timestamp AP_DDS wrote an IMU message + uint64_t last_imu_time_ms; // The last ms timestamp AP_DDS wrote a Local Pose message uint64_t last_local_pose_time_ms; // The last ms timestamp AP_DDS wrote a Local Velocity message @@ -188,6 +193,8 @@ class AP_DDS_Client void write_tx_local_velocity_topic(); //! @brief Serialize the current geo_pose and publish to the IO stream(s) void write_geo_pose_topic(); + //! @brief Serialize the current IMU data and publish to the IO stream(s) + void write_imu_topic(); //! @brief Serialize the current clock and publish to the IO stream(s) void write_clock_topic(); //! @brief Update the internally stored DDS messages with latest data diff --git a/libraries/AP_DDS/AP_DDS_Frames.h b/libraries/AP_DDS/AP_DDS_Frames.h index 0bdaf354ec116f..3b6898f62d8ddd 100644 --- a/libraries/AP_DDS/AP_DDS_Frames.h +++ b/libraries/AP_DDS/AP_DDS_Frames.h @@ -3,5 +3,6 @@ static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; // https://www.ros.org/reps/rep-0105.html#base-link static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +static constexpr char BASE_LINK_NED_FRAME_ID[] = "base_link_ned"; // https://www.ros.org/reps/rep-0105.html#map static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 8df056d484fc71..96dfd75fe9968e 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -3,6 +3,7 @@ #include "tf2_msgs/msg/TFMessage.h" #include "sensor_msgs/msg/BatteryState.h" #include "geographic_msgs/msg/GeoPoseStamped.h" +#include "sensor_msgs/msg/Imu.h" #include "uxr/client/client.h" @@ -15,6 +16,7 @@ enum class TopicIndex: uint8_t { NAV_SAT_FIX_PUB, STATIC_TRANSFORMS_PUB, BATTERY_STATE_PUB, + IMU_PUB, LOCAL_POSE_PUB, LOCAL_VELOCITY_PUB, GEOPOSE_PUB, @@ -73,6 +75,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_profile_label = "batterystate0__dw", .dr_profile_label = "", }, + { + .topic_id = to_underlying(TopicIndex::IMU_PUB), + .pub_id = to_underlying(TopicIndex::IMU_PUB), + .sub_id = to_underlying(TopicIndex::IMU_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID}, + .topic_profile_label = "imu__t", + .dw_profile_label = "imu__dw", + .dr_profile_label = "", + }, { .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), diff --git a/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl b/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl new file mode 100644 index 00000000000000..2770316110bf82 --- /dev/null +++ b/libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl @@ -0,0 +1,48 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from sensor_msgs/msg/Imu.msg +// generated code does not contain a copyright notice + +#include "geometry_msgs/msg/Quaternion.idl" +#include "geometry_msgs/msg/Vector3.idl" +#include "std_msgs/msg/Header.idl" + +module sensor_msgs { + module msg { + typedef double double__9[9]; + @verbatim (language="comment", text= + "This is a message to hold data from an IMU (Inertial Measurement Unit)" "\n" + "" "\n" + "Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec" "\n" + "" "\n" + "If the covariance of the measurement is known, it should be filled in (if all you know is the" "\n" + "variance of each measurement, e.g. from the datasheet, just put those along the diagonal)" "\n" + "A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the" "\n" + "data a covariance will have to be assumed or gotten from some other source" "\n" + "" "\n" + "If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an" "\n" + "orientation estimate), please set element 0 of the associated covariance matrix to -1" "\n" + "If you are interpreting this message, please check for a value of -1 in the first element of each" "\n" + "covariance matrix, and disregard the associated estimate.") + struct Imu { + std_msgs::msg::Header header; + + geometry_msgs::msg::Quaternion orientation; + + @verbatim (language="comment", text= + "Row major about x, y, z axes") + double__9 orientation_covariance; + + geometry_msgs::msg::Vector3 angular_velocity; + + @verbatim (language="comment", text= + "Row major about x, y, z axes") + double__9 angular_velocity_covariance; + + geometry_msgs::msg::Vector3 linear_acceleration; + + @verbatim (language="comment", text= + "Row major x, y z") + double__9 linear_acceleration_covariance; + }; + }; +}; diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index b8e80c6e49e6cc..7d4e4c2001a518 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -114,6 +114,29 @@ + + rt/ap/imu/experimental/data + sensor_msgs::msg::dds_::Imu_ + + KEEP_LAST + 20 + + + + + NO_KEY + rt/ap/imu/experimental/data + sensor_msgs::msg::dds_::Imu_ + + + + RELIABLE + + + TRANSIENT_LOCAL + + + rt/ap/pose/filtered geometry_msgs::msg::dds_::PoseStamped_ From 73afd26465cf1ff874ac7a410814f9baa6f31176 Mon Sep 17 00:00:00 2001 From: David Buzz Date: Sat, 2 Mar 2024 17:13:29 +1000 Subject: [PATCH 02/49] AP_HAL_ESP32: bugfixes bump tick rate to 1kz to match chibios experimentally make delay_microseconds not delay, and only yield ( vTaskDelay yields to higher-priority tasks ) ESP32 disable all watchdogs emit info to console to tell user where to connect tcp/udp and what ports comment out bad code throwing a ptr error --- libraries/AP_HAL_ESP32/Scheduler.cpp | 10 +++++----- libraries/AP_HAL_ESP32/WiFiDriver.cpp | 2 +- libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp | 2 +- .../AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig | 12 ++++++------ .../AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig | 12 ++++++------ 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp index 976b7245fdd484..f4a69e2b105d60 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.cpp +++ b/libraries/AP_HAL_ESP32/Scheduler.cpp @@ -231,7 +231,7 @@ void Scheduler::delay(uint16_t ms) void Scheduler::delay_microseconds(uint16_t us) { - if (us < 100) { + if (in_main_thread() && us < 100) { ets_delay_us(us); } else { // Minimum delay for FreeRTOS is 1ms uint32_t tick = portTICK_PERIOD_MS * 1000; @@ -548,10 +548,10 @@ void Scheduler::print_main_loop_rate(void) static int64_t last_run = 0; if (AP_HAL::millis64() - last_run > 10000) { last_run = AP_HAL::millis64(); - const float actual_loop_rate = AP::scheduler().get_filtered_loop_rate_hz(); - const uint16_t expected_loop_rate = AP::scheduler().get_loop_rate_hz(); - hal.console->printf("loop_rate: actual: %uHz, expected: %uHz\n", - (uint16_t)actual_loop_rate, (uint16_t)expected_loop_rate); + // null pointer in here... + //const float actual_loop_rate = AP::scheduler().get_filtered_loop_rate_hz(); + //const uint16_t expected_loop_rate = AP::scheduler().get_loop_rate_hz(); + //hal.console->printf("loop_rate: actual: %uHz, expected: %uHz\n", } } diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.cpp b/libraries/AP_HAL_ESP32/WiFiDriver.cpp index bc57391f009311..9d8b5cfd5c4698 100644 --- a/libraries/AP_HAL_ESP32/WiFiDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiDriver.cpp @@ -59,7 +59,7 @@ void WiFiDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) if (xTaskCreatePinnedToCore(_wifi_thread, "APM_WIFI1", Scheduler::WIFI_SS1, this, Scheduler::WIFI_PRIO1, &_wifi_task_handle,FASTCPU) != pdPASS) { hal.console->printf("FAILED to create task _wifi_thread on FASTCPU\n"); } else { - hal.console->printf("OK created task _wifi_thread on FASTCPU\n"); + hal.console->printf("OK created task _wifi_thread for TCP with PORT 5760 on FASTCPU\n"); } _readbuf.set_size(RX_BUF_SIZE); diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp index 7352254d85fe81..9348f56cc7ac89 100644 --- a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp @@ -62,7 +62,7 @@ void WiFiUdpDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) if (xTaskCreatePinnedToCore(_wifi_thread2, "APM_WIFI2", Scheduler::WIFI_SS2, this, Scheduler::WIFI_PRIO2, &_wifi_task_handle,FASTCPU) != pdPASS) { hal.console->printf("FAILED to create task _wifi_thread2 on FASTCPU\n"); } else { - hal.console->printf("OK created task _wifi_thread2 on FASTCPU\n"); + hal.console->printf("OK created task _wifi_thread2 for UDP on port 14550 on FASTCPU\n"); //UDP_PORT } _readbuf.set_size(RX_BUF_SIZE); diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig index e1a5a3b0da288b..d5823701001e8f 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig @@ -56,7 +56,7 @@ CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y # CONFIG_BOOTLOADER_FACTORY_RESET is not set # CONFIG_BOOTLOADER_APP_TEST is not set CONFIG_BOOTLOADER_REGION_PROTECTION_ENABLE=y -CONFIG_BOOTLOADER_WDT_ENABLE=y +CONFIG_BOOTLOADER_WDT_ENABLE=n # CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE is not set CONFIG_BOOTLOADER_WDT_TIME_MS=9000 # CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE is not set @@ -455,10 +455,10 @@ CONFIG_ESP_CONSOLE_UART=y CONFIG_ESP_CONSOLE_MULTIPLE_UART=y CONFIG_ESP_CONSOLE_UART_NUM=0 CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200 -CONFIG_ESP_INT_WDT=y +CONFIG_ESP_INT_WDT=n CONFIG_ESP_INT_WDT_TIMEOUT_MS=1000 CONFIG_ESP_INT_WDT_CHECK_CPU1=y -CONFIG_ESP_TASK_WDT=y +CONFIG_ESP_TASK_WDT=n # CONFIG_ESP_TASK_WDT_PANIC is not set CONFIG_ESP_TASK_WDT_TIMEOUT_S=5 CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0=y @@ -579,7 +579,7 @@ CONFIG_FREERTOS_TICK_SUPPORT_CORETIMER=y # CONFIG_FREERTOS_CORETIMER_0 is not set CONFIG_FREERTOS_CORETIMER_1=y CONFIG_FREERTOS_SYSTICK_USES_CCOUNT=y -CONFIG_FREERTOS_HZ=500 +CONFIG_FREERTOS_HZ=1000 CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set @@ -1150,10 +1150,10 @@ CONFIG_CONSOLE_UART_DEFAULT=y CONFIG_CONSOLE_UART=y CONFIG_CONSOLE_UART_NUM=0 CONFIG_CONSOLE_UART_BAUDRATE=115200 -CONFIG_INT_WDT=y +CONFIG_INT_WDT=n CONFIG_INT_WDT_TIMEOUT_MS=1000 CONFIG_INT_WDT_CHECK_CPU1=y -CONFIG_TASK_WDT=y +CONFIG_TASK_WDT=n # CONFIG_TASK_WDT_PANIC is not set CONFIG_TASK_WDT_TIMEOUT_S=5 CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig index 3d841f1251b122..21c2a67c1cc01c 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig @@ -55,7 +55,7 @@ CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y # CONFIG_BOOTLOADER_FACTORY_RESET is not set # CONFIG_BOOTLOADER_APP_TEST is not set CONFIG_BOOTLOADER_REGION_PROTECTION_ENABLE=y -CONFIG_BOOTLOADER_WDT_ENABLE=y +CONFIG_BOOTLOADER_WDT_ENABLE=n # CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE is not set CONFIG_BOOTLOADER_WDT_TIME_MS=9000 # CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE is not set @@ -437,10 +437,10 @@ CONFIG_ESP_CONSOLE_UART=y CONFIG_ESP_CONSOLE_MULTIPLE_UART=y CONFIG_ESP_CONSOLE_UART_NUM=0 CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200 -CONFIG_ESP_INT_WDT=y +CONFIG_ESP_INT_WDT=n CONFIG_ESP_INT_WDT_TIMEOUT_MS=1000 CONFIG_ESP_INT_WDT_CHECK_CPU1=y -CONFIG_ESP_TASK_WDT=y +CONFIG_ESP_TASK_WDT=n # CONFIG_ESP_TASK_WDT_PANIC is not set CONFIG_ESP_TASK_WDT_TIMEOUT_S=5 CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0=y @@ -555,7 +555,7 @@ CONFIG_FREERTOS_TICK_SUPPORT_SYSTIMER=y CONFIG_FREERTOS_CORETIMER_SYSTIMER_LVL1=y # CONFIG_FREERTOS_CORETIMER_SYSTIMER_LVL3 is not set CONFIG_FREERTOS_SYSTICK_USES_SYSTIMER=y -CONFIG_FREERTOS_HZ=500 +CONFIG_FREERTOS_HZ=1000 CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set @@ -1087,10 +1087,10 @@ CONFIG_CONSOLE_UART_DEFAULT=y CONFIG_CONSOLE_UART=y CONFIG_CONSOLE_UART_NUM=0 CONFIG_CONSOLE_UART_BAUDRATE=115200 -CONFIG_INT_WDT=y +CONFIG_INT_WDT=n CONFIG_INT_WDT_TIMEOUT_MS=1000 CONFIG_INT_WDT_CHECK_CPU1=y -CONFIG_TASK_WDT=y +CONFIG_TASK_WDT=n # CONFIG_TASK_WDT_PANIC is not set CONFIG_TASK_WDT_TIMEOUT_S=5 CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y From a836bd85831bdf0a07137b9c058f3e1117c2be6a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 03/49] AP_AHRS: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_AHRS/AP_AHRS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 889272b2d438bb..9f41559e1ad092 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -256,7 +256,7 @@ void AP_AHRS::init() external.init(); #endif -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#if AP_CUSTOMROTATIONS_ENABLED // convert to new custom rotation // PARAMETER_CONVERSION - Added: Nov-2021 if (_board_orientation == ROTATION_CUSTOM_OLD) { @@ -274,7 +274,7 @@ void AP_AHRS::init() AP::custom_rotations().convert(ROTATION_CUSTOM_1, rpy[0], rpy[1], rpy[2]); } } -#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#endif // AP_CUSTOMROTATIONS_ENABLED } // updates matrices responsible for rotating vectors from vehicle body From 093deed6106239bf13e98a374d441ee694ebc624 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 04/49] AP_Compass: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_Compass/AP_Compass.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index ad7397d0617eb0..039ade2d5a70e2 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -731,7 +731,7 @@ void Compass::init() // convert to new custom rotation method // PARAMETER_CONVERSION - Added: Nov-2021 -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#if AP_CUSTOMROTATIONS_ENABLED for (StateIndex i(0); i 1 // Look if there was a primary compass setup in previous version From 2f30fa2f561415c585ff8084666d1bf514bdac4f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 05/49] AP_CustomRotations: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_CustomRotations/AP_CustomRotations.cpp | 8 +++++--- libraries/AP_CustomRotations/AP_CustomRotations.h | 6 ++++++ libraries/AP_CustomRotations/AP_CustomRotations_config.h | 7 +++++++ .../AP_CustomRotations/AP_CustomRotations_params.cpp | 9 +++++---- 4 files changed, 23 insertions(+), 7 deletions(-) create mode 100644 libraries/AP_CustomRotations/AP_CustomRotations_config.h diff --git a/libraries/AP_CustomRotations/AP_CustomRotations.cpp b/libraries/AP_CustomRotations/AP_CustomRotations.cpp index 0af266ea1fe44e..ad938c1b109577 100644 --- a/libraries/AP_CustomRotations/AP_CustomRotations.cpp +++ b/libraries/AP_CustomRotations/AP_CustomRotations.cpp @@ -1,3 +1,7 @@ +#include "AP_CustomRotations_config.h" + +#if AP_CUSTOMROTATIONS_ENABLED + #include "AP_CustomRotations.h" #include @@ -5,8 +9,6 @@ #include #include -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) - const AP_Param::GroupInfo AP_CustomRotations::var_info[] = { // @Param: _ENABLE @@ -156,4 +158,4 @@ AP_CustomRotations &custom_rotations() } -#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#endif // AP_CUSTOMROTATIONS_ENABLED diff --git a/libraries/AP_CustomRotations/AP_CustomRotations.h b/libraries/AP_CustomRotations/AP_CustomRotations.h index f26c43b7106027..7b802334d8495e 100644 --- a/libraries/AP_CustomRotations/AP_CustomRotations.h +++ b/libraries/AP_CustomRotations/AP_CustomRotations.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_CustomRotations_config.h" + +#if AP_CUSTOMROTATIONS_ENABLED + #include #include @@ -80,3 +84,5 @@ namespace AP { AP_CustomRotations &custom_rotations(); }; + +#endif // AP_CUSTOMROTATIONS_ENABLED diff --git a/libraries/AP_CustomRotations/AP_CustomRotations_config.h b/libraries/AP_CustomRotations/AP_CustomRotations_config.h new file mode 100644 index 00000000000000..67aa8f5b95c70d --- /dev/null +++ b/libraries/AP_CustomRotations/AP_CustomRotations_config.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +#ifndef AP_CUSTOMROTATIONS_ENABLED +#define AP_CUSTOMROTATIONS_ENABLED 1 +#endif diff --git a/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp b/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp index 07d260d2cbc6e4..b8904c1bd1efca 100644 --- a/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp +++ b/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp @@ -1,7 +1,8 @@ -#include "AP_CustomRotations.h" -#include +#include "AP_CustomRotations_config.h" + +#if AP_CUSTOMROTATIONS_ENABLED -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#include "AP_CustomRotations.h" const AP_Param::GroupInfo AP_CustomRotation_params::var_info[] = { @@ -33,4 +34,4 @@ AP_CustomRotation_params::AP_CustomRotation_params() { AP_Param::setup_object_defaults(this, var_info); } -#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#endif // AP_CUSTOMROTATIONS_ENABLED From 077dd82bdc5f4356b3bc045f035f02b9481d80b7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 06/49] AP_HAL_ChibiOS: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index 47e90a88288992..00906d54108351 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -441,3 +441,7 @@ #ifndef AP_RC_CHANNEL_ENABLED #define AP_RC_CHANNEL_ENABLED 0 #endif + +#ifndef AP_CUSTOMROTATIONS_ENABLED +#define AP_CUSTOMROTATIONS_ENABLED 0 +#endif From cf9c85d295617582d99a87128291b572a4fe1a53 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 07/49] AP_Math: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_Math/quaternion.cpp | 4 ++-- libraries/AP_Math/vector3.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index a7be6db0e9c7e9..552dd968527dfd 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -381,8 +381,8 @@ void QuaternionT::from_rotation(enum Rotation rotation) case ROTATION_CUSTOM_1: case ROTATION_CUSTOM_2: -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) - // Do not support custom rotations on Periph +#if AP_CUSTOMROTATIONS_ENABLED + // custom rotations not supported on eg. Periph by default AP::custom_rotations().from_rotation(rotation, *this); return; #endif diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 9a1ff1a93bd378..021b737a013158 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -259,8 +259,8 @@ void Vector3::rotate(enum Rotation rotation) } case ROTATION_CUSTOM_1: case ROTATION_CUSTOM_2: -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) - // Do not support custom rotations on Periph +#if AP_CUSTOMROTATIONS_ENABLED + // custom rotations not supported on eg. Periph by default AP::custom_rotations().rotate(rotation, *this); return; #endif From bff8688ac9ca20d6ecac6bd75ebe4893ecb4f4eb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 08/49] AP_Vehicle: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- libraries/AP_Vehicle/AP_Vehicle.cpp | 4 ++++ libraries/AP_Vehicle/AP_Vehicle.h | 3 +++ 2 files changed, 7 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 41885fde22e635..ff42ea97a108ba 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -94,9 +94,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(airspeed, "ARSPD", 10, AP_Vehicle, AP_Airspeed), #endif +#if AP_CUSTOMROTATIONS_ENABLED // @Group: CUST_ROT // @Path: ../AP_CustomRotations/AP_CustomRotations.cpp AP_SUBGROUPINFO(custom_rotations, "CUST_ROT", 11, AP_Vehicle, AP_CustomRotations), +#endif #if HAL_WITH_ESC_TELEM // @Group: ESC_TLM @@ -488,7 +490,9 @@ void AP_Vehicle::setup() fence.init(); #endif +#if AP_CUSTOMROTATIONS_ENABLED custom_rotations.init(); +#endif #if AP_FILTER_ENABLED filters.init(); diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index a6a7c1e9950b55..9aba3a9c5c5a24 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -528,7 +528,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { uint32_t _last_internal_errors; // backup of AP_InternalError::internal_errors bitmask +#if AP_CUSTOMROTATIONS_ENABLED AP_CustomRotations custom_rotations; +#endif + #if AP_FILTER_ENABLED AP_Filters filters; #endif From 40a0e06db3472c4ef74e4a9b26ac18c6f42da715 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 15:39:18 +1100 Subject: [PATCH 09/49] Tools: add and use AP_CUSTOMROTATIONS_ENABLED also add to build_options.py --- 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 3cf264ce3ec993..2edf76e7629663 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -315,6 +315,7 @@ def __init__(self, Feature('Other', 'DRONECAN_SERIAL', 'AP_DRONECAN_SERIAL_ENABLED', 'Enable DroneCAN virtual serial ports', 0, None), Feature('Other', 'Buttons', 'HAL_BUTTON_ENABLED', 'Enable Buttons', 0, None), Feature('Other', 'Logging', 'HAL_LOGGING_ENABLED', 'Enable Logging', 0, None), + Feature('Other', 'CUSTOM_ROTATIONS', 'AP_CUSTOMROTATIONS_ENABLED', 'Enable Custom Rotations', 0, None), # MAVLink section for mavlink features and/or message handling, # rather than for e.g. mavlink-based sensor drivers diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 5a8665bf7faf7c..5c68d53db36994 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -244,6 +244,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_LOGGING_ENABLED', 'AP_Logger::Init'), ('AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'AP_Compass::mag_cal_fixed_yaw'), ('COMPASS_LEARN_ENABLED', 'CompassLearn::update'), + ('AP_CUSTOMROTATIONS_ENABLED', 'AP_CustomRotation::init'), ] def progress(self, msg): From a1a920ba783b12a478bec4c00a1a9dd74e02bad2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 19:35:49 +1100 Subject: [PATCH 10/49] waf: add and use AP_CUSTOMROTATIONS_ENABLED --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 1fd4bc7915ac84..bb683a06147a11 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -888,6 +888,7 @@ def configure_env(self, cfg, env): HAL_RALLY_ENABLED = 0, HAL_SUPPORT_RCOUT_SERIAL = 0, AP_TERRAIN_AVAILABLE = 0, + AP_CUSTOMROTATIONS_ENABLED = 0, ) try: From d6df142eaa934504dd10f8eb89c97950683b0f68 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 13:37:25 +1100 Subject: [PATCH 11/49] GCS_MAVLink: allow FTP to be compiled out with build_options.py --- Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 2 ++ libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 8 +++++++- libraries/GCS_MAVLink/GCS_FTP.cpp | 4 ++-- libraries/GCS_MAVLink/GCS_config.h | 4 ++++ 6 files changed, 18 insertions(+), 3 deletions(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 2edf76e7629663..d63cf9f49bc076 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -332,6 +332,7 @@ def __init__(self, Feature('MAVLink', 'AP_MAVLINK_SERVO_RELAY_ENABLED', 'AP_MAVLINK_SERVO_RELAY_ENABLED', 'Enable handling of ServoRelay mavlink messages', 0, 'SERVORELAY_EVENTS'), # noqa Feature('MAVLink', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'Enable handling of Serial Control mavlink messages', 0, None), # noqa Feature('MAVLink', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'Enable handling of MISSION_REQUEST mavlink messages', 0, None), # noqa + Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP Protocol', 0, None), # noqa Feature('Developer', 'KILL_IMU', 'AP_INERTIALSENSOR_KILL_IMU_ENABLED', 'Allow IMUs to be disabled at runtime', 0, None), Feature('Developer', 'CRASHCATCHER', 'AP_CRASHDUMP_ENABLED', 'Enable CrashCatcher', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 5c68d53db36994..9feb816893b962 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -232,6 +232,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_MAVLINK_SERVO_RELAY_ENABLED', 'GCS_MAVLINK::handle_servorelay_message'), ('AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'GCS_MAVLINK::handle_serial_control'), ('AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'GCS_MAVLINK::handle_mission_request\b'), + ('AP_MAVLINK_FTP_ENABLED', 'GCS_MAVLINK::ftp_worker'), + ('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'), ('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'), ('COMPASS_CAL_ENABLED', 'CompassCalibrator::stop'), diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 736cadfa053a93..52f728c9d293f9 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -941,6 +941,7 @@ class GCS_MAVLINK uint8_t send_parameter_async_replies(); +#if AP_MAVLINK_FTP_ENABLED enum class FTP_OP : uint8_t { None = 0, TerminateSession = 1, @@ -1016,6 +1017,7 @@ class GCS_MAVLINK bool send_ftp_reply(const pending_ftp &reply); void ftp_worker(void); void ftp_push_replies(pending_ftp &reply); +#endif // AP_MAVLINK_FTP_ENABLED void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a4d700c6d478e1..b04f34bd751f86 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1156,10 +1156,12 @@ uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t // we are sending requests for waypoints, penalize streams: interval_ms *= 4; } +#if AP_MAVLINK_FTP_ENABLED if (AP_HAL::millis() - ftp.last_send_ms < 500) { // we are sending ftp replies interval_ms *= 4; } +#endif if (interval_ms > 60000) { return 60000; @@ -4005,9 +4007,11 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif +#if AP_MAVLINK_FTP_ENABLED case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: handle_file_transfer_protocol(msg); break; +#endif #if AP_CAMERA_ENABLED case MAVLINK_MSG_ID_DIGICAM_CONTROL: @@ -6771,10 +6775,12 @@ uint64_t GCS_MAVLINK::capabilities() const } #endif +#if AP_MAVLINK_FTP_ENABLED if (!AP_BoardConfig::ftp_disabled()){ //if ftp disable board option is not set ret |= MAV_PROTOCOL_CAPABILITY_FTP; } - +#endif + return ret; } diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 3225688104b3bb..10725ad586a236 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -17,7 +17,7 @@ #include "GCS_config.h" -#if HAL_GCS_ENABLED +#if AP_MAVLINK_FTP_ENABLED #include @@ -694,4 +694,4 @@ void GCS_MAVLINK::ftp_list_dir(struct pending_ftp &request, struct pending_ftp & AP::FS().closedir(dir); } -#endif // HAL_GCS_ENABLED +#endif // AP_MAVLINK_FTP_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 14449af38220f2..64877c10649dd4 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -84,6 +84,10 @@ #define AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED HAL_ADSB_ENABLED #endif +#ifndef AP_MAVLINK_FTP_ENABLED +#define AP_MAVLINK_FTP_ENABLED HAL_GCS_ENABLED +#endif + // GCS should be using MISSION_REQUEST_INT instead; this is a waste of // flash. MISSION_REQUEST was deprecated in June 2020. We started // sending warnings to the GCS in Sep 2022 if this command was used. From eadf5596ed4dfc6fe9f5a0fba1e3bc367165481b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Mar 2024 08:25:06 +1100 Subject: [PATCH 12/49] AP_TECS: fixed adjusting speed without airspeed sensor the pitch trim variable that was not connected in aparm is needed to allow tuning of the flight speed using PTCH_TRIM_DEG and TRIM_THROTTLE. This was broken in 4.4.x by this PR: https://github.com/ArduPilot/ardupilot/pull/22191 --- libraries/AP_TECS/AP_TECS.cpp | 9 +++++---- libraries/AP_TECS/AP_TECS.h | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 82f1aa06cae23f..604a21287f14db 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -853,7 +853,7 @@ float AP_TECS::_get_i_gain(void) /* calculate throttle, non-airspeed case */ -void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) +void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg) { // reset clip status after possible use of synthetic airspeed _thr_clip_status = clipStatus::NONE; @@ -873,7 +873,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) _pitch_demand_lpf.apply(_pitch_dem, _DT); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); - const float pitch_corrected_lpf = _pitch_measured_lpf.get(); + const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(pitch_trim_deg); const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f) @@ -1205,7 +1205,8 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, - float load_factor) + float load_factor, + float pitch_trim_deg) { uint64_t now = AP_HAL::micros64(); // check how long since we last did the 50Hz update; do nothing in @@ -1379,7 +1380,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _use_synthetic_airspeed_once = false; _using_airspeed_for_throttle = true; } else { - _update_throttle_without_airspeed(throttle_nudge); + _update_throttle_without_airspeed(throttle_nudge, pitch_trim_deg); _using_airspeed_for_throttle = false; } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 4470b700f546f9..fcd3fe97f9bfe7 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -52,7 +52,8 @@ class AP_TECS { int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, - float load_factor); + float load_factor, + float pitch_trim_deg); // demanded throttle in percentage // should return -100 to 100, usually positive unless reverse thrust is enabled via _THRminf < 0 @@ -455,7 +456,7 @@ class AP_TECS { void _update_throttle_with_airspeed(void); // Update Demanded Throttle Non-Airspeed - void _update_throttle_without_airspeed(int16_t throttle_nudge); + void _update_throttle_without_airspeed(int16_t throttle_nudge, float pitch_trim_deg); // get integral gain which is flight_stage dependent float _get_i_gain(void); From bd928fb044a1f2e66c00d98c3098c7d8f7acef87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Mar 2024 08:25:21 +1100 Subject: [PATCH 13/49] Plane: pass pitch trim parameter to TECS --- ArduPlane/ArduPlane.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index b8661a45be9fee..53e265b4fae38c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -597,7 +597,8 @@ void Plane::update_alt() get_takeoff_pitch_min_cd(), throttle_nudge, tecs_hgt_afe(), - aerodynamic_load_factor); + aerodynamic_load_factor, + g.pitch_trim.get()); } } From 7db7f95a3b42d656d29abfa427e36f59cacb4211 Mon Sep 17 00:00:00 2001 From: subashchandar Date: Tue, 5 Dec 2023 17:06:54 +0530 Subject: [PATCH 14/49] AP_HAL_ChibiOS: hwdef: Add new hardware pixflamingo f7 --- Tools/bootloaders/PixFlamingo-F767_bl.bin | Bin 0 -> 16532 bytes .../Pixflamingo-F767-pinout.jpg | Bin 0 -> 54246 bytes .../hwdef/PixFlamingo-F767/README.md | 180 +++++++++++++++ .../hwdef/PixFlamingo-F767/defaults.parm | 1 + .../hwdef/PixFlamingo-F767/hwdef-bl.dat | 63 ++++++ .../hwdef/PixFlamingo-F767/hwdef.dat | 210 ++++++++++++++++++ 6 files changed, 454 insertions(+) create mode 100755 Tools/bootloaders/PixFlamingo-F767_bl.bin create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/Pixflamingo-F767-pinout.jpg create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/README.md create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/defaults.parm create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef-bl.dat create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef.dat diff --git a/Tools/bootloaders/PixFlamingo-F767_bl.bin b/Tools/bootloaders/PixFlamingo-F767_bl.bin new file mode 100755 index 0000000000000000000000000000000000000000..0d1fa5d657fb3e1758753df8d4d040595ceb2340 GIT binary patch literal 16532 zcmcJ03w%>mw)Z}dJo*4aQy@(XBsm2OK@F5gwTz@mIV}yyqk6@n<4G%WQl!M!h@*1} z4;5kNS_NmYb?!y0<8y#k-h1t}*CATg!mvco{S%>M!9UlI!t!2A^p7F5BAi5EO8@!3qqxlY z2Ytpb{O90YO85QIKY%d`|9?83clAGq+yC^q|1a>^GejY3JaYG&3n~Kx`W^G}pMn4R zOZ1ZSJR!eExc=hYyMB9~FnXzTVEN;n0~;TwHAl`XsM)ddn>EW7LjLquL+14t7pD7Y zErEgD9kds#9Eowt{Md5kM#t18`BhK27Iv1Yd{!(nd5P(3IWy|=Jx#9bI#;6Bg`h@= z#q^CftkMHm{#fqP3Cmt{ai?xj`TQLfLlL65)bUGmzEOs84-v>Yu^|wm*MCXQ5P~$y z98RLA<+mfq2qp7S4*C6hxUyGSe&v^jo$spIp%6mHUkp1tzEB<~!nT3&JBq}86~w|Q zd^{dul--xeC-Dd&B|)P6=P;}2Vj7WEu8r|xsC}I0r>?vZQ~Xm`+4&vwayogDOk)a( zg;nB1*{wvmUnR=6OGKG8QX~>3hFpV+8BO~#q7nNdq>~}KgL=R{F}$ze!mW0#F4K4D zf@SA(IM&L%^mCPW5asMJckot~A1(eC5 zJeiq&e~2&M?HUT#%Fns(I#KFrmbrdX$6Yv5+~o2dC+PpPVb+p}{w^`kL#*aI^KLgc zl_ZXG_Osl#o?suNfsY&iCE@=5pnX(532^eb`I* zQE9eT9aXm*Xs+5P3|&~DicK*X{|DDYH4Sz2P?<>IVlAv#n-1U5#iM&Utb1awmyTPdBNk8zD$ zGqImPP3s>Y=9;agQ|({$Fwd=01*KJs{TyL~j_5CGz6uHCf0se@y?UbeAif9TX$0El z)pR^Lljv8b`+1S>0G!c0?eleb|0qqzq%!b>I4N=}Lt98QYt_7TsGp3}cNU0P2gN^E zbFl`zu_q#sPd`zvi|MBHou+;2(C70Ivq*C_IseBkpM0THxXdvI<9v<41BPZdQN-A! zGraZ2%Ap8vt*N|-|C=iZBFmKmy;Icp^LnDJi=l0!<|U#yVsG`&0^b=6=$S!cx@g6@ ztgPE&yM~FfcX)bhmXgI~+4Y^IFcG~fUIkbcmA{FQt6m7DYMu>Kobuv2krXwDK7{c{*Pso&R-WYSy0* zO^i$fPZQDa#0#;0UUlhC(tRX5#yFhHx-I;?0qi9<&mr+Tak77^=T_+@V6@^06ni>?E2T?lMm6us_1 zj_0k#%x>WiDSAIL1legKGZPnQAK;pp1xz!iGWIXAcTpMNjy3=7{eq`^20}KiNBAW$ z_|YXwmqml)4otmre9wFR7B=(MX$nWSFL5n4l^1ooMCM7N+#O?@64uR?MA;o%e(oji zmjjQo4=pJY8SwJSm^KMv!vUsQ)3;4i9HBGwUZPMx+fS5V57E>o`^gn=iGI-PZI|if z)T_t$WHvL!V?$Rc99LPqzPwtsEY$3Fio8wpGQWT*6NUp~BvJyJ)(&YJ&qscv<_4(r z5T$5{jS%5>EB_LY(I)}==D~ye>6lj}$9!c!zn3Tvpx5VLYsV$qJ(Fx_Ot$-RvfX_{ z=aTqlB+GsXe76t%DOp02CENCEUd{!NHV^4U-pV)fz#yVtBeq8<=Z>kmDT#}B?SC6w zYuH~>rR&Q`b8TI6q(6?(xqJ6xmb&HT?TZ%;zOcVi%;@Jch;mUKmGDlrKe>hy(O)Fn zF98;_fp2+;(kLsWw`IOecZ)xu?Aj3BtuledZ-)}mH%AJv6K#tb4<^q@NSJ;9sDw@5 zPpOnzZ`)PM<;{ZyNvw8{1d<$^us;!fY6Rz_gTXr1fNCW%QCtX`|7GZBf7XiEe@<5% z^Z6n1Uq4?UKJ+=287f=*9$|>Tf{-hF)#dbT(u!k5QpgT-(NEMB0fffAL&R}gY6&Og=D5UTjb;us^ws**i-SaOBdF87|S-5?CbbKeVi%$EJ9`thn5e<{ET9b z5z9d2L&&fJv5? z&XI^7Ojuffs_IDUj9#uJ95~-eCb*~%CZf+K0t!Qpv^iZk*FBk{9$hvdA2nb(uT%w>%7(TF2ZBp2`4=F*>Jl=nyMGGERVzj5Kn zBD-|{1aRv72>lun7^R_u^b?6(IN~5OD2McugytA!=rAD~k6G4u@^>_wx4GDphH^%E z?l7_VD`;N|qXcu;Et}C=BwNZmz9?}rO50(YW|SX8UIs!X7>l+wU@c{Y4_?^5kw1t* zvG8Z6c$g9;I9bj#aY8}mRoU+iWbyMv-HsPsS>bXyOJo-AcA3Qd4W)!Z$AjZX+t=$qaABOv`M03u^pMv(lj#Y0Xfm>^#H$)O7*1 z11D<)v@HD_-On(70qD;b@nPPxd@yi#lhKo zA>w?-#f8nS%xvb`!HAibSoWF?9R_hE!ilUHGUp62`D8{c?5|+UIdQj(4S^E*v53C{ zYsEcvS+1xPcZg4^g_c!sf1>&upLrc=hrOMy=}PhH+C=nh0wttOJkqacHGKi{oU5qQ zCVtfRBuPXM&^pKS`IE}0lsA@t)^@Q?f7Qckp>#To^DRM&ejnDKvDb5bTe)qn--Qy< zaVYyBOe|Pm+ZfAmgz}X2m^UH#DPKp|S4l8$Y2Gpm*8Sn(86u588@^Jer_8yA<0@a` zR#@&R;zV>|+<;x5C?AiUYZ$kT))mPDYR4?u=b|l^5B{k*OJYJqiN<7kpNkJ^mr#i% z%HIx^bi7r>T0Z^`(SL?O*VAF(lkM>bvzPJWlug;58`bHSCfAryCbUkI%_JtGwUHj&Mfk%A}k)jlr=?DFt@L`2Jk%DpcAsmykU~ zsT|5k%3j72zQ2Ufl6Qx6RXWeH-bn&`5VF$Q16;}rS<6%7h`xLp_DaAD3@zZeha5rc zQZmETnJqH*?C@02URMVG&3C#6$s9AZ>tuXKeuGWs8s6Q&^{wKz-|_nJ`GHz_0`KY+ z6rF}sbe#L#V(tT}1^7BSk8~C>!|jq>gFn89rm~=KCW^CaS86ofIf}ui3AObHZsS7% z^>&A&W3v#8EE3enUvQ_VcoOKq(0?m_u|Xp)kOzHM-P~GpC|A)lrcTWHu1|H9on?xJ zY0)iL92`wM87Wy7V3Ien$%*LAiIfKXsV1{Kr6;4Ow+J;$(>3Uuu^d`(u42)bgiEy( zyJ?B0za-P5HFQ4)?Rf=gy%Oiz{8M@>?Rjt*QH!2BYuQ))hXShZUv@I>J5mMObmS8}cAOhlX~dWYG6! z}^EgvOB4NefZ%Dk8*zTzUsAC{l>P4|7z9CT$JH;FSLiT#&aeD?AdZ;{v{ zugPzb)Q;?v2l7qg9Mqk?bglR&*-@T|{_)aTxwO15lIrt&R;i}I<<0XGYp9o> z)8s1b?2_GHq6Ff^qVJ&RBRwPhxXbe5SdsGh1^I>67Wp|JQ`#a`7qoaMY_j=wqu*J% z#e3G5)oKy<;$%rmgQz<*JGpZ#f$sn5sfW?q-*IJcbUB4JN!%-j)PhFvK9kZP5|K4Q zT-!=D0t;qSmf%FGy)4;(2Kx8o21TK6%IA`LO*?KKT!?A3h`+30ns=e*OY@X+3hCp> zyHb~zG;fvY+%@CJms1Kb*xyyqdyF_yOv&Yo@{5=WokyLorl8h4*$*xxX>eAq`)V$* zo{ZlyI+M?!|9yJuQIz2JNI*@T4oyjVejoHC))YmzdJ8sjz7#dLftn{TSwuscn*Z$l z{}%sh{x4(H|IYlsy`Sac40m@DLiYli{tnV5h}+X;H2pf#v@Ro6Hk+oobi2A#*&N#L zR-~C3xt$voBcb|8BI@tpJseh>TRPIq7Bd2OFFi+i?B|B0+)%D1qK{r;1!58IrPQQn zXi2ckN4b&T4&4iq{6b=eNO#vn^fho>d&)8yC$*QJ6N%_gI!ijfJtt$ZeEo;&^Cf0^ z{3d?Jgs}A?c1BJ(?*T^0d2E-{=*kW=FZ2CN38Q>Jfp(S91DmZ6SY)DnG(>miT2}R&n$UJnAx&u!O5$S(0wa;1F)7Rd zr`+((z-d(yr?FuQ;k$8GtZbz?VQ0L@3!FN>Fkm0r)K2N*T%U-Z20VHWZ4Tymm}&fU zUZnKZ+)MhmvuvIVH*K$H-$ld%qZjH86N)p9v7wJQo`=}O)sgVbVYwH+^UW2d?2TlY6_-WRyd+QZ{qGHN$ID! zlmxlWyl8A~RBv#++RjX~OlGZ#=-dS5*+zw7rrl4JyGH2P+tnLZ(2~>{zwU5Jhat#V zK@mt#0nPy7ezb6;WPn=WC{2EOiT0d!KcoC4ZkbFWFo3Q}Sj3bBtm1>5?hIGOuGQIGYqzaU>8x|1Q&7Gv2%52zQ&}-5 z`7-HivNAqQ`j={Ob5%@<)ve6SG<942n^N<(${looKcsI9rv#_eKYWl%Z@e|Nm+*ZL z>l~By!1>kqu&l*g*7)+}{8pxsQ7YriUhZW^nV;l4*PPO(cB`rVOoP3BSg!Hq$Ykb) zQ2tJ))adLF^ZmE5=OSZfmjqYf_VICg6kt?jlvh!%ai`^G->vfER^BR^umOO$6PI zjPiiW3WA^R-9;kq6gUeg1vKIc-&iqMJSCZ04S<`7J`vmJ8%^(u%@&KmIlq%pgmyx< z%B;fVY?X-ZNH33|wXwCh&$KIVFw+2yuXyZ7&d=oUxS#he-XcKKpOfk&QVDmY~` zG(uKzo@a5+6Xm^twdZwMiYh~6dKmkn5DP2g?f|j3LBeilZEIOu?b?>L??zs~uw^q* zngg$2aBhA+yhr`6gR$=mzX2_g>iKidyncafTo#%HSo=244z*l3QpeaEd+Kl}MqA5w z>cVdMOe^CIio?bA)=>VJ$1vttCPi@TH?{ z9^7z%jzez@&m%t8n|tF6Yd1yS01fGAW0Tle#O>-Guq-Nx6a|?i1vyA2mEzpuXCi$6 zI!;*20Isl!w;RGKd8TXONSsP9XRVXBtLs)+!N-)ALdPfeKZa21-~ow3Ciag)Y!dcy z;3qjO;$$tSjAJspv2UwP5g%eETDvC1^yIf1Ix(qEo%KH0EGAmb0W{I)&NbG&_=+;A()9HQK5&VPf zDR>lT&bW|W%&3xaL%M#5%Es9QeQTik6+;JxVqe

JKX?Y>0x-x5r6k2E;r(emWjB|+U%2D__viLO%cuay|( zo#D|F0p~&ILEN`*hip-pL@96Qs#eO?@*4AA*KiLg20IHmvrA$Hy;m>p+Ul>^Cbv*6 zM$MhY`rK;yEEoQrvofmOYT>M-sIw5aYMRB`I5m3~!GG2)4t0iE6xMgR)Hu{ucRJDW zg&w-07rGMbIW6z?jYl;S>4m7M&1`7hvk=8TfgOe?iujuA7bOL>fvJ8Oj1Ch+D_>&B5HS{EPz3s=Dqy}?6=rMD-lBqlC<@ycGRtb=7Wm29m zS7fA0$>e>>#3;XyF-kb*&ew`9F71hj3X1Zs5a;16IvG13$-FuRRRz>ZZ>t@YW@?aH zY#HT0W87$)t-`jGzc~)Lg=PW>FPtJh+H79J;hFh6x z^RG$zuLeA55dv&yZ{RJ|;|6`GUTuI)N$~B3O=gQ6O3QzfoLSz4GoMRRsKsl5rZ0%# z_|?_W7g))LdlVO@8Zx8YcWAe~Mb0YU4JoXP*?fQUJqdiCbA5JlJZzu_XNEZut?ndA z9H<@R6ZLRF-*3>(y1qv>tYDNSu^zR+`HIZt<88Yqw;XrQjBeoZOO(>J_?}7{(bDlc zZgdRZf2gIPyO*0?gwr}DQ|o=i-VEM4@S~$L%HP`9S^E6OVm06nSCc|d+4MzOs*-c zSNfh!WiKV2koOtcA#MgAVrJ+nO<2?Z3T~#RM^j1qj-nl3iMA*sozClEbAtXmi1q0Y zHXYQ6dgyG-%S1VV*{AH*qXZ>L&(N{8orV3#~(o7oqNMZT@SmjZuXP%~o^PANvoo5CT)C5fn%&|LV{-F&|Z_VLt>9UCXY>|dqg#SICi&)z5n`!pwsVtbaEmL9XarA3!W4SO%4;vk>SzJ6m-^ zKFy7MNac?#WS#n;&Xd2Xx;+t95_r?_k(vA{zN$uI`*+N#k@OvZEylTlb^^0*u|9|w z4|qN#qNfsEmC@%`h}-D>s!djr5}eqRy)=7?u<60}v0~P;YyoSymMOhalD(*;pl6Mf4WHoFoE)VFf5=CV-Ya=O$oJw zU&uQmk!`w2*bbiv=RJ0>-B;?id*Ru$UYqnMN<@VOC;z*vH-r}GYe&y(6U1^RLfZ>o z0qq5E)((4s@)!DC;>sOAZ=0>MrLVfWLPD~S;Qb8t&Hn<8s0}&E_a*vZ)~2G?d~owp zoDs78`VL|r3F#rT^xTG>sgv@aE!TODw?W&(d1~k=FCcA{Kh!JfOxqyfttcYSanOGX zHWu=_4+}Py50ZNe)+OWef_0fo*>XWX4%!Fq5hR_5LU8~JX19j4xmC-GBovC1pExNj zU68S12ejZkOW4l4Vw5hqx@m>n5#V*a$;9~^~Zt|C@=r{s+0 zt06ViXJs@$>JyXpgSi)eFl5vZhSxHpcy7ym)DK2l^-Vt*3TM=ZCW&na#gU#NR*$XH zR)yQEYrzSt=iMo%3UE3p9J|X^0{;MFjYs##3aY5j%ytLG;{P`Co!=Unjy8(@H$89s z9oT{Lod`!jvpr7SMd0T(8Yq5#*rxN{#D+6&-oMLypUb$CIibS0qeyd##!e;R>_Uum zR=O-JU6#ensWbi91+Su;5!ReJzFc99$JBxnKlG)?82VztcsXCQcI3kx@O}RwLr{bF zoOtxU#Byby`|KU8M_7lI*jWVroWWZ32Vot+tIUeETpY zvmqI;t=aHxpQklGqRbtc;yEi#@f=f02@(Fs) zjurb(>r!~tB2DYa^dj+RSmobKte20e259fc)LeuD+0&lsY4K7kZk@L!`O?!O`Ju^D zIi$GLwdc$wP7W?@WTE4T9zhKrYtV?qihc49y8GOpn6VKrSg^-DE>nHtm^!xvu#5OU zcMenhA9%4NK7{p0Rec6R*7@UE^0|if>*{I^g3&wCH&HQYhr^VPnXwNsLt!vpM`zjs zsdEZ$#lW?AGX&rbsJSJXo<8VxN8HGl>M2D^k@6wFX8lH-ODrVSSii2NR_!^0?TRp-N_=tW`MeJyTe_rmxJ1IWl`X z-{ecK?@|rx|?GOA56XOP+@F9JHh-AjKp__K{{i~S;?j_8|72RrqDa%c) z??GnlCguwC&U?{W?ek7)e%-k9$z9EQX6BC0YIl!%uVt^vb&hj0@CUMrzf|ufMWO~a z)N1hSS8;vcV}#z8-RHJfSI(W{zMZ*hC196Kbp!Gy`r@kTTQceQD#^DFlCu9oOh>k! zOfcK139lgOEyX#+y`lUN^5%Se8p#! zPe^&~PW$#wr(l%U@645tHWZYzePsq_VMf~pMm?_F*n{#fvcg^E^{bh z>|?{nRecFM!g+Mw-o z8c(SD2TOEV*$-xRe*~Gz(`makecL%J^q14}8hLL;OgA-Ib6S_5tT`$Fx_y_ga%YX) zHmqDE!xL>g+fDK*xzRP|c#gTNmlUS-z^qCuR>aQ4cIdLuq_*JgBF3Lz|Cn%&<<@Wc z0M>zlkX2-Hw^7PW73nnwx>PZKP^PQlPTzF-bLn5+HkaeA!g5qKWXy!6lY{P?*)}!| zy!Bk4E7yQ|W2I!|c4EI;;Z(>9$bSZE=)AG7@lMP;gRz}Z=jz!Q)6clO)VU^q2WWm3 zXl}7c7TH{W)z0mC%)&Q@FAh4-9dlABzlc!(fl|<^g1Jno$4m+{4zzmA9Qp`r@U(6s zC=%dK${t^e0uE3B>mLiRhCFdfzcAfpG=sJGX?H{xfH>YhsAfnwk4$ z!wEI}$Y*U=mw&A0S~Pw9Tws4@rF_@o2`9SIS~#cGzNgNWlYRwBI@*(Tv~Sr3I-UR> z=`HGak^OxPcUU!$ZSY~>#NVr?j6mD#eSvYG_B6`--~)BD%0CYJ1ori1==YYt)z6Q+ z&|@mkDsPmkC8}T0eU84NJ-_tN4Gq3meN&th2Lr1cyt)wG(PJaY*TLmP@f{{t<(%rq z-rKCxglJ496gRxW7>^xUgn0k-7qFdd`mTVp*4cps0@)>*Ba zD)&|lu%!R7z<;t=bbqMc*+{Oy9+mlt`pedgzKY~3;IN0nUVHU;tQt~E>|;*VK9(8N z4cM?p)NSGW9);ciG%U#1h^s+OW`VFV(bg>ghwG0g744+%NW-=DM+Y(utTK*c2bG*Q zK!QJHpzVV5YJHf;zCvue3xS(}dwrwpU&F;*w`!`K+#TWAZmJQv<1ZjZJs7WxOGr`w zxohH9q&TGJ#y@4*ZfYmFI^Kem2C13x`?<{*79aRrmxAzS`~{>agx0u(6ov3w+=>*1 z@M8Q^Z3@D3@fPjo3o{N-7|+BDBWD|uw8+4XeIL9TH{KR|bhue=UcBg{4<3YEp9i}D z$zukc)>h1ED%KXsBQx@;W{_8=G?qa#$V-(CpT1c<(}1_j5$j_av8(~Qy9%J(F8X2u z%H1rkNK)?CktcLa7<+L*HOvb z@N8dpHnHx&+al_C*!q1L)-w2#(5`?WV~%DR8=QzB~!KEN)6irK3J$`Br!YaNfdB zcRlBgM{C|m+05Vt(OT;x7USd`xuw=vezsP3M)-blJH5ZgqYLor`{k*U4va*o&q0@b z6g~!Bk~u}HbAfXXWt&bpIg!^_&{ipS~$*$S$ zYN=&UP~E3HZT*A)7|s~=vL6EVKcRacnf8kKp;~nCZTJ>sgsJZV^$E1JQttx#o~1ti zVQ*&6i+!4`Rp#A`i#m6UZ}&UL<)17TsXjozm3YGCPTsLw>YekQ^PI&kwdeu=l+^f7 zC8&QwhpXo}9qs*yL#z=mMSnZeQvcoZTg%s!x70@9zrjAk7c+Nr!@Ttv<&u$FPcn-+ z@QVKiuiSOeSqWQDd2>{9)zh~%`oflJ!`;6Fe8RYtPc~Q|R;YYw7CqMmlNf{T8{G0S z#eYxTcyt?Je#I{V{4!5|uJ%)K;hX%r4L%89=NFX$NWis!F9BcY7{ObeLl^_^>G4T# zl1J*V0^j~CVMua~@@%r7@{Pf_2raezi9h))B7aRH8p3^)dJcR^&phyLjMykXEspw* zkm;|BFV-(_Uy|h5K5&eBXe8~Hlw-PU%`f$7bw9z0Gf8w_oqtl(@8n=Hu>aJB8;N}= zoaEMJUVSy{Qwhp>aq-FHlyAR@%n+{yzb+29)PJY^y7E%+>(7xCohOCyp3u6jepf4B z+H4*hGD=Uw_#jfrh_o!QIUPJvUY zJWWx%AKKKV?SR324}I%=%jfobd|d4=pSv~- z?*4db!mK1z@a0oaEM zm?Ynm=@O)Wl`g*l>0{}%AL)0}=^rBfRyzF>(rxMVF{EEhr_Uh$vvhg{>3s?I!VmAF z<0Ya^$rlCqIlq;(VG!1-V$Sq?pS7`g^uggoG)twni+HpbYlP12Zvz?IG)v^-s`WO7 zVXNx4;mfU*?Gd$qBWD~*nXP?ujAv}sN!dDh#+$)sWM{gwmYkAKs3R8KBJb`(tjG5+ z2d9pg%dnEiOBUOy`?tXx12AweCR|K&<8NPk58D12hhPZO-EORBjSTI+E;rQwSHrcy zVml<{9(mzeZ-(dedf|@kz#|@IhYhV-+!qZU8qaWq_Jgl;(q2JvTaLGsRS4s#$04v! zdK}XDj_j)ZhTX~AF0`B#;rdr6+;rB zLu(`V-~~astv!t+>_7A2tLZG8UuC|1e${x%EGOrU_(|PJcauylAx+Yio+dd*$!3Sc z_$o>nV)CRW+Hx>3$Bej<8|kjuNc+{S6FfB=pFqz-STiZCz};ra;;E5W@gv=%J!<3- zde9tvIwez8wfr>lxv&>K$&7SQtZKqHeoRi6`a=t`b*YUO=XqUmPL)wI;`TI-)7fL; z4B+EZSYQTS19<6Y#=L%@ZD81zGF=TI6-Y(JfsBkG~MaUAbWO{P&=HiN*}ED zanN?Fy+W%7o~>D}D{yB@S>ZlHQp92-)*egjg}#_l#nx2 z8P}uV`n?rawHvt*aw#k9jqgA{(lp3RkMWSJ z##@WF6RV6H&~wAy3IREch+GJ{Gd$moSF#4wHSDc;7CDTFTnM>Zji}p*e#l`& z~|Nr6NmRx+(hVU}NQwYx@{0QLzgtZ7k1lsW_ z_^)k2=sQdLLl$arZQV=;zspK8z4VaHGu#aLQoagvAFZb24~zVv*Do5(A?tWC+6-vaF_1 zOBkJID~AWE44xruCOYag4C|!P78;Qpgi4%KIg9}#5obnD*-Ze-azKLTGWcn(V0T-Cd57+`I)c^S>MSe*x3QshI!( literal 0 HcmV?d00001 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/Pixflamingo-F767-pinout.jpg b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/Pixflamingo-F767-pinout.jpg new file mode 100755 index 0000000000000000000000000000000000000000..ddaa88c4afe332c312042d498ddbe46f30eda3ce GIT binary patch literal 54246 zcmeFZXH-*L*ESpkMCrXlkd8EwULz`9q=_^Ekq!dVJ5dmj-UJk+cPXJGHAqK9dhbO* zY61uWlJITMId?hd+|L;I`@Lhlzn(BQBsZ3Zh}CdYao1F91uS63PS0U*E*XC%k%p?N;3% z*1vCTZvEIs9v&V4JUK<3o&Opa4hZ-6Vf}Mte;F4AFs>_jc(`~3zs7}g#Tz(qDe&-b zh+L&q)FH5NrDDJR>KgUkgq*6+*EvLW5j4--MhIy+#a6hGzlQeP$o{*51^@pV**^#N zuW`+Sh=GN9g#woX1OZ*FX*btZj%~KJHHJ-2PI9OCZpL%EvIiT@#1Up+QIiAlCtrd> zYY;GOqczztWT;$%)be4$5NpV5WACn#qDi^?J(}E1F!sq| zwJKY_&f0hDF*7)B1O#kTN-`b`lF}lM?g!>56F4DctH?+45=3wUN1K8X4MEh<5iNuZ zmYNoL9RRx!Lz5$}wl1@%@8}?~@8wdk+!JMM_S%%}VS8&zo0a+?gX~LCrwm%;5|m>J z^aAdvGr+nOhsMbqx&#qF{y*R0L z>up%O1cAtGfzdZ3^01N65GW$M?h>@R^|v!SJ&=`*Ao@$ta8=$VXsIp_%P_SPhaqBw zU@nZME$5JzpqB(Sm!OUXl+Gnc3Gx|^(l)*XAw8gfJ>$6sMtp-4L$?fp4j;?{zle7b zz1YwgFcwqJT(yU~1hwPN0E2Qy^In2-vzC|8yn;|v6XdTakZ~wm^DxMT-`lMj;5 z!qOge#GUhDNdS8XPhEnd^8R)P229nT?D1z~eUEeL#e!J>cNw9Y{&yMu?=t#-ibe|5 ztXWho00x9y07z11e+i;$V=~_+H#9`wMU+oWjF(cAE0&wIB=&XTD`}tEaJ~esCz*4h zI+tx5Dcyz4;$l@LW8k~S77k(CHZMUVkpplpr7@2+VoiPhU zclkj=Q9SO~Q>r|XrDl{4^d=G((MEvCdcE&dmfbi}M>I3uhX~A-YWdWNGvIsQtv%YG z@x18d<0UAm>C+{sN@(B`q`vRC0h^`5s++`blGKI;)1y( zd=3mzz651IECHt45RU4$6PxAuLGh#7$8=Pj!*kr=7N+>6TM(4f3TyPksKH>g*OODk zSWPk+{O#7be=ue^kmmrbzI#^gi^IVaM!)g7C z;@Cy+=CG@tJ70V`V>L+CdU;dLKu4q?#!_UGPT}^fi$Nru=HNU7OJ%F`f3%M*j@ieyVezpWg(~ z<|XLg8@-z2dg2JZrTip?keaoz$U;EQc}etQdgxErE?MmC$eI~>S6){%F4>Uv+=bn6 zRcpn9C;5o>7&n^LKEAcD`BQC6b=tSpncxB9?Z@{OqVuniRh5(&va$-z!dsJrKIcJ) zc^+MYK9`xJpCv!{xV3FP`qMg&!(XU~ipFf6jN=38_(185Ws@CyZQ`?p_w{JsuYrenm58u0ob}>-ODu zYPixYzC8{N(_${?c-vVh@u*8sw0??|ZXmvPxNZVE#gUIvdYw0s(yNNTkAR-j`N_GJ z-)LoCxH_M-tZe(! zc*^wy)pvH|OV9jICFX6Zce%pV8opvrBzGAlF;_gb(2U)epq`t)yK;Odyb423ooT~p+El13|sIuJaUzLk9jK>|C1Fn z@WWnh^2~}2THKhI>xt71oiS)6T95wV)t76(-!>xfxGCV&?EY2XXmyi;Xiu+pDV-j* zWt)Qfq`?L?*O+ZmdERPpWt{hEl{g8cqXXHK>{zyFw0WvG;7={c_H$-rQ^86T$uqH8>!H@N0b>#OLWGvkKz_!e044o5z2hDsP!RU&eZ!t$KT zO~AbA7BuU+XQS&DWcy*m$`99mjAmb4jkyHf$eiQR&hgNeeLIo@+Hy8(%6XTC-+}swC zj&UErCr3{+NF785Z=4(Mb4SXmag%eOF#sM77HMC$e%b$%wa#YJTxNmmcdw2j8IEay z`qSx+wTQCj$@w|emj>D-(u8d-b%Q_Tq3Yi~Brp#7^K$4K{N6DXy{GGX{PFR5bfOx_ zUz$TBIG~X}qdX*7u~$mYRVkE_LXCUwtonO-c&J=d2o$j97miENeZvFx1ly#G=zyFW zg-;wI+JAv4<6@U0N1usA?&OS)o5H$U5Bx@+8u~13V^360NSLs%WQwpP&n`i0G~;&I ztB91EWfF7a>Tz*9m*aFbtISb@nAgr6HsiW|6*+54R6{%ZEuGk-Xh^!;RS;83WK0CQ`h9rJ zu|!X?;fa2uCb_z@5iF=ws(EVRB1WCpf;|4CM8 z8u7q_dLI))c4wkETRy)z>o#g?+2C`j`D|848*3E`n$I0m?4zRB`l&`6`=mxi{$pNN zy0rGb%&GY2fcq%uG7LXx(ws04krgg*oSfd})uZUryurS~Y|DAvkKS28TWO&sieBrX z3_i;=g7w_((+-#7mJYqa*P*)>AwW=hT9HP(p?un;Fq#|5e|vQSaQ+61dZDBZ-P-gZ-A6Q58xH%?w6pgjq)`# z8>ddo~O)!k4^#4M$by8O8cO|WS((1onm5}4zQ_%X01;S+%t>1IwE=V6%QeM z`j;RcPuEM(Gt*RT^8D1Gc`Xn6Z8P)&ulEv^X9X*OI>63kOXDsmcj3ns8*MI_0?ydA z?s{WClYG%`D{jf+F4Zm5sR53{Vda%2&htqPR8gpAU6#BIbg%emF6tW3`vP}18+ut2 z2G(Tx77b#4N?V6$;t8!vjZ2UsQQWuW=hnp6Ths24s!Pk7OfQFo?3rXeGS$7-#}=of zZ#lN2e3$i!^2LM(I{SRJaFTqRG5@r(mo=67E8)xxlGqSr5%g(jO+V0IE!kI6rwx5o!I>`qRI@NV_H za5OtmZ0^zFRK0c|b6AUohf814EH$EdGTkZHV{rQ_f9>9L*B`obUGaCSxV>Nx(n?RR zL$_|Yo{g4AM}8Uf6^p#aE%6LD?yd?O>j&-Io5Zbd^qw@jf)cY5%6dHeb8f+C++#Gp z=Eu7(;@2y3hzfi8Q%p4O1{ABD8t_0ivw!Gg|2gJa%~HAu+>+kOoR<2?ZE-bki-*dj zykn~uyjkDE*?gA@I(&ESD4WXLp*u>$aBpUYs7BST zdyexG^x?NLAAf{AO~nS&fisL7wPe-nstj)gYuF}ao(r6v-lz(V&m4#q>6Mqi<5XDo zW?x_}&5}dzVB<7u!yLmYS?#AbC759I^J`CnJWvAbksb*ZW}eE#&)#Mv zf`WFQhJ*~SzN(%gZeb3Y*SC9qBVx_kSL>~slI@oYzvV}>9W3`ArJc~Pjjp{v70k%I zf9paBdfqv$Z~l*!^Gd0KdbUQGl0a{&;GVA&%lI>CZzlpdX1&_(3&sV3%6r9uh4URT z-ic0XY-d$#s2-7VPxMXLKj)i!&OX=Q#Bguj`|j>&;ObtYLqJHoyvzjn9~US}p-t>y z(yr=2G}AUmH)Oam(NkKj5k(V|?Zh)V%o2a947N{ueEX_v*z7c(Ur=zFB4amU*ZbK# zTyd>kNeEL@~w&8}K8<)%*aN~G`M)X8)57yiq`x5w}kZ#RCapr9s+nGMh*lD^#}`$w-{Db*)Abg$mo_B$@F$kuq% zq1KEOLQ+H^*5rptTs-7s3&T|GedXht)G(%0JmT2<1aQ|1>A`=R?WJb@m{Pil$E$p6 zdxm^z8?Sk_^C_o4LAB${Ki2!sXb_`mH&2H-z5Pzn+&!+?bhgCpTLA;D!(SAW+vWJZ1)9_W5hPy+zcXv_TNCHVxmsK=b9`Y`YQM4 zd2=H{$O8sw|E`Y~wkyU2fJG<%fi?vI_T~csZ>$Ori*^60}PN#>OPJC1XCq*Y_(RyBgKNSenz$;3v@yX=Kbs+oH<4qo(pt)#mmY zXjPsy?!IWh1f9oN!6SVhr5PQ;53z5K{7a_IpY+VW_9`W5OuU7pJ$hnE5lwo%e;wIv zG-l=d+%nV&U~dQ@&`|M?*evU=HsVt zmlE*7Lx>J)74#h$s&Iz8-;$O(6%#vo;5Is!v~6TR;QIXP^!J!JrTc6t@-J~q#M)Et zy_t?}pL|5;MlbLkI~uEIn=xu)LCeU+BvJMJY1n-G)Ai@l+x*5T`Iz+-zi200&+Bsn zR4b;WqJ8J&?n~lNG@p^)Pg!G)uD!0NW-3oaqw(ZZ%HpC##7Rm3)k~jlqgZp<7j-I4 zR=S4*L@l|bMu0&`)e7sn(}~3pk`te^g)(1zRYSW*&u4_!+JYCK#n1OLD80W&&ddjT z*^V31H)vc;yE+)OJ1ji%vgrNYo|>lHCVlGNFFGmuI+>D9ayU3R=C$cmpPlJ0nicdk zUQMZ=By$C&Tus_@tB`1KR-7G73VY7m%boFTkL!c3^uX;aTCL(m)s`tK3vngZ1#}Ya z7B+8A2+OW3v=f>tKUngZkea!gRWM)Jj6c!6^Hr#fav^!QgPgo3p{^lVb@rjipka!B zm~z-IlhGQNbucthb=vc>UY|JQ;4>z6lNW*a1evC(G)-x5rZl?>)}{3mIXy#d$&-~E zsoj5-hHpQ~wIq69@|qr-i<`ps`m3aEzH(Q)P7*nipD*kqe!vvV2gwc2(6>z zf=X#F$JVbq>EU$VZ)T&ta<9qt>8uKFwmtHF`s#D1s1@=sVLY;-s9D33I@axLBX`=E zk;s6CvsQ_9QQ*f3gDXCjGN8^gJ^G6*r&}t0Q5IKJ@;d zvW3DasEvDa?43%LKF%4>(=WG$(Z&dA+me24za26~ENTktSUMvQVuMc0UV@yySFkt> zOzjU|!O#;P1kues5$+^o-X@_UGHE6-xP;II6V&g zHf}U7Wa&^ffD3>^OgJ`4#gxV!{ul7hv;Cx_M2)kyC;|v0(}*lg1Y-mC?X1DOfB@jc zCzRGD$Py27u7~dz9t~MPk>df{QZBy)6-Xp+fB|B@zTXDG_jC{t8G@Va0#vun2^EHZ zp2BIZgcp#eF&b0W7R5JfxcBV1ARF%fEmcn zZ#70pkqhE{|_HxG_KMIZ2G0lHr!kW*^ za01JR|C#B^=vU({-ntJu2WlViiU`Vz zcg36*ygTqZPwHO-CyNAinb=)OKYIb1OUo?jDaWK*TVi4?sVAorwYYPmm+Ug=c`4w7 z4Gw#pPjOSEx#Kw#EcQLD)UR`Ea zaj(?L^5vbq2Lj(qhl3Ij;Aw!A3gK(McPv>H{*?!eHTv0l6Beq zH22o|^TeQJQ_ZAXgiihPpgFxyT=b;NlU5Q*QT|ezF)dCmvi)3Ns*bowY6$1oh-%kx zQFGF3phPtzMpu_mE7~zNXYPBM6d)qo1X|&Q2rj z+TG@yRIws=j0B%+GR~@H30ge&oW&{f$^jlMY`7L9;B+wDZkP;&425C%TD_`4W)G?`d!B! zZm@eA4yWdN@#^i7F!}^b+51sbjyU61xFK77oEmmhLh;2ve;fP&FnC%im9JoV*Xc0| zF-LpufVx*zQ+8JioDN)AT9VYbS_B4(NoQi&r|0$kh*wM7qi5^BFHJk`S~kYFcunp; zcoD2N-N?CYi_4i-b4TnwE+&S_5tE^nGt1Y(ORRip6PfXC}nGZ+7?y zS}I7pQrBhc@~(BFYT_XQ>v*Ed^QgvA8rHJxM|I{pG3%<4cawbG`D>kDR1jzf#%cAu ztY%>}?*C)%D-KoyyEa#fVt1n}oc4BEa^7U6+(xLI*zG0azu4^vwA60Xygl_GY=3!x zCOOBFr0D}X;#1133~{1kEz0B)#7@>lc0mB$YpMJ2^VRoFzCfLGAd^;C`Qh^U;EYoN zNDHchI)Q3)UPWZ%(q9o+r{;HZ3@Ew)J4XGB9e-5-0C6Dz0NBdo-vAf-3vhqE{wLr< zSRnx3QbVWxz!=_t!tJ*|;C2Wdzr+B*RwMMEtQT^|l>+~H@f+)Hi$eiSnNzk0dLm?g z3Hk-eacHU`teSNLCTJdjgVEjz}W? z$|U%iz)NJ@aw)F5O)>x{d==bPfc&5!Hz_CYjY*|VL(CX`zAfk4&-p4Mo|4pBULN0V z3GpQra%?OI6bpjWg|X~4uBnjQa(>gy#=Ua-*Hs)HgC@7&daFw)gG?Y;X5*QoZbhN( zEm7WU^$Cx*UMSPQYK5)d2>eFsdJLd!bWx6L8YVxU)>VdUhhCfUXb|b(q$iMNsFvu@)%25*O_}YvJkQSa{}|8r?55 zxCya3BOA!v_48va1yaJ(Ug)tfu(Cp~!F$?9flBbBMxc~Fvg(4So`E8%p`ACM05uCc zq$HN`IUE%U@m(i0YMt9C0s=Rf7APRKAeS&8cAz`2&DS1tqHNd1#@L%oWPOD=6H+vU zYuRgWD(v{apKU*Xp~B)H2<)C9E&-$_rM#{s6d#bntn&Wtl0ip(%tziXep%QD0e~YA z9sxZB0XfrE3TO-w;-L5WXJ}>417ss?wd3Lg^VF>QU+f$AHya(uh3|<59bbYDE!X&g z>n%#uw`=hzH9JvkkbE&Ik@B&_}%pY@(>sET!8`g zZS&iOQbucBGLf8r1*M+stFEVfXKv6pAISWGGX9?i6Ik%ySws5BzhMyINZbm7f9YY_ zlK@fV2fx79uU=kbnj#j0x%2GlFGOnJU+<*dd^LufcEOKj(A~sFsYJjLdH_-B()s%( zF`MzyHv5Y|f33y_EX`&tpakO3VTplBWekLOW-$-QD4&bB5boltpN>@bkRi#7ILQd}X zXnG~A_(|$t*Y#h=aWQm5*|$l}T@Q+)KvrZShI+geyk`aWb_0qvx>#2@=}512`RW~7 z*>14K-h`w1mwvSl2md~-3yr@v_xG^?X5rg74f>B}Niz@2l!)!&53Q_AQ!@&gEg$8- zLvYs+U7qRZv4*%1{jXY=-uNGk?g1ff@i_b_5rL=p4wqx8M_;~=fRoJGagXe8EBmij zI?cRy^8OEdNB<-XTNJNvOCfTSVcN2MuHqqN89vC`OhgG+5C6+N|Ca`N`wLlqbA84c zpyK-h@kC8NqcvTT7G>%9I({B*g~!YTeRuJ~-{3C`^}j?G-`1tp|G3bbmdJcGbP&BA zOSDfM67LIn3f_L~=!13Pu(}N*I(~MK6mYNe)W4Yh5o79Z;?%ikcg$3e7L^+QxF&T8 zCfQ1o|H`$#C&%1LdVaF>t9&*-Z;BtirFKqlkW>jOJ|+8WEY(I-q_T#tI0@eivrUs1 z>R}Q~ZYN>yX3Ty@0E%xc{h70aPpxx@6|zQlCZBa32#pD)&>xxwzNx*?Gg(+DjA}oE(^mv(E<%WqSI^oo8IJ0_8+2e#=iSF>K%}#y#a)%mvZ|TYm%YQTLf24&Gv^L zM{ahUw~y=}hg@O$vA>W<`-9lx3p|tSO^-L|o|}Oq{xL60ekjYT-TSvxObLyoGR%~> zb-f`2WwOBnRSyiT~&uuhBF$GJz7dVA+1*SB&!r>9mmS^^Ik!c&_$ zoOf;OEVdQ+h6AqWqn#1n5ipDJE@Ldr8=7NJ3?2Qsb@1FGY{rTGVv2Z78DW8IkwidE zz;~RS(M9LwYHFfh-6`kDse9u~Q$oDrF$2@(aTljB0{7>?8i+Fsl}(4E}R2`18%8UKpltjQ;m+l#MK z+E9&ZoTlY`O{|F90wcpT%9*T~g4<;tCX}MmRVv3hkrGipDc%N1B~eAz0$jNqh7fA) zXbnpcU;py3c&U6xWJ!)gnoUE#$##=Oo_+DOY0-NZZ(Er`;sMOMC+Drxt1VHF5U}tT z%?l^d4=ifX2F0>$iPS_ zI&^A4aIBVN2lRx~Vd-4Y-pmm>hJARnVt@%-1w_*~{Dt9Nu?2{{ z-H*JvmAv1BWT5?&$T&>Y6rd4k{jL%G1f0**3D5|pAivAKvF3;O(G~zr{&|W`>Nl4H z2#-8;Jr&?5Q$L5WaSn@+&a%PNla49sug~{a-`+pAJ^r3r&Z#%+w;(joLxw{r(EHf8Ojoll?42hAxYYYLCP1 z)^f@6X&Kopk>)4FNo%$PmKkXqY0_iHhBSREBRdSUD!qPNnu( zDuh#8Ne;^YQJngOI8%)IZUc#H9ji-?$-DQ~swt1=hXjl)*o&RA8*&UXegL{hi$)G= z&Jd1(#W)==kaZbXJIrQgxaMIBzw^v7eif`B{*u61N@;Dzca3XCRX}17bqHZt@Tuvj z&t%Bjl`%H&_g*EMG<>4w#QdPthT^l+Xg|BeanHo7!}jXZE5kWRXm484YtJFVtu)~< zI@9q0qv8PzC#LNylTr+V>senDPNWap-|Vx|5+#0I$Cvhlc42G|)`$Wi=A?F|{zXPJ zgQY!->@%M4-!gDkH~7%)JszRe9uvK@&c(<9!Z3w{c=eE+A-j+%w# z0U~9rX&vJ2wLI?#jSX%$Mx{j@?U%=V*g>oSDq+>hwj1d)@NjsodbH)9x3!C4=Eoxq z=e`^K&Q+L1HY8|2V8dx0p~oEexI%PgDfp+>b<{xDRQyi8w+!7IPd@QSl}_R*+mjpj zL(MTf-)VfL)NN{O)=_ z&3|$|7i1r_$1DI~-Ie3ea~*t-k%ixa9QCgtHxS?k_y*q^*##X8ew?xMs~`2I|;}{J%bM@Si?lqJn~oc*@58U8#%zn;L8rN8=uX5i$KiWUaG2ON4PPNLy zu*WF}`X;2n`7!Nhnyz8kLJR`_5I&j2olj|ScSMOg`d`4!{}m{CN5~v>lHu7i!ooeu zC3V{->ZZl&qI44Phu%;4auM;+jo9)??0RjIi{)4`EXf&epA{uB>g%n*gmS@MleM0x zh+mt+dt{CnhfqVe2oM9n{%`}xirx_GCi~Z0FsvEJj^U_LMVeTG_D$#S?u&T_UiC4r zaPqAgdP}+kN>|8cZC49x)`(ge=TuOpPU7`x!i@gqZW~f8#Gw-WPro0EMtf|EzbeE| znNvE~vT$f?5eGnG<<++r>GP5vHB#QUddp@{hk8VnBDd^r;A!RM?;uy{J*uT8x;tsa z-jqo9;^GclXs@wDL};z#RV9A@y}6nqVAnusKo%@$3{DMXTr3WZc(cNt3>5-<%gOss zC-;pP71^F$QMjr)>rL#Mpl@rcP$2TKlU%88bk69}b@*DUECrbd?ktuYPyWgs(g>@D5`pFX!OpVd!!EE=-6W zgHr!=ayP1K7-^%M>o54x?=6rjv+zabwnzY{3W0k}I7e*WWYuxp?9{6M_qg!QxK492 zOH-Tu*=w|>kG~sVeY;|8fhL4^`S!L7iqX{<3=lx0Tts)@oQ5Z`gwW4d-088OIAp|* z1N$+*)s-1uf}WzqkHi*~HPozR28U8bi*?OJr{G^BbknogL0X_F{o9N&z9Tpa}>~Eu3U=C%gknwqIUmDJ(qW$zQ_MfE#U_Fg`UE zFP(U8or7suLLv9{*mRSA$%N)u^6)lq?)zeZvVndbx*~*)mald+bnUAmJh|p?R~GI< zYkk0z!Ff#0q+vb#`twFl&DtwAAUd-d-05yn?>5y$&;rfwsL28v6%|f@`hYOrPDj|H z-fUzeO!-Rp3z^dAE-#?xKOg3IxPG@#qSjVi!wLau|Lng6{PbV`nbd*;=)MIIzmIbX zkK^c|{d@mbkNu_~1?^w_4FJACf83`TQStB;4h36|DA7cy2qWQG)dqkc@0zy(k=Yye z0lJ(CL{N=ofWuy7VOS@Qy?YMM*neHOD(C``yB6C3$O!g__KSpI;rq~I|HmU(@K@7^ zOMiq}fPeop&aOj#E51ViReUXD-IUjg2qpQFum=!bv08)2W6!jM?4~Tq?%teuc8{}C z)#~+2lLNjj;GTk?rTibI=p7|^8jY?(h$mP?((Fcm^p{CkaE%|r77`zn{G{)Js;6um zuCAy%k@xt$bo(lQeGm2M4(Jwv`h&~FP zr5!`Aqw>0@%$d*{%NU0#wXn{IqXM3SQZu!2B5M1>7MP=Y98O~nr$+@g|+P2r@MMDInKd>sj<`lI%Ob1b^ z8c=Kec1`yA;xx87sITyY31O6q2if@*VAl_dIBUh!yO)dr1^oDJss?0*br`xu z1n{Gm16Y~@CDHsrR+eE<`-m9YFYrC2BM!JC$qcq@?9eZ(h6nXHnHnc8KsKATi94$o zBg^yaK9FlCA;4ZeQ%EL!>$fIAHh{P!o9%(^9z$J%ihjQe%P?f?I*@-Z--d?T>)c`R zl0eJ_u%JRoYOhm^EX0>E?&)L3vE4e_Tj0}YFMlWnKSspwUNS7>&_32Axxf}=r#{p4d~iw4SmzF@U# z<5j~vLQ1-IcoBiPTualb+_{hULr*UMBXwXon&(XF=zEwn9@M7$50u~D(M+@Jl+T#E zKXyk8(HOCC9lTpZ=2xef{SQ}t)MWXlPWau5jbC}2`=&BZ7zw`eV{b{jvQM5& zZ>FAC`7TE1^P$}z20ncJM8&bLS@aWIkiLqFoPX@pJpJlRPjBMDmz$8xn;FDL`L|W) zGTM*;iBeQjd;R@u2SAcT`s7T39a(iNkPAw27Ytu5ObWFGobU&3UFrm~RDdPt0cy{0 zESY2bHIdnZd9EO2hQhSCtjdft2s-`$LR)8wosg?nlszUz5(fTn&nItw_v?dviZ zHnqKdIVWr8Dyc6`?}Xe)=<0Ldj~@`7XlD<+sEtJ8&M zCx~-IAW7dwbXGm_to(|TQoBX$O0zd??1CIW8bpex5!a`;sXLcL?fSM#?pu1(!>NPZ z=}iSih=x>)FMCwvM)HdP(lY+j zAs5mOKlC>P7J3!<_f0`Zuoz%Z&=L`JS11eMseZyy(DjoR9w6gWCB2sm_~Q#k&zC4((?`x?o-kL;qm1aC8zqboYiQup4N$EfzHa zxQlx?`~;^JptfY@($LQekoEUK3CDs61_ZiVK&&Axa{QwV4MfC0*P-j-|5k^7Oy-{U zYv(2Z2UP3og%$`cJcYLRcb+2OGWKW)!w)Qc7OCFDM1UWA^@VHNeHIp)wHHg<=cMJi zb3PX*uSaX+=(evCMFiabQFyQD3umf*@FD}bnGCa>JTXKj#5LRU4yXVt^}ma{d^my@^Gev6MVS)@Tk zfTGidI?A>~!Z^6IVB3sgLYU{(-pEA7jg*mRZax9L$yn`n=A&VXn1XEu5d^IFfYb+~ zgWQ{n-1|953M?x9hStj3mp|I>-KAWma-pP&Skz;kF}^oOtolrG`sXbF8{GpxW5joa^}NH{@QvT7s#5NRd3J!w#TFPy({IE=uDPBrgZJVW=j6vot_mCbh%- z?8h1z`ld7B{vS_fd8=5zu_UYl%R3E*l+AppUVFsIxof|_tEA3!qy^CclGZ`**r4QbIs zSM&fmvD=`2L@8vZvsw<2zcp5b(Z@(Y2bP4Q^??%ApZZifXP5hVcV&i^#q)v-4Q{IU z_fJMz4fowb_OJREK4yJQ`x*8k=B{#aD>XYlG>b&k z3Sc`&V}-b73|Ox!^+BD+nMSv}?}>zd;rrexn})sbk!^&^}wL)msPHpEaD*_$9i zRE^`xp6G+o0NEnpP>(F~ZIK-b(ZOO9`LS=D)gon8_%GOT{h_5+Pjxkc=nNayEc-{O z^CgG^O0(!+2=vEZn9VH4aJ2Pl$I=CnF!rsYS;XqlnfwqC%>N-dFZBR>tPet$PH4Wl2CN`v*MD9tux)V%6II4W23{e}|_^ESHbL17vR}#`g1h zU(zn)LUg@*trdZiCx=^7JcAmZ73&=s1XH3Q=o7w@yczmDKyqb4)UXN3dFq-_Y}$Y2 zE?W$kEw@UV*x!q01BiJeOLPElPDjZCX>ck8ZIjb4IMZ*!5T3)un51@=aS4i$V?p|N zD^QKL(5?E)amW{hm0NhbubMdB{q79osBMZ}G1TrSRH6pz@BgW=CzFFrHsx9u}ZymPQ}lC&YlwPNSyLwNyuZVHm-Ym z#g}OX9&++A(Y1X37#G|4*dP=Tdtedg%@o#%wGKFP#n$^`&}KPk->0ST{J!tpxR?AO z>!|#fFjs z;AFSh;ar!*=j<5+^wyix;zS9KIB!cOCIs&0Nw+gd>Ek8s;s+$TUOWzX7-f>A1r2AV zIZD#5tC5;?mN-jNlq(rZ97Gyln zjmfhUoK?Oh@bPQ6^y3dToh=tzp0NH2H_v%diOJ>ipyoi*Q@@Nf~=4;5-*VXHV zS7K(&)nUmVOS2}c6N~9Co@AZSYrgIsiQIe6>nvs2(b$~|5>2u_QKZnE;iv^LS12O z$H|1(f|JA?1(T!Ns4I>BU{>(RArLNaeLR;0Uwd2(q?hYy5xUT_D6C{REK#bb?_cq~ zX~e0U4Ik`dxY_P|5TgM4!I-fh>RqI`!c;U;d(8vF-`z&TI4?*nU@RH)04b(xy*@ve z?qiXVe6^MAa}cc-nPVHrxMfr9{i%bMW)<4Z$Ok*=!&Dp*hRcP@b)5t#9@u=fMt>- zt7+Yuba#!}Qas1;LKSVA-knAonME4F+TgcXm?B}dlPZf>7Tf82=<|f%4QH=g*pW!y z>k`DD?(ZjA#ASU~%pgi>dG&p!cD}b1WITOUcHy|w35qZ=#tCOXO$ui5n8?eI9u z<>jP0*JVHOo)%I?8vxhAhE6D9VA4P`Hqd5Sf#RJc>tmUcUU3`ntov$9>TF;WfUXIN zfF@aQ{FPy{0=O+VX7Mr9vlCs!5ihR^vB%qU3#z-9lHWLy{qu0D1{Xt&E|I>aLl;}FLVQRV|#TQ zT>xXw6zzC1K3R4&U=W?}8KPZUaQme938C17Q!XfZ5{GK_T$3kL1bB8x_kNALfJA8B zKzw>4U2;Pf51W(n7MaxfsOQrp?>Q_p%t`6n2jAq4iWUr6ah?;hG;Wol$e=%n22&h# z(@}w`dg6Hy6H04$>O}fS(o==Z>;g??Fm zH7U&06T$#w+onJjN=cH;4(9J1sIO#!7M&_UYIQ=%d@>!a>XS@Hom>|2o()i>(Zm-jUF>$uMG3jORoyPE zrg)wH@W_!~o2-Nr=fI4n04;~;2+&3BhZOn@VJY|6oy?UPlYh45E;lXK7N3Ol$A8>C z1Tut6kQB4QXjc7DWNjQS>U0GbB@Sp-3ay1ds&|ClpC)u|C`s6fMg-|K+-N*rOFLWZ zIBj~IaiQi@qmxkX(JHjG>YOBf`}RGOGKfQqvld}w-quzAv~o4R!?}96tl?q?y=enr z`fyor3@Z)lx>Na&MJj}zgXSu4i-BX&HgL0S$tP9GHZ`O8B;NJ zzHFZw-l|deS%edp<%i*U)6`wC1KYr0K1A5|TTl)?KCv?AlHRKCL4+tG1Y8Gowst^f zX6Kz~f6mnwC;r-ej?N<2H~U>*cNgF_#LhgpY)`2kBH`_@FhB3!q40D3%(!9|i}KkW zuuI&9GorTb5d}5Q*XFm@b2n6pNlBFe3C!h8Mk^Bf^LsW>S(ZBvcmRm;xhVZhkXaf~ zrB>8zApX+TVtAbgJp}V3Dt3_>-OsL^4Hj# z_C!ydP!QLmxkb$tjMBc4)2eU24Cdqq)6?}V*(@!KZkM1b@S!7C9w_JG_a0X40O>(I zO&rEg38-%nJ{kQ!FaJT<5iA`*grY$7c9|pIbHj3%9ySTb{A6MRiKlmwcLcNLeSd3R z*+mBt+1Hfq%DNK70Q}E|Oe#bdqxks`zlQKSBiIe>Yzz0}nDUO84b#uA9M+1Rw(&3c zN8mP)RwNVli%=;#rvMoTY@T@uIG5oh{8(1qS?m2abJfT+0@!qky0?5a11a1cM1q8! z#MB)J3Cehf?`fVCQqPIKC{yK(sb~H&N?c<*zDBbta}vI?M|1)kmlL++tkpw&- zl-StFfk2!|V>HO2u0gGb6A+jBiQ48lr@K9CJ!t{)^Rr*8yKkRWx>e+dF;jdlw_kj? z;r80#eo$tofd31tC+k0*{l@Fo%$b4&o@~Ks*N2^)%WFv_HOBT}KQ$j;bQ;X&%i5&q z6An7`v*dp4_&WCRBFG~226LQsd8~0l4Ipxb05>L(x^{yCdM^|Mo+9bq$eEM?dv1&u zdO`~9yajDPI!ahDZl5gUv@!XT-dAIgvIQ$3489Y(uNykk!m^GMs* zUtQfj?H_-SbmICrQ&-;58fSw^Z4=rK;+iN#(8U;EMdByFYpjHas>eK=xn__&PEz%y zApUkN6IW_<=*buK1?K(tiG2l0ANIvgGwjTGS&OAVu}t*WvLyQ-&K>Kqr75rL=WUZd z6Qp7A%AX!EWu`Fdo672=lI%~syqW-;6>kypR)CiQ8AVPL=Gb<7wB`RJ?7hR9>b5mr z6cqshktQ_?C`wg&kEk>e5RhIXB3+2|0D%ZdZvp}W(t8&|ks3M((wl^u03w}G10?Y- zzkSYm&fe!f_wpa>Nm*-U&avJ(#_t_d&k;bC@NOm%{fy&ms!AM5%=M4N<6rglI~#j_ zc=R}^%8on)h=);nfDGE*aK4H=X1;aL0w)>++rmq(uutUeTS|CnimNbcu7L@x!GGI*#|M zinX}i$Yxc{Cg6BSc9KM@YwJeD<+fEHPfXo!{>D=sqTy&vyZ=qO2dln5G3A=DrcG!a_JlRFjZ zE`rk2BA%s!Z`-yH-g&dOb4-EThG)((kTCyiuqR!zt)U3T{35)6zhg>IJuEFH+u#;| z|HTR;%p)?}_X4wj6Y<=%z?1n^S|#zu(GckeYTsFr97r$Q0BZN?9c}RjFIu%lt3H1Y z0tMcxgzyRoTFFNFx!mgEvg=~?77^;TXDUQm3>4y=uV}zBNva(a}n#!x&(Ki z7E`+yO6rNMBubg-yZwELms0Tha&ZDN;n;r<+_QTV=GJ?^$lgaNz3%fF@8c6ayOCz$ zaSXF#&Sf2WakOp0|h#Ac^EtA>=9uveRfFV$cX-dUqhFHjj zjAOhycdxB;-JGi3ekO9o-jHQ4@95Xpp;Yr=0r_P>BhDh3=b<;12 z+zAaUiUPocaSkY3hn@Z7JIjOv#l?hHJOmI=O8;mzrN3Hj@qcMGpm7BsAsx`kbmx?@ z{}OBLsuQA^{DNA_gzReq2fs#7lVW|&v%^H@dJK?3c)F;B=eef;`^V7AJdJ)+{8^Q&E3F9e%s_jQ=fr?r8|bZn@$e ztTN4=)3!qw{ubwtl90v9Lm094WqA4%g=*&Pt zL5z2Wu+>yb#JRnn6ly`YM<;0Y%O{Jo+F!A`f5@!m1{q%LNp(5y($pgD zPPiUUzWM5CMELgn?p4n%Gn&$vpo`Vur3^M`mz>B40gq*xr*pr(rjno-m}dX?W8YWR zQ1dKnnfvXBl`_q^_n|Lwmv=*L@RQlySkA}GI^Vt`kz=hxe4ojFF8uh}QM5T?0OD--Y{oIz7KcxDlK00pHFZeX|F^=#c#oslkZpU7FUcpOX^QSr37u14C*&x@% zhkBj+J4!9EOwgc0%5uhH!PhvcexYr;BM5Yio8v8>kzt>@)C(6Ev~m z77AvwTEfhnMcnnGG+S3$&89!uDc-|zCU=499V^QS)*%We0^%_{3*#D6UW(6T z={3?-S~?K`E@erBFr2L@t5k+2buHYk!-+5WSJ4F>JGk!B{W81y^4V-Ch49T4k38bX zLujNAIOvEMk1XYUjP=g)<4x|`kFYav9W zI4WEg)f0PhhH!5pCKJn(vr&eO{1kL@d;28%$JI4~Ztu{}KV*!s*r&4VX(spqTVj5N z*<#_bM?n(&D8czo=8ttTr_(_Amup_RFLA12JN|d=7H*q6E{n(6pZU!r7c}hiiFFM3 zqdt6~oBup(x!9DM=(13Jhuj6Hlhi zlLZmk{Hav<6F*$1NzJ&2oRhL3RPZp4F~0p)oYdRfRdGL0DnyJXj%ZYmV;XUAf2Jw- zO$Z~tSN%IG;z8A;de4g76}LKV(d*!FV+j5kVN!dZuEMflELyF8BevdOkRSl);3#=x zFBtk)t+j2bQD>o}C$rd7p`%E<6|d{Vt&$#~mPC!GxCY~^^Bv2h>s1$vYeQ(Yl+MOW ztz_ey{!2~XBmX6WPB?bg)G%wp6@F-zq6kx*al7-YIBW2p@wKw5;`lDhmtsyr+)b8Y zm*1%mhnW!W`?grtXo$S$y5;m*-g|1?p7i`dP?VS%^7Oglo1vc97v$lEpzKd?mRXBk zp0AV!6&jcg=~~1)?#Wvn>*87R31UbHy#t$H3o1gS+=R7!+gX0VTvF0+@EV`poatNT zxi!H|VU~+684>(3McvB0b-<|x1iK(;NL&LL5aVX*CTy|GXC>oyinB=8#B!BU)F$JD zveIK;yA-vEtK;l-OE+Ft&70>lUchA5SEIpOGnJ}EiUWKT?xXImD%_-zAn(4lonNgY zxSqnj1-e_}wMONI^~R96YldPWsSLK4m~pMdCFFbhfIgY_O^73+U73EB_z|wd z=wQ5(+ODsvt9eL9bxN)|4Y&5HfJ?OEK<_gzOR?*J52P55Mb-ohU;@xs^d zuS{~pYrPZuVknz_CUG)Lh$bPQ(buZqJ43E#&KSPj%Ru5nu_3uj-r}d?_bU!v9MJHK zHz}E;Kn|D9;231D*|f|#?Kfyvn8+R7RvatiZe$BOEnf|2a0^|@ za5GD4S^g>Pf7?+}Xyi415R+27>bd$sO*Qq0TMZXcy}}j~8%|KgEK=8{ zD_6iveb<8J1;5f9&ZV(2`EBd)fUIlCA;YeVYQ6a5A2uT+1?gMdb_RU1`3nI0}S6$ zBEK-tqhmRM$mr9Pj~NK7t&FO-QlGMFiQC;4|9aeC)z|9OI&`fQMg|@Td3zAyIq6UX zghl{V@dXhm5h;gjfo09s1*AbyW4KbYRK}gqr>$&JzpRse=^sMUVc^?Jz4_k51OO9I z#Um}3e(yHBl}b#9h`0+d?+rFIsrwIeiC*bu)t>*vab|d-41ZwJG+HUd(zVEG?HFm# zO+Rd%d+Ka_6@BiFb_7$D0zC(3_W0-8=R;_6BNaj#YN|^=Km0n zGMOMrI4&}{gDyIL^d@o1zM*$`*TZA9FthZ{{*vpBtA-0vH}^mDds&-_}pZt_j>C()sH-p6k0Au zxjwMSBv00vA;Y;uvG&YbTU+dDQVqd&e5-2O$uwmmab`u49}t+c|4VWHRg4L3cuPQW zf{B%veJCPOD<|@Qi@X1Ym$VUH|IaH#fL8$3+kXV-r@w-82oAh4UKN{peqrIi1t(%( z6VOXv6Bqv_I4KD65JsRB{J#_@0eBA-P@I6AJj~v-BY+-kctWaT2?>bnh@acQ(Z6nl z+Lix%=pXh^iFma>1-S^VVf*NATGm#OVZ4GR8!nr7i)GK`)UR zKn#|clK+sgSLWO+uJ*geuamE{@J{nIZy3WXp*pb_S>tmB^8^2zXy_BUPs3bXw~ zCcF#)hn4#e|Kl9~;~RdHZozd&6d7S51DK&?zZ}161&LZ2J{&4$1*shV%lavcAve}O z-^Tc=u4GA}Ad347Cmq%{v=(@RT5#a=!HkG1p1=<#lT?4+Pl|C3SE7)*EY~6AdnQ{OYm- zL~~E!j35wr&z~F3JfcUs@zUPv8$cr2ac_BFc>TU7K{{SCNi(eBgPga6YeoB!Aee9# zJBRA)1>h1r(4p~PFy*}ggy0OAV;=(c+XMh8xeP>%p3SLN8vGQ3?RR5d^4Vj1*@ED* zx!-4rd4`z|!8XC6gN#it8b#%usktA_2j()h6V%3?p68SlKvDZquLj-P0_s4{^2IoWycgC=NFmWkloCXRQw~cmQ z`)S@`)RP1hzZ#tfq7t*kNnr7b5RlidN zk%HB6LA7+0EZ2_)2#@dx41{%%ap6{7s~g7+Q|X$?in63%EbVr-M!ZV$wjkFR0|PDx zl1k-6cNBT>b)8+H>mCBLP?`Zu`V|51n&Q6UV zHrtZNH-P>ec6+KtMymEj-afV6U<|Y_J|GfHRR`3-S=>d*_>`?$C zy73o*afNWmU$(|cVSoS4ZZ#c@8rvhWRm>l^AvNB{@>AKbsmLxf~kY7$cj)>sf1oG+-0 zi@$DMsMfM*amK8-fKqXYre*FcO@^hQD-Q)9tNfBXCy}G{tDhci=H9wQHbWy!++3FWYQZP6eO?`)&kDzz;7izAyPOmt5%pIV{I5h5IWCfw#9z|*=8&~pi_Ce5b!bmA2Rfwp)%*_Bq$hU{*QA)4u>GAi$s98D7}GX zC)Yo&2!9|0DiurWFhrcaI68!O@1F9K?g0|v&KfDq=X97v2@G0Z40!7fFeUQoOjVao zL-neNQHYf^d=RPE2{;Ngs57tR{Y_PDb>?zaUZtguBR zTq4rjIz}Rp_oEYi-v?Vak70-XVLSxE30~X9)RU+Ip>fLnm(}Wq{KIM?k&DGg^Dc`F z&D?M=$E+?7dP`yuR)EHln@(!@Tt1CZ+1%viK0l;D17DNs-HnOs*k`W{S#wATC^SC^ z06dvYu^0eiT6X^W6Tla@Kse#5CgM(}$BAv|4}I)%Neo>J*BEDiVN6H=r;YpT?B<*K zB-OE2(LIX>zV17?`XG5#t6YbJb^Ck=`VAAkMf7}IynK~ZQbOARw|M;fj^|7t0}H7P zP~ojNd&}{uoZ4NfE2(*UFj*iQ$QDK8P$hvi|4#m0@^>F)0OgBp?L`W&f#D$*P|bAD z&)RkIeMJz9=CiRMed6ck`33dg5=}{piW^}!L7pa)k;fnKD+_!<&lun`2Y1IM*hl$} zJr+jlMp%ut&r}yI+fFvsCj&L>oUSQTzAU^f^roVD^pG!5o^RPn;AyP-G~Cy|WL?t7 z&BR1dmSv_Q-#m?eCtpnMjsWh~;JWhVns; z<=tKlD2HPz!#rHdz9zZO*w$QjcOZGNF2)i`X`+Lp<172c|qI5;Mw~Yd?qZ85n=#T#N@CHF{Jy64v)I1x(iOgs z{)uKd=V|HX&z5`=S`rdI9za0a?7weDwng#CoKkQlG`p05?U;gA~NvhVV)?z#k8@& zY#h&OWfX+|XgEr$YJ&}{Zk4nD*8QF>;+Bf~y%XxDx7FKL3GW}c#E6+`z#Kb&G?_mA z$}daq{Eehv-0Q#}lRJsBMnp-0>5iH}6tD=a28zld*%9if&^4PkS{V1W&^NZIGej=3niKpn5G|!@RZ;StDJVz6ZPuZSYtS;rP zQPoQ3eYYAGsTWwi1KQ~Mg5kP4^uX=*C=MZcbK zST%s6;Ylbik}Na%Hi8Bi=p(WKA0011(U}QM%|WfRg@xODXLmFD9Y2oQpBPbdZcVLf z3CkvheFf6a!6NotSRiCfd{|?hSM@t2(B;XqU5% zKYVaf)HCJ2FD&)y;)0*Yxf&cAs)@TiiU^Hr6xvB};*XZy6=U!9P|-e&3ICcjT-0rE zOz^|6OGP!y

RTx=1#Z!Ib(&-=p)* z%E}2EHHSVdoh^?SE%vzbbMR6+qgk~$zlDaf?&8hCA`5Pff;m&cg8{)YW=|`str&H( zn= zs(;j*7Mx*(;%x4%hH%X;6?8i}RG3N8(OZS1_|(T$Y=?_~Ld3m=*>q6uY3o!)0+)Gk z<0{?2!dPNrVc+ zse27$p^`{L3)?FHK2Hn&5faxN4AHGfJ?YLmu0J8NoUyUdZ&#?1HnXOzjhs_!PvkY9 zO;qt=-n2kMSYhDf82n;6%%ayIfW+C(xx%d+0fbl|TA1{=&2H04upK1EQ`UTN6R6E9 z*9ENJYxGz!;!g-5#Z5<4z!u%&y_0BOx_B_{I-^$nf|UWjj`C;##Xu0;8xDt#-P}2f zW*a-h+}5osi|>ek@Iw8zYcDHb%+ZKGXXr=#zpD*U58Yq20Xe1zpD9vp`QM(KLzB>4 zH{z#m{)%g6Dyf*^$ZG#qvvZ+R5ufH#I&cmlJd`r=YX;-Q$+o}LfB)5)#LDT)Xf=0c zaG2Q~Q$)CT&JS3}B4lqa4l>@t zu0LeQmzw}{j0i*q@IQf738smFdP!d-eNu1&1^wdG>O2P4uJKST-9IRkw5COqh4T5$ zzp4jEL5xA~lD^z6U!>S*y$7n1ifEQ9bBtTG|7a)cnO``o{Q9F%cIxJfu!=2Y!;BLx zb}+C#qJ+E#sFeY|81|re{>d(SI|~}GqzCfl_9|=Sf=*Use(FnxAM6m+@*r!RZwFer zwIvr;|2$G}WNOFJj>WovVZh~=X}?wCr%(G0e7|nLm!`UKCUty(6Dkan*V|j`mJJOq zBWT+f43w!$Cc=JKX`$XcQWuU9ZJ1Nk>yJryY-?s!Oeypqwlf5?{;j{?ByNxIc!wW3CT}U4e~-ADD%vT(E!gGf*nkJh)O_kVJyLTG zh{%_bfQ1m!Svo4u4yRFvsZ?e|=ZD0L{k^x(HQ+3G%`DPztLRh>G!)GllWUo)Em1!4 zfSy)59{RPvl6DK%KwvLx@2b{1KM9_A-(7K z&L8-;1-SZv%IqE#qgA~iOPbuf67Ee*UyfGk86SL~$-Mv;c%v~))f7X~A`+oU1HWHS zEP%6TwF6hBAbj3=R^C0C(*|$I58wXa+^JybYeif(M{V_{FVe>nUz!`9KG9gFML41^ zECc(PIsPGgQd8mzpN;76jm1Md7B3ToaG-8kikQ|L^EY=1Z-nH*FcN3_mM&%dRZO(A z^rHfZSi^=U4DdQM-^UPnj?oia;=DR?u+7kzD+OfMi}A(cp7?OdPg7& z|7pYPXoA8Xac~0Hd5KGMlsC!Pj9(>ZTiu55J=vvrAH!YUv0;cN`T zw!X`Fyk8dgto1ENX@wF8eYc}}-OkfPu=cI^hd0ePzhsE*>P|-y9PpJMIdJ2INA(TJ zJyKf!`tkHxL8&)pVB3HUslgv?Q)jnaqi!8#BHqvxZ^=bQ}{_oCV@*vQ(BIAeNi9gdZG)U(1H_tEtUxf$wsDn>^3Z`H04KfRt*n`bZE zlQG5CnrKyIB6IEdBIyHC*=I~qnM|E~_A?}bP z{vR)iSGm&%{Ytjr$a5*e{T6&7){#?ppOt&hInDEKOT+7tx-1-FRQ2u8)A2!_mWicH z)|VI{Ub9tp`e~)tQe%4hfj@RRIXPcnM|b+pOAq-~Z(Mj^l4bdbldQOn>=o5hEqEV* zW?uumd_efK_Jg+R!d39;*?74wv{qhd*L4de&bqJlNKKYwi;Q?Jv!>>t;f36~o(TJ_d%8fvg2SyIEPVJhrURx8l-rCl=L{0blE8>PS z%SdynbX`vRR-;=YmExQx>u)~w)w$H>mqjO2QD44vP-Dv}AftXzG^-(n(6I9ukIw9es*zP+qcZDP`@F~sef?a^k1lPBiB0DZ3G8_N zN`gr8izxzm4rHDNq5ob}{!)E1=hkRXiw{44(ecH`*qO?%R8XX%G+b}GS)$}gv3;_k z&x4SSYg-@7Q&f5$$&1E~*V_5_f%a7azN0GHZ% z3^Tbi{a#$%eOtI!#h2aS{i)}+pjgVYDBQP_ghNIYhd(f@$Kc9)zi7nT*zS~>R7NdT zx2k2`GHaWfzGmg`t=m_)$23cjp&+s4Vq-xEbR*zHQ26?rn{w-AtlAP5E})$ z(#p6F&mJd>vN_X*LyEed))XrcuSRag+d=%#54y70oTcZ{X+)Y{|7G8 z0lFzYo9+Oevk9XP=Uw!FoYs@d1O&Li*jOsUO~7QC&@+ag#t2QcvY47bJS>gWrqgz_ zc7PY0D;*fFK5tU}k^%I9tyv55zxf+dvr~nMjMRW$v(v%*L4_>3M4cL$A$q6EMRef9x7VE9dJ>}*0e7Z)ye`?7BZ3}RS6DLx}Us}{ep zC`X+Kv+Sb?GSO|$JX(2P16D@Z%;aj%?6zT5s22C4?N?&YJ!6_m@J-!pNYme~*ITEo z_A+wZD?`xac*D*N-m%ov+R~hAN~>|zFM)CeZ|YSnIflp|djRZ?9M+fz7s;i|EMHCR-7$K9TJY(q6U&VITo$J8v+r4 zK9Yr;Q!XIZpG;N##g2gC^KaPE_=m}#>kwLbkwUo2H{4+T@#673dMm>iu8a^A;d&#$ zTJIgDXp)kDFHJJr_2vBvx&I@>j9UHy#Z8E zvh>aWUL+I~&pmrND!#`4y0Dkxo!3T*6rK+xcjPB6y$>Mu1t-GNjW6KHcykZ`qBGTF z&CF+wdvskI=~wz@>|ODlFC7J}Y@Fbqqpoj5c;8#Kk#1a35CGbe1VZtMUK8z(^Ky-D zDJjw4XDuyX7C6;C=oGBp3pNF+emms>;Djp(suc;;4cNuqms3X?G%U-%k`)`g6pkia z(|v@>Gq6erK&r`#f{IQrAfgiwDS`Q_;=OX#O(sB~@*{RR-iQq$c3>Nmc%_L)8FuK5 zpo@t${?YV+9(e?n=d?5A{m)P*L19Nhn5hs5h zSb{-g9sW4gGn*#<=b&SW=;E`{-0;f-rzq*m$pb4(%nEob zaRe=BoMf$hEvga-MuOA!FFx`967{~t{z&_)pBoJntGcU-g4-Y`YZiYxntsKRf4^I7 zzf>Fg!u#$)N}BxghwA%;rnfYcLgDYp*g$BCPf90?wAqYtK8m15Cp1_U?1R%m=dzmw||@QSel_Q)r+sFy*v$k112jc zZS~QAOlj4 z2&r72^n5(-?JVk44_|ZZ92y@%c-EkthiUPEg&M>xPm@QGXlSgPj)^Pot2T82zee1* zScDyTiEvc1{%QuwojbW%>$Vo8sXF`#-E8e_S`&DyTqpNHXwjzk8}ax2yLVpoGN<0+ zjTQM>nz$#*Q?Kgr$7u76bT68T@r|hIb-@rZwsyX>{hr5lxVU(Rpoq#^SO12P+Lk>3 zLtn4$rMitg&>-qn4#OfH99l-LJ{fmA0DPHL5K zvAhARY``rJDQuP;D5%Yx$q{S&_8?J@+34_=t7^HgEL`IQsY}Vl!FRFSTaBEizzgs`o3qAnnGh zvwTfOiguYvL#f$-OY@G7)JANBB0qtDq}dvqRysB1u_>ou($6VpT2&o?rylst;^v-6 z;!`I4kM)HZR94^ccu~iy6m+p#q1%EI+W(SBZ2yo*ffxT@8X?lr5L;UNGm#ceck@{t znI_!F-6i9%LD1Ks9IpOmhre2-a0Rs(HLw#M9Uj@q=9~{*`gX*5=P-bn{H|Omzu5qn zd6y3&`bfU|$_2hCTkh5&U`~ar9FY3_`2rs{tY&IIW!(TU9)#>EgGL%Q(vj;zD~|31 zwiO!chVIQTuRVN#$a&m9$W9n+RJn(eS-LQu+V5hv;q`lZ5xOq{>u1bKG%i|fs_Pv` zAi-hcyNm=Ue13&Jy@jf+n^*4Dsr`NpW0hIaLU-MJ)~i~gGB*`yTv1<^vTztJq@l-xXzIML0K`$Z0ClyQ3!A7<~d)bmBdOLT+*px|M zEzjSL`g3>NaxA=jEq+H=QB7xJt@x9q5ffu4xrV?B7PL8;>4|UN$*L;Hk+P1Ii@BDHR}WW1pwp7QoC$6 z?R4qf-Bjc4?2S0S0gfQLfCJyuO_cK>yD1J7LZHtEOpl}=lSg!{N;J{CU-pO@PaNGs z7HK@f*$2S^_V_lyIC@jaTlxTY#UtJva?%d-PL^)=dLD zHC=1EP3wyQ&YL=aBivhRww0&6^V8K0cnH6IitF-?YfH{IPm7OCwm|65?BGRJfGkv< z$e;wW$zcScbMfyO;EA~bR8P((W5VI}$%}UJ2!*hefW=$1KMi#Hwqp|4fNgvJkdedA zJN@ZmyA=6whQ|frm39WYc6A_V2K4J~_Ks6mvN%Fi?p3vAPJtgsntDc!V>{(mdnet| z2W_~@bex(xm$4fAxb2VTeYvQJ;-%SisOnx!a^4&sfelFQ8M?-^nbWb zU&wJ+>&j)#8lRoN=Un-(bIzKBvG*?AA=Wo*V(F;yh36v5BOcMr<_^8v2c5@<=Yrbx zZya>BdqsP(6CLKHu~Qh7 zzEVWJ#W%cl2FGdpx%BC8#o^mJD~d+eZ8GKq3#BDqKY=DQk|c4&9u24X0kcF3hJxvDs>06qp)<2u~%Hnm>~%u##tO>j*q zbY44u5GqaIx*pb(X) zK+y|pV^yRFjwHn?!1lMuP_uX`oquClN!FD*T9-XBklcB1&RM74^Y>gL&t+rP7ixuc z9zj38>W&)3F2vTgUJe5-(Y@C!0i80r?>-2Yub;^s^y)0cm}zfK<%4ZAFUFoFx$i4x zTE{dw%<(_l!tDYb4|eciQdf&1kgakOi20kX;xpgNB(-XjV(Ke;dpP`OGw6nils=Vz zH#OnJxskL`7%&kIB631ab@8J${)~Rs%8UlqH$hB53z;N*GO2C)AN?kR=lerONT~gr z=2`>NTz>30Qv z5tkf#n2vZ{6GH-1!VC0GWe|D-g~3lHL*wzH^wEKPBe33 zg;ce1oWEkR8qnHJ1e!8OIe$h;{EA)P_+Z(f=VMe-1m6L*%1Yf7YgUEnmcwQyGIVP_ z#~#ROOu;3fg(l%QcwD@&0za)5w&+>h%q}F^=#w|xqSZ75=l~NuuyA|)G!rj}0!wD->`)l{5FQ>k`y1$~*AAiN=OW-97Dm?qSAijq^ z9L{5#yNN@KmUpgO2sSicPZ}EeC6`e1g~PUj#s$I1wQ*vGI}WmWn+m!PHysf_uR%q% zl*K7ThtG&!ycUPda~Rz*U;5NXlX{E_@qdVu&txQM0&~7c@Avey&z=5%F6s0qDvB z3|fMJ{U0*>VP=Qc>-blVA>nQcEvvsO3x86U3zDWi81?2=IVz!7rVk2?W@cWLUNlU6 z`|1=(`bGE)*!>}+C1_#=AZ)M@*d3g6bOG%3W;YjmU2VeF*c@8<-9B2Vr8$o!_02%o zcm2zf95R=3W+og_NUyE!9D5eF3M3}YPqt%Jeju`GnBJoEngDUl^VM9y9;pF5L z937oE=;5MJVwO@t^F!C>&H?@7-qnP<=T)bk0ajC=<6V76&0^ai%oh0FrQwif%Odm> zNAI%rDziD|EAXrpc#p!|7T+CL^>&Vo+UXZ+|CG$0$G=oT_kOX>&KJEErDnRSKzolf zMb%=uMuDa**<{L{C{Zg=Q(eW9tWMeZ4bgDTH*AsT6~>TvIlzSJfNoo}$etvPPprh7 zJ;740?1hFHWmh}zbkPD8v!SVc;$_UqsD$ z+V+)`?RO>+Gb1gMUT-yw*Dn9L^rdYN%eeL)C7#r&%0$xT&5Zl?t(n$Q| zt$26g9))!-2AF0*ioX75=Bw{NkV)4#=xD|~O^~B?nU3(dwDlJ#dQEt;mmJ#cfnl$t z#+Pcbg(SeN#AaDDgjPA&txezr z%96OVW45>Pyf9!prY|qpf$d-s7{jge3Vg-#^U+pXe9rl^+DYp%Tii9a^iCsH59X&4 zhTEBT{MF>FA0Jy?qnxi{=3FF*PPb*A-QWd^AM=06fP71Zl<8En6b2G8g$8mupq8F0 zEQqO_C0voKCI=&|F~C1R@0Y5y_ERl8!{!rHG@5GN4uZ)o7J#L1Jvk!PLKW-EGbhyjnKLony4x)CDT_gyY z>*vefVnW`DD5a- z{=MhdgQ1(Ymu$=5Nm;P(%@XK{S#U$iWT;DpP;W%}_7Uas!QHyht$mw&#>my%f?dIF zuhnJBM9^OPBbDNVR?ns=f)1OAJZ*K=zYp-CTA42Y`?-_@Yc@Wy!|6swKCB z+l;1fxw2k-_*HQpQ|%G&k~J%8wjEB zm+sM4UV_X3hM1EuHoYP^4UR9AM{N?DtdFBT`ZoNAx$#36eY;%X(XxcW!dl1l0vPuu zlfFUP^k-L&BqLpVae;#=g36YJ|om!cN~8Z$Coa0f4{@Gi?Vs$B~g zV><&Xf2Ema8r9ZJUfSS}VQUjj3b?YFD3R)^Rj6L_Rsd$2&CZvaU#GdUep2;pW(fx;r0VDw+Vl4M^Nlo>Dh$%0I z+ik>l#FhcM=pVThSfB|rAk7g!xqt}&wB;ZB+TQ^g?+MfKwa>|c zvHCl?@(XZpGlABv+7!fK5ir0T8~x>tJy8F~8+SMy{cRZ4OHxOLfww$?oz^fCKHINP zWJ=)AdvGTa1cjwosyTdew@IFlbAnADa$YTFBY2K`?|s#PXbgqVK{4=bLA_^qyf=uW z>QP(Kt%xn0XL<#@6-38fN?Kgpck~%1esD(-rA?>%GOtVQGIZl;m(|83!pfX5RS~XS zgLegg?9lN6Z=z|M;m|Lgh_=%$WSmRzX>x+{&7J9WfEiG4{*cjZPBjjtEqlcuXa{T` z9#06@`mT&GzsrtC%SEp{QfC8SJ(DUiqo!9Lg|bcV?3U#oA5Hs(uT>4r---$ou#$UQ}zV z?j|jr$!MA$?nmpbNr^(-n=1zezZyu zBx;w`YDfGbtM)I>+nDkq@mHmaFwRpCLfmEFiw$!C&|y)#7(-4w^Hf-|Za@foYB!EA z24y|3idksF$ccngI@)8Y2kbpION%@>0f%bfjl#b8jV1Z4>j4*RoWhK5=_-N5R7IkA zMhOb}l@XTbBEi9JW69^j^4-83Dw}gl9`ZfhHV`}lahTe1-5JfsnOz+2F0D6wlCSc- zvwy!;?@M9d!``V(J;;KuX3MPC<~pBOVLPPRk;P0q7A(T3?)y#)YR7TatU&y+y?J4TCVUy|(9s-H!mo*W; zwg(Fr2hKn*(1b_CxZ8U@4O!}dps6!~#m_iN7@`^`cXpg#>GQ|2hu1%^DzZVBEZw)c zb+K0x?u1i4*1_2z@0OOqewMFx7u7T+Yk!+8Q=BCm;{L{rx%68~lKD~Fl~>YMnMsIR z0-*DrSffZ(Rse9j?oYodJ5+94dKc-F>}Iq9vQ^i;94WAx>o>a7gZz}w+M`SwSZLdsP1Vw7r2Qd#7(ECXE z_8&58wvk_gE|#9Yz)A6!(`|$I7)>96F;>R!fW*77lm+Ujzg2Q?I85;t9?^B8cnfGC z3uPkCzyF%5GRE!kAXJ#~zBGl4{85=BH83B4pE)Q0osZLM|2-e$)Oa6jZGCyb!By)s z!xg&QV7XIzG!501Akfu{gJKa#?a-rg=Ur7?cY00Z_Q`Qf*0v*jTcMLX+pSfs#wnUi zC-?&FVnbvsh}7q5=kt~x);yzbT{OR7YPNpj(~{nG!(2}N9Zv>lwVF?BObLH%fVx0?D$LtX$ToN^94mwFAYquVylJ5)&|+ zqrQd8#6+&&t5=@HpAOJ1s<$s^I`|nwT7D%rmXDhg-N$i?d0%lSTi#Gi?cQo~*xQV;~vh#1h{x;j~Zb3uLLfL|7U_PCyqXa)kn*8RT zM_dbMwqD+B>b%}ulHQ#2^E025Y+kGLCO~fEy)5tn*&FDB!UBBbf?T>yw;+_YChW7k z0E+4Y_&#yjRpwXq;RMb+xFXWY5erKK)6LD!tuL*I!5ePNM8%u_>doHNFu#1h`DEU3 zvtl9D5f5_HK7T>DB>Q3T?@v9P-tt!XV4T#T13Wd9_83SJ5}712b3hQ5;0tvo30{8K za*Zx8MZthtPSfw_W_Px+pa;T_d9{XhR8BQ4@-jfM zj(e+!l;bL(wb8{32{^7{a?%%PV6`>r6BAVYbJK(~37J1vJ{Xhh#5q~EarH(wcZdqv zdzF_#eZvx`lNfVbIrCg2oW!mGj*BXutV$puY6I70*EG_LBm< zh*rf>6=<2gpDFt3YFd)-JONu`j%2eR7DWf+>qu6-gEJ`PO3R!1#PH9vcwPtg$Y?c1 zmAu7!ksv|QrhRsA3W2`>1caWF`fVbxNg1*NF-=MVk`mNDr()QG1}wYCO?$LKWQnR3 zXLsIs|F8q{O=%Ixw{Ri*d%?$2tS!~V?+zB%)}2{+%F)_?d}dMo-#bu|&5r2kL1NhR zgBN@~_e-&v+Xzlz4LSob3xS`3uTEB*g8LT0s;9e>{DtZ@Rr=A6c5h^#zb)m+eiE3s zv}U(f`x5G>GpV{MznMWPa$gGXTKVH#Rs@Y$x>x%F`0Q%TG|9}l1+kfE)bRHkCG&ZJ zqcy9B<-j){@0VgUs3E{yL^~%^gy))?-^H>=D$6?Dh-D0X%DeNu^FsTk$=Oi-^0r|Q zO9o#H&Z%j`_Y@;B4A>$> zH5}|C;*LjE=gxDq^|?Z1EK?vsy^52LosB15G<1M{SV5|L3OBdOeBTN-nD?|Dqq}nN z5=MJyF2=iXV*Dgy)UF&DQ~4;pt)F0fAQtAxLGj%5NhIKRhn ze2?RxUIgaWJ78<=1Ls|YDd9(X(JT-2bala5KXTQ??hekq?1syv+8I~QA^`6`iI?2B z5Ffk0I-=EMWHf}xFJIn7XH+od+$oqgGW`}>8EsUFM4=uMovyft6 z1s<}2Z^~h40Y9xE8}R*3dwetGe?!FbIKj#=k4Drzy+w?G*KUuac+5`_ZAr+XR?6*q z0?}iV!-$0xoZ8#iA$i_9gA|3;5mvgHF_U zOKvQI90zBC29NJD0ZLI5+ceN8r5&0N1-ni}ho4-z@}EIkm%nr6G`6{+Px60J<=_wS z`enp~#t4`maeu*^?1V8h?}f?UMY_naEyG7&*hH^K{1)3f%wniw`I!CbE>EtWg3-6b zSo53yz_zbs_sM*~OWD|3Li?pbZmSJ{{y1|e4}B;~j+nkfbCr0r;78#sa@Ay99Si+tp}0S zIB7%V@Y>0Qb@P6~@@4W&sl=HJpI;7fVe7*2$u`_A_V2d=diaz|Vy4!0Lo>hib`IxL zb-%1o8%#B*$&RxMVV~MsT9B;n%&a75yxMxo*hlx5l3$`61s%n*&Td+p{J9Y8Glt?- zltO5Hst7)R^F1Iw4zSntUC6Bz(w4zrO}?}5ZV(83_5S^~t3hE|^Yi`WLwJf=Ll4Pc zrc(TAR9WGxo~IAXI!vnGh8Ztf@9XEq&5+ zQQ`3i?v@K*q{B7=Jbp~RgM-3MPYaw)x*Y2eab~9AOLPxA!-??oAhCUKa!k1K0H$1n zoD`4+9lwo0ta-^ zdDDmBTw4;$&9nWSZ}3>#N!l3nB6t&UaL!jba9h$_+;m{gj9)~zj8K+nQ&sa(^Vkz! z?`KY4$BcyoKh0h$Y^bjh{E9+koGii>IP3PiZ*s>=%FLB~cpR2|mg!NRAi-y>s{B&V zYKrmtDT2rIa*ra@AwU<7I~1{3H+E}X%z|S@y{kU@vl*-U%g^`DC6Bt@@~Sn%9tBx5 z%ShJm_FggBc?x!NLuJWjavRTYvc-;#Sca=_hlkN8aEm@^Qn7p?75g0@a7@)lZXpYI zqWJ7)`)I)p>gG#hm~Bbt2;BfZ@t{W|<*hPz0e0}L?eQfdY}OMOAJ z&+!cu&`}o3ML$c|H=*=_G}P~sE0G9L@8%V-_cfF7-*4$VmwTgdZK_FUYsd^yiN<<(U1eVkw-e+imEz52vK> zs8e5afG`>2=fWN4kRq)WG-Prly2%8ur+Gr9?n3MS56*+2Uwh|Y&VY&EodHDj@Uqq` z$_o#(7wkMYWOdJI_I+abEIBI3s&E)DOYCfB=9%SbY0mF3YmS*7Qo_%05HgLtt`eo* z#728*(vGa~h5H(R2X#hmf;_NKpuMFC25;MMj8|tTzt}1dp9?!WKB6YR-^xg6LUp*n z`$S7ECNi$|Zc+n=In#1vE$;=@`bSoqj$@>DpUd?rPHn!f{;jN`VRC0G1}8#TqU@iC zmPi>?R)`6p5T_O@@Lz8oqOwT>+i{B{06>$`F=whq#xEmT9cSvd7UigczzTBT%^^Gp z4}qrTz|Mr4V1zi57xkehe1mNK6-h`70O0KFdjdH9=#N6nO0Zu>_`2s|x4mGvrocgBVIWRd zu3;-UsnEQGuiPskHZ$g>g#K~8P#zlF%s(8xPF;AE0d$=8YfSlgY{RzX?8cE(GH>mD zZ$2s1G`a)P(!_kVVeg}U4q)yh(-Jrv%$Wuf`${EF4rwmxW(I3MmFd1P)VN2T&y(E@ zq^Mu5Br=aW<>3&FyvOoJZklw^4_lwRj7rMQy8ZceBdR!c#6C)^+%E$Mg`1!6m5qv! z^DU1M>MHST=~}>PzFADc-8DHOxEQ^+a#EgaUYz$|#fJkwifqvum(s;yL%QYi=M2t~ zJF2CBU4kj}_51owc)YII*Gk}{)9A$fB{#qZ`I|ub17;utCmy04f&(!nxBz()0vO_# zp^h`n=>K_g6h+b^U)-Tlsf2A)#iu1ndKe&Z(4UX87D;%fF+pjxgOlO7ZLd1VPxU z_zY%Chac6KhuTY8aLX{aBig3Y20D$#9=GhD;`*ND7U;Z$G$5bsa#oZj6g4RG^_rdp z`Fmy;#UCExP!-lYe!lxsYg;lOlUSbCBA4%rmme#L*NnSv!CO}_!?PxWlmyw2I72&& zUYYWUo6&C?^vZYDT_Ey!z*hmq0dSy)`kvT`m|vCP8n#QgW=08K)M9DS;Rd9Gv#uAK z{L8luN#eIiAp|)i0y?rDp^F2h zQkMmW*Ybv8Db-B+X7|`*e$|>Ol7a$P(epnlA)6@LB8v8w z7d1}fR+by~po~uEVX_#ZB}A4Zv`EIE<4f~+eN%>%+8!ZC;lts1b+uy^JQ*JsePF); z2J$XoXe2oUz#s)pPm7lj<;bZ^kABuN2J_7B{(uRCEqkLGcWCA&5fuJ?&N}YyEjW#S z5NW%6ErfWHs#Wv^nm-DJHr=$wjq(5*M?>FDHvogeye6Ck+yg%=)*Y+>s8<)M5Ks$W zMs!jh7eT98b7};7gM3OjW<2@NwB+3!WaAiWGjjPVO66~y2XT>)9R_2dHJ!eoXr=|b zYs=o=Zt;X$pV+=szqAzAlS$%NlNHtw7901V4}vpz?D<(K;y>vK^uK*iEUS4TjC*)H zw=!y6)Ml7kEF7hDC`}Jipl{n|0+96xh|$uH2b=(l_Ty*K7tAgbgqwH?YPLWDy;~Ft zUv+2y28bUBzn}Tt04EsO-E?jQ2c#k*7?6F5BGkyBvYekCg=S|()8C6W{#+$IsOxv| zs!g02Zy&X1v4+<2SA^ssRjUMhW`r8if$GO{j#t| zh~JwDiSaI?(=t_@waQEq7NpNid|K{}JV(Ks8X|$uHu@HZ;)xZiYz5EX&*+NPM`PYwT1EoF<@v05>Sa#kTF(~4OO|VPF2H(;OBPLkRYjW_c;l|j z1C~o;h4TnCRhRL)fY&jb_PJhiPWIP5T;gwIRWD5l7)gKzoyT??5P(1WsVFkr_84a) znu*$L^0`kGlW+yv3lPlo3~(LXKIq*+3RsLS+w)*o9858V(i=NTn*u~TZ$Sl zK$}u&z&(@=eVVgHuOH|EVw-MC(SEIvM#xwOA^wJ?VZU1CgT0E#u@SEpg@A~x^y-lT zvPh`hvp2I3<|AibkUwWU@Ezzv60|YwfC+-3N2&RZVOZ_ACO-2J!^GAuZaQT~C_aho zv*U`E(yCq5_0u4;g;0M>Xu+;c&{N}8&1pg1N1#SyVPnuXR}?j_K}BQW0=sfz@$>jp zNok9_k~Q`W#o63+of8)f%99KIzA_G$-b>SqHlCEP_wQQG6N^r}^i6P4e)Kl;tDN5C z*uH;h(E421t}r9;s+?=1=e7RWoUSL-?r4GRW92JTuq>HdvOYZL32JnH$1i~`9*|S5 z)OSm}BG{mpB}PZhr2DS@kwa#WmYa`7-_h8(;kjjzp8rFzW$-J+67RXSjVE+m%e7Hb zvj*E)NV=5_BtWEkx72wl1X{@zUK|5mU_~0Dww5G;)K*HeLZ6mM2x_Av@(+d-(BUGX zgehtzptbK>8;iT?fLrfCI$+zm0AYYJ6$XGRVRr-CA#qnb01rC4{EzJq1RJ(|b!};T zAK>OpXBVZwZ^Ji_m{IFDFkAX-nE^Dj9`It||3Lp)#0Ufx+m_+~v?K2R3@QTgyTTz; znN=omrRCn;*ht?qemuSU+^(5t*XU(Ku5Ge--t^mZiE`>=M23F5f$93XD5pJptigsJ ztapo{Jwm%k#En%4rtjOubL#M^z*V+c-S45DFLtlGPsIl<#h63i!6kfs&DcO#_%7d( z?<^{Mq?j~5eV=r#lv~e^R2uX9PKFk|iXPAA-EOaYv!FcFKh2#TUC2xqw`I6PQ*H+N zW0t`7XRy)hFL?&#f4Bgu+q(T`yO{7sbBAWJ2A-m`KIgntP9Z?`(a53H*rTheRJKr; zr_-txh|TpR>SpFj+=n}3l=U5&8eX!P@~}hY#YpHo9CxYu8KMJ5I;BHTDV4b_y+b1y z0ncqASLq=0JT}2uuum`kB&AG7(Bq4$ zF;`#v8k@kG=ZsHXwG+PxYUclknxMV>zDcTs6MGf=gTF34Y=25hhl5$}8O{yo6)$yM z2u@%3^!=6^061(Uj+M2MI_ccj*5VG^|t-*dK+r&1Zq13zBYuU@b2>z01_QHx~unxe(L>W zL`f>*D6W&@kpPO<)|vJE?8CHX2vRuMEXe9WANYYRo-VAANi;B8=SC%Jy1x|n70Coe(H~culEzHU8|zj!NZDm+jaJ> zVJRRn_T!EBiMt)*L|;8f4+FfE+no1ovS<{-)Q%+xj`NuQ>_~d5FppXQ|B-KK3_|%v z=q0`D%EAWvsxGM^S;!C89QL)bSAT>QTr((rKM%z#Sx3avg>gcDns3qw>dlRTa$>>6OeT<`Rmtv>LaG%OOC-aaMwu*=E z6z{$KPCG*~R>ChFpKDCzsOpjM=+L=R_0r8T&#uSE%}kS)q1N4{^HlA_wK`Z5xgZ}Aa@ZJZ|+@K5pwdMTz&vUoyq!$3b9NFba{kuOOktaYslN5my z$l`p_Pk&M?SsO@v6A7TZTh;)ZyfwZx2H#Wy)+cb9N8|&ixo(Opk`)>!*6>3ETA0;) z?4?RSi`QJgrpEqKU9J(aDYCUh2?7m#4UE5_-%667Ab^w5Z;2un3*aHp8DL8R_5aqY zg&JnR*|WYM7zN;nu%VyTCkK8C7}TGH9DTZA!$f4tTDrmtRF^KAFId%o6L>0xvuBfFbM z#n1z>W=CB!eO-~WRkB0l3#x1!Phl&5aJP;>YPo>-sVyfWQY}=K|0Vpn-S=5(+k%T` z4xT+%B3PdfUWF8#;vMN;PipoRmvisXxL><3otr;&^T;RZP@%(n#%(Bgy-qd@yGwd| zp0`Y5#%w>Z&xMdEgp7J5mm)npSkd^NORyjB^fRAzY0FR{k>yj~ck0#z0v*__i7dTG z<~0QxgkV{!B{n(a_nmm{t;1x{nhJ#Z!=PfxmTx7F7C&@=_YL{Mp;FC4qSWF3{WF3h zkK#4Pm<&G@^#H>wRf$mEh@rhfYN;Da_6<(RJ$Y>)B(&v6q}ps`PEgL~?SVrey&6XF zYU&Kp_qJ^GRN*T26DRFX$MHg}6m+;qoll@Kz}Fkm1aZSHBqKP7l$uJshO9GKpm$e0 z*BQ+fS@;|8x)$bew+O{M^<9RmK=+DWVsSTY(5sgxjRqnxZ&(oX`h? z`}^sJVg#pYsMT8IU-x(;)j(%H+_vr^)t=F}J@<|u85*ih(ii5Zfeqv-CSoq7Kaj9P zF82_qrLK81fBch`+_&3DUX5;_P%K+?))PM3-fcG>-Q>1oZVckBZ1ctzudiO9dvITg z5b%t4e23;e)X2<1)*vikA{IT62cX*{{%MjjewrtBDEqCB#qvH4ei^I9em?X_1Cv}Z zJ=sbB>Att&(*~@=-B`Z*%j0;M|%E4QW;$HZ7<%{w9(7Oi9m7MO$879( zJ>l=*;j(}g!-Q|Ds3|YJ_1)eWxR!2LcLg-qZE%}@kXN-&Zk zfS<$zDuo2kHS(0DuX}Nbd%yZN>=WBA+vU#mra0}Y6s^?p#ly9{=+cCTOI65B(m6*m zkK!$>bW#PQj$v2lH^h*HQUC1u`VlQY#7aA?tz&8ELC=fgQFs{ZupRXb;)!vhJ^Fxm zB=$;zarc`X9i4AyI0oxESi7s71Qsu(czrl5=fvPWS6am9?WRrKxBk$@&F4&Bj-AVF zIfs$2V@EI6D%LoZpOtH{Ri_zmZx#GKCeoo{iOS(!vs8yX0>^O@ZU(S*ukjY_-Usz? z?DEjr0S-=+i1~?WxYd5wDGoC?6o7^TFjh-r*qF$wfGIn>N8(pCLLg*$3fGB;)O&`GMfGo(D>?o zKm83KB>^t-#sX=>qJM%agWo}w(r#X?CHXQqAL#!&Fb3B5!-y)Mz>)Iw=aKT1^biyu z<^Xo|?e?hOjb-4uIL?P&=c@S$yEMtidS@`do*m;*G5U8?W2q1vYrCe#r^P=`%qI+@ zMPigJrU>QAN7DvqPZtlLz`ASwLlH!#ktV= z#?HvpsT>b|+h{U}5hin43Orogmo@GeR`#Vy4L8Q_!JKVamS>;AmcW~x`M;r92%S*@ zdq|hOM-)}=hij~RG<5}!WLSP^I3%9$#H%I3ph9OI^wDIo*9dyFxONB_6Eg`zrBQ<( zhw??ODoefubczc~3P9g1G;pSDtn7b)J!3$K=ii}m=rF6rlqjbWq8gC&rIK9s&&8QK z-_G89e>0&>=lQ`sX045<;3Dw)78-`xn*C%a;!>k{fdw-=`7n|5L0b1bV=t%ad)PbQ91@udFM}^-<@3O| zHsJtIe~WN|l{kaGI5@=P8leI5LEO^94$YAp!$N~IsRr#+#j&!M@1(fb^Q?z5IfeuK z15;4ka)i;*iAt!Xk6s>V+V)d;$Qp5!G|&t?+jPe__1cX03-yTWC+fk=h*(4e*ye+|LC6C)yI&^vWv~nk1 zIbZhnWN#?_CHl2l#L1#-YI-#3foe3S-I$3S9L0XZ;Lm?_euqF+%ZQr%`had2|*F`kgwSqRf{{ zhQgQ&&R8~+tiPI=xJpSWfs4x|rPKp863<3w*!ZQQndfsGcC!eQ)mdpsIL2_An z?0*aZSUUlUcGV!j1_E-u@;&FOJIv#Y&N(j-$w<_BmrV4W%Hx?l;dw zlPts?$^|Q z7+b+=hpTOs&nR!g+scn&Ke$${M{-Y{=K%J%e|@VwKg2UcG+ww!K= zIv8R-?H%zkKNUvFsSz^wo^99u0Ft?WH23@2jmKS2y5f9t|E)^PJ5!NJ5k3IFbNbj9 zj&7Q+oGp$0NWH3uFH}1 zT6GMw-+sukxya5+ZgBOye!ba~vD-ycM-DL0Ann@rw6!Vh7t~AiV^=R#=yJ_oj4L-R z`qC0Z0yQrm{>_?3H{RKy;f?}f)fu4b@3HrL|KkHWz+6y-LeNXVM##8bFtZp*0^X$O zyWS)zWkIMlqyU&5=kY(y>6z`lASM|qLJ5|{tiIOXl5W8+A9p~mGkySeiX-qe*TI&L z|8X#!iYdg^^+>IwOduH=;oW$DOp4)a0h5UD2juRzJo2_^W~&O za$vVKe2^vspV0)v&V!_kX4w0}m>h3JLtrI*$)fq@+)SgW)vRQ2}&OV-L5_z-BV|J>B5B*ZXN!Q zw-xLLw&3%dqq-v3OA@=c8tS(ktNTjY%QUTYZuE5pPOI)Yu*Qyq6O0fCyo;J)`xGzQ z;3OICx7Io^PdH3ob`J$B)|cAP-QcA z$mpx%Y2eH%A<1$-E@(U7qBZg0tQP%?CYa!J6HY~myC?yGZjeLJdU3Zb?C>lV^u~Tr z#hYz*YYt$k@MwvNb`j9&%YZy1n4X0WU8Ah{&&$t?SQn@KXj?9j}PDF%M@k3)2?S@aw zGCF(4{HnKvUCKscTCt}pQRVvrL>5oUgk{wwtX0F(XuWQL#^bMo+Q8c z;x<(Cftcy4i<`@}(C^lw3wx~)tM@>Z$9|d=M?Gj%ksr_rv2tF`5lJa5fw zM{PrW6sF@#FFQC&VD!N@F>}rNa)vmq0W;mP!@!Ja^?JW9y=HkYckSpjtof3V2=7{i_5P`NLiQ0uOpv44Gs^=`pP(b|DePJ+9sd%v}|e*Pv<*ySO2||T`KR?-me6&c?mT*izpVAr zv1c*ig2yZUIq~pFMc{X%>jD`HlCqUa5Sv}@5Gt`W!{?~H7H-d_PZ5Qd(R+;f#)*m3 zaBB>MHyZdzf*iab)>q_&c$i6e&NcXwkIZ;+xN5jBYoDRBJuc)q*;?BoFHky#If&Ye zPePJT!Xr@P9-|AMSo5mh6VI{QNmt{~Od_QQC+>(>5hB8w?z{Z9k0{b8DWC>sk_S9c zPt%F1)vzbly#ivgk9xDr2Cn6PKXVvYj*d6<$>TCydr7C=^Z-94*9EPC1~DK zFz^oZ;0>XxD~CoroKl#4jSJ??xrkub6Lde2#r6m0eJWz6Q@Q7!S>om~`NrV|`IVeA z7yOL0CKJ>GD-nq@qaH-3ql|jSqo}O;o54vw5FbPRw|<}74zVwWxF2b|M5JYLan%+j z1;{vWmrbIgO&mno+3duU1br%GNUx34(Qcj~U{^f$dAc9|U- z7Lp|Opd7A~(P7C8=cqr|_|88=tG6ICXmSgtj3XDFE}WQZOPH*!_ZZ)sFbeT`@5Lo= zIY-EL4z`Uc@ZBELFv|HUBMb;)}4(H1;17~~eA^msi6WZM0(!pWQJnhk87C{}g<{IoZc!OGla z1e=DGL-prlOTJF~-s2DxKg^B#Q12{MEPRyIzwir=bcms3tx2*@qxYZ;$Cw!$TxNKJ zpJtfbi{J0pac%ij7OQrh$y4l1-g7IL^83TF+;^wY&0dDfEelPj^{8J{5Bg+9$^r+J z%{AVXpePdQ;m4v}_YEI+lCaxz@Awz^JVE2nP#MS5Oz)8OMv_{_+>ljaR?hdC#GDid zk0Y`2=QGZ>r805tU1M`yv2`;}TbR*~UvNit<@=}N$Gk#?1%7L<=y5oFc85k!fhS=e zL^oEJm?_)rr9>Omt}1!rcGH$}VAjw~qCaNvuu)m@0sQ6u+>Xb5hl@pK9Zu$H#NU`} z&|klP?CQoloy9AVO*yD$?hP#sX8d)j6KuRJlvFI$ADHAAi>UX*J2ahD@QwNF&-Cd2 zlP}Q1z5yF!>IZClU36OsL*53W9nhquMk8IvT3bV3{y>Knz>$L;HA3I_T5ew!7;#t~ z!c&!D6g+uurL(q%jqn-$UdOvkmmcBhEBWL+p(XDH{SXIy;iAV7tnh53<=4cmjrA~M zc~}*!z_vldd*0peso%_+qbc%$J3OL!ZV`N?V<~Xsa-6c~TrZEL~?DeQWrKn#(Ao{*{kqf=yB->vt87 zAlg(~#|auIKVqLCmwf<`c@GS+L2-M(j0!+y&P)#zmkLLNs%1|Lpx*&nDTsIM0<@<9 zpaCu}0BGVZ=w%@Q(3t)JXjA~ugn$S0PSO(P4}cc;-vnrx)`r~Hht_0D%}o@a_9fn^ z8hlVo|4N3PYh4#_~>(O z>zVsN4E_do2{xsr-Eta{C-baU>}xAgYf}AwLyfd9$kfEHzahz`k-Z4Ig@I%I+ROE~ z6vn>~{xIElQsmV2;LB%cRc7i&Z%0zk0N+T?6OT@jtHh0kLwK<3&MCa@;nU1G^mXs) z<=oo6DapqDYI% zW3al}BYVn^nA&@tH}{o#C*vx^Dx`StO>G8i&W8)^8Gi;5;y|#eO@O;=ewC2{tNAu| zZK2@$8%IA#c6?A6ooF{Q9#G|E8TjA=jqw*)X>(5YG`7s9o>9lRSHgY!f_sNC=%tim z5vJH&>jYvIvGgK&?5k_2{A~|YQ**$5IvBj(f9MGY$J5JumzZA|&_#bi(K^!U)^Fm* z*TuB+jo&FE@F(-L9groG;j$9E`X#;hCbwxKC%ZioMViw0hUlx*(Fyp5HRYQQ1g;H| zjdN!O1QJxr-q$RL)ycR@KG#2+L%V;sN~7)ExvzhB|+zw4*z#r&ULE!6wZ-~X4Z^^iClc^CrPqi_TL zs}udgq{q(3?V1<%wTrB$eEfd=%h=h2UljHt?~~ODrGP|6bauugk9Y?RX2#rX$jnMT z6aV4j4?mwZCl(`Q4cNOIPgB0A*40l-NlQRcN6+ochz)gD(rU16`8;^ZlRJ-k+gT z*nfmdfwjj2eNrIOVaKQNS%(G;J+dD_H&8~%|PC{6QLq;{b8c)!@&OyTSh7W z<^R^6RD{4Lh#_px0M83JbtoS72n@v5MwMbcD_PrfUKvsfv1ZDi@$~uwcfJoLk^`lk5qFYR` z^_zdj#H~R-hNUc$3-Pknq4EWx{JU$zIEi!6rBbw5a^A|t)JioUda^h&6wKV!w1vu= z=)6O7RO8nXaglF6shI!jhncM=eJSkYFhap@E<@KR2N+4)4nx-cWq|LEQo_r*RpV%= zzLW$cL*v972q%^el80n|9dUiTZf;Z5EhT=xjdxbOsRT)bGQjl9>PV7oM@%>o-p}CV z<3mnJU#IbVc=jCNn9m=JqVT1Hr}C&U36$B=f9ogmeGPF^a&sKL(VBD5Jm6Yu9lq2D zK|WO!248jc#ZXx*!2Ppm{BuC<;Wy=}{J_8m3htTL4RO~rz8XIR75-k6z%J1c0oo2H zzB^|?-T!6ihEm+3Z%r4aYx7zOZ)a=NzxcRQt}vE$T= z3Bj>H$tuH^r{SuePWD_^oQ)6HZBw~pFzZXWa3|Wm6SCtqM@cGp45KZTw|BKYg69^Z zL1AR1ISDt{VX^3UNaDQ1HTyCaq}Ea0M}{Vb;TQHLo=v+X+x2gz`fr994IJ{a2-0=R zCqeojUe~7S-E{YecfM(qL8OHAXWNDACUplNy=3D!)g`rr_oiKMscc7FUbfhwN!8k+ zL4_@2NQOu@XnK4NBA;-_QjbUdcK>nB3*LpkkO>W+X-WmUW5!{gD8B@i-FK*?+Sg_zZq(4_u9Ti>p`g^vb#E2(S^ zg>|#b8yJ0fgVGsAhhR#aAIVfkh^t|`GfR{7K2~ErUj6V1U(xP9cTscib48}qwPMQC z?Q6jF#fAU}kP0}Mnv4gpU+aWTa1)n6Zl}f$4Lkq_i0u0|05?J7ml5QhYZ{8sGv+9$ z7_x_NZi7ROdO#9GdJX@_>urWGc|7?Q%I}xoyY|b-A2ZmC$Ic^%qNo96IB>rmTB4qc z`p4^<`t(Te9U4&>?@TH$C1>8_&|Ist`_U8ILfX74HMOFZ>e!^nQ%md60s`EJIS7*0 z0iQeQnTNd9r6B{R9Rmkeu06E5MOXsU21>Kymw~OtF!>BH@lXnS+W;)oKnO%o0VjM@ z*FOga2HdZ8z$W&NBLEB80d^BkHMxJK;I#g6RoskG{qi;n6j8zIJQ@p)^ zhmjK?vs)H{G#NrZ z#@LN0%XjmRFg|YjoWO`tskvZtFIw5@Wzl=lpVMi<3}Q-QWC{ta9UUi@YzX_uYtZwA zj|irsL~Wwvf~hPufaf6yV)|pbzl`M1pAww{llh+{qkySuLG6WBHgMok96>_F>(IZx zHe6?Cb8FeWL`q!3hT`GU7`a$uGv*PDY`GmVIbXV^k z?QAG8tN(GlXK==MqH%yHlhZUZ&7ZE|L}OsIOl0F7b USB (OTG1) + - SERIAL1 -> UART3 (TELEM1) with CTS/RTS DMA Enabled + - SERIAL2 -> UART6 (TELEM2) with DMA Enabled + - SERIAL3 -> UART1 (GPS1) Tx(NODMA), Rx(DMA Enabled) + - SERIAL4 -> EMPTY + - SERIAL5 -> UART7 (GPS2) NODMA + - SERIAL6 -> USART2 (User) NODMA + +## RC Input + +Supports I-Bus/S-bus + +Any UART can be used for RC system connections in ArduPilot also, and is compatible with all protocols except PPM. See Radio Control Systems for details. + +## PWM Output + +The PixFlaminog-F767 supports up to 10 PWM outputs. + +The PWM is in 5 groups: + + - PWM 1-4 in group1 + - PWM 5-8 in group2 + - PWM 9 in group3 + - PWM 10 in group4 + +## GPIOs + +All 10 PWM channels can be used for GPIO functions (relays, buttons, RPM etc). + +The pin numbers for these PWM channels in ArduPilot are shown below: + +| PWM Channels | Pin | PWM Channels | Pin | +| ------------ | ---- | ------------ | ---- | +| PWM1 | 50 | PWM8 | 57 | +| PWM2 | 51 | PWM9 | 58 | +| PWM3 | 52 | PWM10 | 59 | +| PWM4 | 53 | | | +| PWM5 | 54 | | | +| PWM6 | 55 | | | +| PWM7 | 56 | | | + +## Analog inputs + +The PixFlamingo-F767 flight controller has 4 analog inputs + + - ADC Pin10 -> Battery Current + - ADC Pin11 -> Battery Voltage + - ADC Pin14 -> ADC 3V3 Sense + - ADC Pin15 -> ADC 6V6 Sense + +## Battery Monitor Configuration + +The board has voltage and current sensor inputs on the POWER_ADC connector. + +The correct battery setting parameters are: + +Enable Battery monitor. + +BATT_MONITOR =4 + +Then reboot. + +BATT_VOLT_PIN 11 + +BATT_CURR_PIN 10 + +BATT_VOLT_MULT 10.1 (may need adjustment if supplied monitor is not used) + +BATT_AMP_PERVLT 17.0 (may need adjustment if supplied monitor is not used) + +## Build the FC + +./waf configure --board=PixFlamingo-F767 +./waf copter + +The compiled firmware is located in folder **"build/PixFlamingo-F767/bin/arducopter.apj"**. + +## Loading Firmware + +The PixFlamingo-F767 flight controller 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/PixFlamingo-F767/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/defaults.parm new file mode 100755 index 00000000000000..f64860282eca76 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/defaults.parm @@ -0,0 +1 @@ +BRD_HEAT_TARG -1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef-bl.dat new file mode 100755 index 00000000000000..1f39b861741892 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef-bl.dat @@ -0,0 +1,63 @@ +# hw definition file for processing by chibios_hwdef.py for the +# mRo Pixracer board. This is a fmuv4 board + +# MCU class and specific type +MCU STM32F7xx STM32F767xx + +# board ID for firmware load +APJ_BOARD_ID 1131 + +USB_STRING_MANUFACTURER "Dheeran labs" +USB_STRING_PRODUCT "PixFlamingo" + +# crystal frequency +OSCILLATOR_HZ 24000000 + +#define HAL_CHIBIOS_ARCH_FMUV3 1 +# ChibiOS system timer +STM32_ST_USE_TIMER 2 + + +# flash size +FLASH_SIZE_KB 2048 + + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART2 + +define HAL_USE_SERIAL TRUE +define STM32_SERIAL_USE_USART2 TRUE + +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 +PA0 USART2_CTS USART2 +PA1 USART2_RTS USART2 + +PB9 EXTERN_GPIO1 OUTPUT GPIO(4) + +HAL_BOOTLOADER_TIMEOUT 1000 + +PE8 LED_BOOTLOADER OUTPUT LOW +PE7 LED_ACTIVITY OUTPUT +define HAL_LED_ON 1 + +PA9 VBUS INPUT + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# location of application code +FLASH_BOOTLOADER_LOAD_KB 96 + +# bootloader loads at start of flash +FLASH_RESERVE_START_KB 0 + +# Add CS pins to ensure they are high in bootloader +PD0 IMU1_CS CS +PE0 MPU6500_CS CS +PB5 BARO_CS CS +PD7 MAG_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef.dat new file mode 100755 index 00000000000000..37e99059b1e896 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixFlamingo-F767/hwdef.dat @@ -0,0 +1,210 @@ + +# MCU class and specific type +MCU STM32F7xx STM32F767xx + + +# board ID for firmware load +APJ_BOARD_ID 1131 + +# crystal frequency +OSCILLATOR_HZ 24000000 + +USB_STRING_MANUFACTURER "Dheeran labs" +USB_STRING_PRODUCT "PixFlamingo" + +# ChibiOS system timer +STM32_ST_USE_TIMER 2 +#define CH_CFG_ST_RESOLUTION 16 + + +# flash size +FLASH_SIZE_KB 2048 + +FLASH_RESERVE_START_KB 96 +env OPTIMIZE -O2 + +# serial port for stdout disabled, use USB console +# STDOUT_SERIAL SD7 +# STDOUT_BAUDRATE 57600 + +# only one I2C bus +I2C_ORDER I2C2 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART3 USART6 USART1 EMPTY UART7 USART2 + + +#define HAL_SERIAL4_PROTOCOL SerialProtocol_RCIN +#define HAL_SERIAL4_BAUD 115 + +# debug on LUART1 +#STDOUT_SERIAL SD4 +#STDOUT_BAUDRATE 57600 + +PC1 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC0 BATT_CURRENT_SENS ADC1 SCALE(1) +PA4 VDD_5V_SENS ADC1 SCALE(2) + +# SPI1 is fram bus +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +PA9 VBUS INPUT + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# USART3 serial1 telem1 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +PB13 USART3_CTS USART3 +PB14 USART3_RTS USART3 + +# USART2 serial2 telem2 +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 + +PD5 USART2_TX USART2 NODMA +PD6 USART2_RX USART2 NODMA + +# USART1 is GPS +PB6 USART1_TX USART1 NODMA +PB7 USART1_RX USART1 +define HAL_USE_SERIAL TRUE + +PB4 UART7_TX UART7 NODMA +PB3 UART7_RX UART7 NODMA + +#define STM32_SERIAL_USE_USART2 TRUE + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# PWM output for buzzer +PB0 TIM3_CH3 TIM3 RCININT PULLDOWN LOW + +PE2 BOOT1 INPUT +PB12 VDD_BRICK_VALID INPUT PULLDOWN + +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +PA8 CAN3_RX CAN3 +PA15 CAN3_TX CAN3 + +# SPI2 is for sensors +PD3 SPI2_SCK SPI2 +PC2 SPI2_MISO SPI2 +PC3 SPI2_MOSI SPI2 + +PD0 IMU1_CS CS +PE0 MPU6500_CS CS +PB5 BARO_CS CS +PD7 MAG_CS CS + +# This defines more ADC inputs. +PC4 AUX_POWER ADC1 SCALE(1) +PC5 AUX_ADC2 ADC1 SCALE(1) + +PD10 VBUS_VALID INPUT PULLDOWN +PE12 LED_SAFETY OUTPUT +PE15 SAFETY_IN INPUT PULLDOWN +PC14 VDD_PERIPH_EN OUTPUT HIGH + +PC8 SDMMC_D0 SDMMC1 +PC9 SDMMC_D1 SDMMC1 +PC10 SDMMC_D2 SDMMC1 +PC11 SDMMC_D3 SDMMC1 +PC12 SDMMC_CK SDMMC1 +PD2 SDMMC_CMD SDMMC1 + +PE5 IMU1_DRDY INPUT +PE6 MPU6500_DRDY INPUT +PE4 MAG_DRDY INPUT + +PC13 VDD_SENSORS_EN OUTPUT HIGH + +# GPIOs +PE3 HEATER_EN OUTPUT LOW GPIO(80) +define HAL_HEATER_GPIO_PIN 80 +define HAL_HAVE_IMU_HEATER 1 + +PD12 TIM4_CH1 TIM4 GPIO(32) ALARM +PB9 EXTERN_GPIO1 OUTPUT GPIO(4) + +PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) +PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51) +PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) +PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) +PA0 TIM5_CH1 TIM5 PWM(5) GPIO(54) +PA1 TIM5_CH2 TIM5 PWM(6) GPIO(55) +PA2 TIM5_CH3 TIM5 PWM(7) GPIO(56) +PA3 TIM5_CH4 TIM5 PWM(8) GPIO(57) +PB8 TIM10_CH1 TIM10 PWM(9) GPIO(58) NODMA +PB15 TIM8_CH3N TIM8 PWM(10) GPIO(59) NODMA + + +# SPI device table. + +SPIDEV icm42670 SPI2 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ +SPIDEV mpu6500 SPI2 DEVID2 MPU6500_CS MODE3 1*MHZ 4*MHZ +SPIDEV ms5611_int SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ +SPIDEV lis3mdl SPI2 DEVID5 MAG_CS MODE3 500*KHZ 500*KHZ +SPIDEV bmp280 SPI1 DEVID3 BARO_CS MODE3 1*MHZ 8*MHZ +SPIDEV dps310 SPI1 DEVID3 BARO_CS MODE3 5*MHZ 5*MHZ + +# enable FAT filesystem +define HAL_OS_FATFS_IO 1 + +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + + +# pixracer has 3 LEDs, Red, Green, Blue +define HAL_HAVE_PIXRACER_LED + +define HAL_GPIO_LED_ON 0 +define HAL_GPIO_LED_OFF 1 + +PE10 LED_RED OUTPUT GPIO(0) +PE8 LED_GREEN OUTPUT GPIO(1) +PE7 LED_BLUE OUTPUT GPIO(2) + +define HAL_GPIO_A_LED_PIN 0 +define HAL_GPIO_B_LED_PIN 1 +define HAL_GPIO_C_LED_PIN 2 + +# battery setup + define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 11 +define HAL_BATT_CURR_PIN 10 +define HAL_BATT_VOLT_SCALE 10.1 +define HAL_BATT_CURR_SCALE 17.0 + +DMA_PRIORITY S* + +define STORAGE_FLASH_PAGE 1 +define HAL_STORAGE_SIZE 32768 + + +# two IMUs +IMU Invensensev3 SPI:icm42670 ROTATION_YAW_270 #ROTATION_YAW_180 #ROTATION_ROLL_180_YAW_270 #ROTATION_YAW_90 #ROTATION_YAW_270 #ROTATION_ROLL_180 +IMU Invensense SPI:mpu6500 ROTATION_ROLL_180 + +COMPASS LIS3MDL SPI:lis3mdl false ROTATION_NONE + +# also probe all types of external I2C compasses +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 + + +#NODMA I2C* +define STM32_I2C_USE_DMA FALSE +# one barometer +BARO MS56XX SPI:ms5611_int +BARO BMP280 SPI:bmp280 +BARO DPS310 SPI:dps310 +define HAL_BARO_ALLOW_INIT_NO_BARO +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + From 674f75fce16cfc128b6a49237bb68b4c448e439d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 21 Feb 2024 18:53:33 +0000 Subject: [PATCH 15/49] Plane: move manual mode throttle limits to main throttle limit function. --- ArduPlane/mode.h | 2 +- ArduPlane/mode_manual.cpp | 27 ++++++++++++--------------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 3721eb8de32d7b..597399cc21676d 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -400,7 +400,7 @@ class ModeManual : public Mode void run() override; // true if throttle min/max limits should be applied - bool use_throttle_limits() const override { return false; } + bool use_throttle_limits() const override; // true if voltage correction should be applied to throttle bool use_battery_compensation() const override { return false; } diff --git a/ArduPlane/mode_manual.cpp b/ArduPlane/mode_manual.cpp index 1f8fcd702cd14e..8c5a8341023b10 100644 --- a/ArduPlane/mode_manual.cpp +++ b/ArduPlane/mode_manual.cpp @@ -6,22 +6,8 @@ void ModeManual::update() SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.roll_in_expo(false)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitch_in_expo(false)); output_rudder_and_steering(plane.rudder_in_expo(false)); - float throttle = plane.get_throttle_input(true); - -#if HAL_QUADPLANE_ENABLED - if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { - // for quadplanes it can be useful to run the idle governor in MANUAL mode - // as it prevents the VTOL motors from running - int8_t min_throttle = plane.aparm.throttle_min.get(); - - // apply idle governor -#if AP_ICENGINE_ENABLED - plane.g2.ice_control.update_idle_governor(min_throttle); -#endif - throttle = MAX(throttle, min_throttle); - } -#endif + const float throttle = plane.get_throttle_input(true); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); plane.nav_roll_cd = ahrs.roll_sensor; @@ -32,3 +18,14 @@ void ModeManual::run() { reset_controllers(); } + +// true if throttle min/max limits should be applied +bool ModeManual::use_throttle_limits() const +{ +#if HAL_QUADPLANE_ENABLED + if (quadplane.available() && quadplane.option_is_set(QuadPlane::OPTION::IDLE_GOV_MANUAL)) { + return true; + } +#endif + return false; +} From 4a310fb2077e55aaf2557b85a502e3dcd782be51 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 27 Feb 2024 00:19:06 +0000 Subject: [PATCH 16/49] Tools: autotest: Plane: add min throttle test --- Tools/autotest/arduplane.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 531a63a24f9d4f..3365d811472d19 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -5312,6 +5312,38 @@ def SagetechMXS(self): home = self.home_position_as_mav_location() self.assert_distance(home, adsb_vehicle_loc, 0, 10000) + def MinThrottle(self): + '''Make sure min throttle does not apply in manual mode and does in FBWA''' + + servo_min = self.get_parameter("RC3_MIN") + servo_max = self.get_parameter("RC3_MAX") + min_throttle = 10 + servo_min_throttle = servo_min + (servo_max - servo_min) * (min_throttle / 100) + + # Set min throttle + self.set_parameter('THR_MIN', min_throttle) + + # Should be 0 throttle while disarmed + self.change_mode("MANUAL") + self.drain_mav() # make sure we have the latest data before checking throttle output + self.assert_servo_channel_value(3, servo_min) + + # Arm and check throttle is still 0 in manual + self.wait_ready_to_arm() + self.arm_vehicle() + self.drain_mav() + self.assert_servo_channel_value(3, servo_min) + + # FBWA should apply throttle min + self.change_mode("FBWA") + self.drain_mav() + self.assert_servo_channel_value(3, servo_min_throttle) + + # But not when disarmed + self.disarm_vehicle() + self.drain_mav() + self.assert_servo_channel_value(3, servo_min) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -5419,6 +5451,7 @@ def tests(self): self.TerrainRally, self.MAV_CMD_NAV_LOITER_UNLIM, self.MAV_CMD_NAV_RETURN_TO_LAUNCH, + self.MinThrottle, ]) return ret From 1bf7792fe5acfe11e71b896a86d5f7a2766494c0 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 28 Feb 2024 03:16:52 +0000 Subject: [PATCH 17/49] AP_Motors: Heli: remove `output_armed_zero_throttle` and use identical `output_armed_stabilizing` --- libraries/AP_Motors/AP_MotorsHeli.cpp | 17 +---------------- libraries/AP_Motors/AP_MotorsHeli.h | 1 - 2 files changed, 1 insertion(+), 17 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index fc93264ed5d7f9..3bb6bf95cdce85 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -211,11 +211,7 @@ void AP_MotorsHeli::output() // block servo_test from happening at disarm _servo_test_cycle_counter = 0; calculate_armed_scalars(); - if (!get_interlock()) { - output_armed_zero_throttle(); - } else { - output_armed_stabilizing(); - } + output_armed_stabilizing(); } else { output_disarmed(); } @@ -237,17 +233,6 @@ void AP_MotorsHeli::output_armed_stabilizing() move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in); } -// output_armed_zero_throttle - sends commands to the motors -void AP_MotorsHeli::output_armed_zero_throttle() -{ - // if manual override active after arming, deactivate it and reinitialize servos - if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) { - reset_flight_controls(); - } - - move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in); -} - // output_disarmed - sends commands to the motors void AP_MotorsHeli::output_disarmed() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f2a8ce6100400b..e7f5c20afee8a3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -182,7 +182,6 @@ class AP_MotorsHeli : public AP_Motors { // output - sends commands to the motors void output_armed_stabilizing() override; - void output_armed_zero_throttle(); void output_disarmed(); // external objects we depend upon From 73760ea393d30a76d0807d9685427e953dda734b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 23:29:00 +0000 Subject: [PATCH 18/49] Copter: Heli: remove duplicate inverted flight state --- ArduCopter/Copter.h | 5 ++--- ArduCopter/RC_Channel.cpp | 4 ---- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b4dba290ce9b35..fbf4c800b46d49 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -580,9 +580,8 @@ class Copter : public AP_Vehicle { // Tradheli flags typedef struct { uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms) - uint8_t inverted_flight : 1; // 1 // true for inverted flight mode - uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation - bool coll_stk_low ; // 3 // true when collective stick is on lower limit + uint8_t in_autorotation : 1; // 1 // true when heli is in autorotation + bool coll_stk_low ; // 2 // true when collective stick is on lower limit } heli_flags_t; heli_flags_t heli_flags; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index dde5eb48e37c7d..b6b63fadd2a096 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -421,17 +421,13 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc #if FRAME_CONFIG == HELI_FRAME switch (ch_flag) { case AuxSwitchPos::HIGH: - copter.motors->set_inverted_flight(true); copter.attitude_control->set_inverted_flight(true); - copter.heli_flags.inverted_flight = true; break; case AuxSwitchPos::MIDDLE: // nothing break; case AuxSwitchPos::LOW: - copter.motors->set_inverted_flight(false); copter.attitude_control->set_inverted_flight(false); - copter.heli_flags.inverted_flight = false; break; } #endif From 24c843dc26283cc207886cdfcd2142a968a88019 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 23:30:04 +0000 Subject: [PATCH 19/49] AP_Motors: Heli: remove inverted flight state --- libraries/AP_Motors/AP_MotorsHeli.h | 4 ---- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 4 ---- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 4 ---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 4 ---- 4 files changed, 16 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e7f5c20afee8a3..c3c5cfc0c1ea51 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -75,9 +75,6 @@ class AP_MotorsHeli : public AP_Motors { // set_collective_for_landing - limits collective from going too low if we know we are landed void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; } - // set_inverted_flight - enables/disables inverted flight - void set_inverted_flight(bool inverted) { _heliflags.inverted_flight = inverted; } - // get_rsc_mode - gets the current rotor speed control method uint8_t get_rsc_mode() const { return _main_rotor.get_control_mode(); } @@ -247,7 +244,6 @@ class AP_MotorsHeli : public AP_Motors { struct heliflags_type { uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up - uint8_t inverted_flight : 1; // true for inverted flight uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming uint8_t save_rsc_mode : 1; // used to determine the rsc mode needs to be saved while disarmed uint8_t in_autorotation : 1; // true if aircraft is in autorotation diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index ddbcedc896e44f..2849b3c85cf775 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -415,10 +415,6 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c } } - if (_heliflags.inverted_flight) { - collective_in = 1 - collective_in; - } - // constrain collective input float collective_out = collective_in; if (collective_out <= 0.0f) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 515ec063e0f2c0..da956f2dbf848f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -207,10 +207,6 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c float collective_range = (_collective_max - _collective_min) * 0.001f; - if (_heliflags.inverted_flight) { - collective_out = 1.0f - collective_out; - } - // feed power estimate into main rotor controller _main_rotor.set_collective(fabsf(collective_out)); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 1c73e10f5082ac..65213c60bff5a6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -382,10 +382,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float limit.throttle_lower = false; limit.throttle_upper = false; - if (_heliflags.inverted_flight) { - coll_in = 1 - coll_in; - } - // rescale roll_out and pitch_out into the min and max ranges to provide linear motion // across the input range instead of stopping when the input hits the constrain value // these calculations are based on an assumption of the user specified cyclic_max From 576ee75669bf9edab4df419f839dbaee12dfab74 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 23:30:54 +0000 Subject: [PATCH 20/49] AC_AttitudeControl: Heli: invert throttle in inverted flight, move state down to heli --- libraries/AC_AttitudeControl/AC_AttitudeControl.h | 3 --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 5 +++++ libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h | 3 +++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f75230980414d2..29ec8eae60682a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -566,9 +566,6 @@ class AC_AttitudeControl { void control_monitor_filter_pid(float value, float &rms_P); void control_monitor_update(void); - // true in inverted flight mode - bool _inverted_flight; - public: // log a CTRL message void control_monitor_log(void) const; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 982ff2048ab881..21cf2ffae7f085 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -538,6 +538,11 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang { _throttle_in = throttle_in; update_althold_lean_angle_max(throttle_in); + + if (_inverted_flight) { + throttle_in = 1.0 - throttle_in; + } + _motors.set_throttle_filter_cutoff(filter_cutoff); _motors.set_throttle(throttle_in); // Clear angle_boost for logging purposes diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index 0d519bf87d8f2e..d978d62f347a5e 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -101,6 +101,9 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { uint8_t tail_passthrough : 1; // 1 if we should pass through pilots yaw input to tail } _flags_heli; + // true in inverted flight mode + bool _inverted_flight; + // Integrate vehicle rate into _att_error_rot_vec_rad void integrate_bf_rate_error_to_angle_errors(); From 52bffc4b4dcaafc2f1baa763d5e443a990868910 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 23:31:38 +0000 Subject: [PATCH 21/49] Copter: Heli: ensure inverted flight only in supported modes --- ArduCopter/RC_Channel.cpp | 6 +++++- ArduCopter/mode.cpp | 5 +++++ ArduCopter/mode.h | 6 ++++++ 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index b6b63fadd2a096..3fa2f6c4d85b4e 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -421,7 +421,11 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc #if FRAME_CONFIG == HELI_FRAME switch (ch_flag) { case AuxSwitchPos::HIGH: - copter.attitude_control->set_inverted_flight(true); + if (copter.flightmode->allows_inverted()) { + copter.attitude_control->set_inverted_flight(true); + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name()); + } break; case AuxSwitchPos::MIDDLE: // nothing diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 921f9fb32902e6..73b1384bc5f6cd 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -457,6 +457,11 @@ void Copter::exit_mode(Mode *&old_flightmode, input_manager.set_stab_col_ramp(0.0); } } + + // Make sure inverted flight is disabled if not supported in the new mode + if (!new_flightmode->allows_inverted()) { + attitude_control->set_inverted_flight(false); + } #endif //HELI_FRAME } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149c296..173d6b47d1ba9b 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -128,6 +128,10 @@ class Mode { virtual bool allows_autotune() const { return false; } virtual bool allows_flip() const { return false; } +#if FRAME_CONFIG == HELI_FRAME + virtual bool allows_inverted() const { return false; }; +#endif + // return a string for this flightmode virtual const char *name() const = 0; virtual const char *name4() const = 0; @@ -1574,6 +1578,8 @@ class ModeStabilize_Heli : public ModeStabilize { bool init(bool ignore_checks) override; void run() override; + bool allows_inverted() const override { return true; }; + protected: private: From d103eebf9157a16fdf4ffd22ff5588e396306ba6 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Thu, 29 Feb 2024 23:09:14 -0500 Subject: [PATCH 22/49] AC_AttitudeControl: Tradheli- fix inverted mode collective handling --- .../AC_AttitudeControl_Heli.cpp | 32 ++++++++++++++++--- .../AC_AttitudeControl_Heli.h | 6 ++-- 2 files changed, 31 insertions(+), 7 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 21cf2ffae7f085..98b5df48f945cc 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -539,12 +539,27 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang _throttle_in = throttle_in; update_althold_lean_angle_max(throttle_in); - if (_inverted_flight) { - throttle_in = 1.0 - throttle_in; + if (_transition_count > 0) { + _transition_count -= 1; + } else { + _transition_count = 0; + } + float throttle_out = 0.0f; + if (_transition_count > 0) { + if ((_ahrs.roll_sensor >= -3000 && _ahrs.roll_sensor <= 3000) || _ahrs.roll_sensor >= 15000 || _ahrs.roll_sensor <= -15000) { + throttle_out = (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid()) / cosf(radians(_ahrs.roll_sensor * 0.01f)) + ((AP_MotorsHeli&)_motors).get_coll_mid(); + } else if ((_ahrs.roll_sensor > 3000 && _ahrs.roll_sensor < 15000) || (_ahrs.roll_sensor > -15000 && _ahrs.roll_sensor < -3000)) { + float scale_factor = cosf(radians(_ahrs.roll_sensor * 0.01f)) / cosf(radians(30.0f)); + throttle_out = scale_factor * (throttle_in - ((AP_MotorsHeli&)_motors).get_coll_mid())/ cosf(radians(30.0f)) + ((AP_MotorsHeli&)_motors).get_coll_mid(); + } + } else if (_inverted_flight) { + throttle_out = 1.0f - throttle_in; + } else { + throttle_out = throttle_in; } _motors.set_throttle_filter_cutoff(filter_cutoff); - _motors.set_throttle(throttle_in); + _motors.set_throttle(throttle_out); // Clear angle_boost for logging purposes _angle_boost = 0.0f; } @@ -574,4 +589,13 @@ void AC_AttitudeControl_Heli::set_notch_sample_rate(float sample_rate) _pid_rate_pitch.set_notch_sample_rate(sample_rate); _pid_rate_yaw.set_notch_sample_rate(sample_rate); #endif -} \ No newline at end of file +} + +// enable/disable inverted flight +void AC_AttitudeControl_Heli::set_inverted_flight(bool inverted) +{ + if (_inverted_flight != inverted) { + _transition_count = AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME * AP::scheduler().get_filtered_loop_rate_hz(); + } + _inverted_flight = inverted; +} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index d978d62f347a5e..974a404dee4e4b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -28,6 +28,7 @@ #define AC_ATTITUDE_HELI_RATE_Y_FF_FILTER 20.0f #define AC_ATTITUDE_HELI_HOVER_ROLL_TRIM_DEFAULT 300 #define AC_ATTITUDE_HELI_ACRO_OVERSHOOT_ANGLE_RAD ToRad(30.0f) +#define AC_ATTITUDE_HELI_INVERTED_TRANSITION_TIME 3.0f class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: @@ -82,9 +83,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw) override; // enable/disable inverted flight - void set_inverted_flight(bool inverted) override { - _inverted_flight = inverted; - } + void set_inverted_flight(bool inverted) override; // set the PID notch sample rates void set_notch_sample_rate(float sample_rate) override; @@ -103,6 +102,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { // true in inverted flight mode bool _inverted_flight; + uint16_t _transition_count; // Integrate vehicle rate into _att_error_rot_vec_rad void integrate_bf_rate_error_to_angle_errors(); From cbb47ed42fdd4cbabefb0ea048986c176b6f0adc Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 1 Mar 2024 12:58:54 -0500 Subject: [PATCH 23/49] Copter: Remove tradheli arming check for inverted flight --- ArduCopter/AP_Arming.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 4167e6a7538524..3a31aac761c752 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -231,12 +231,6 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) return false; } - // Inverted flight feature disabled for Heli Single and Dual frames - if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && - rc().find_channel_for_option(RC_Channel::AUX_FUNC::INVERTED) != nullptr) { - check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported"); - return false; - } // Ensure an Aux Channel is configured for motor interlock if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) == nullptr) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured"); From e9397e10f98622f327e241511e9898591932da81 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 15:57:37 +0000 Subject: [PATCH 24/49] ArduCopter: don't try and send MSG_RANGEFINDER if AP_RANGEFINDER_ENABLED is false --- ArduCopter/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index c35d96e4ba52c1..431e4b9c2232ab 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -544,7 +544,9 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, MSG_SYSTEM_TIME, MSG_WIND, +#if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, +#endif MSG_DISTANCE_SENSOR, #if AP_TERRAIN_AVAILABLE MSG_TERRAIN, From 6461ebdfafc9a559937a3c06f1e148687e19ac2d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 15:57:37 +0000 Subject: [PATCH 25/49] ArduPlane: don't try and send MSG_RANGEFINDER if AP_RANGEFINDER_ENABLED is false --- ArduPlane/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c8ee1e7a20c379..dec67d5a2beddb 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -675,7 +675,9 @@ static const ap_message STREAM_EXTRA2_msgs[] = { static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, MSG_WIND, +#if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, +#endif MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, #if AP_TERRAIN_AVAILABLE From fd94222832917a2335987eb255f1988d9b292a59 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 15:57:37 +0000 Subject: [PATCH 26/49] ArduSub: don't try and send MSG_RANGEFINDER if AP_RANGEFINDER_ENABLED is false --- ArduSub/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index a2be9236718b58..3ea5a72fa5e2c6 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -389,7 +389,9 @@ static const ap_message STREAM_EXTRA2_msgs[] = { static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, MSG_SYSTEM_TIME, +#if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, +#endif MSG_DISTANCE_SENSOR, #if AP_TERRAIN_AVAILABLE MSG_TERRAIN, From 99bfc03bc53333fab2525c52763b28ca89fa96ee Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 15:57:37 +0000 Subject: [PATCH 27/49] Blimp: don't try and send MSG_RANGEFINDER if AP_RANGEFINDER_ENABLED is false --- Blimp/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 17193438c1dd67..e6107aab6fa3ac 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -357,7 +357,9 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, MSG_SYSTEM_TIME, MSG_WIND, +#if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, +#endif MSG_DISTANCE_SENSOR, #if AP_BATTERY_ENABLED MSG_BATTERY_STATUS, From 61560fd12965d1366230224ed49f0563e74f1c74 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 29 Feb 2024 15:57:37 +0000 Subject: [PATCH 28/49] Rover: don't try and send MSG_RANGEFINDER if AP_RANGEFINDER_ENABLED is false --- Rover/GCS_Mavlink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index efe14de0780a30..e0261f2a3f83d6 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -568,7 +568,9 @@ static const ap_message STREAM_EXTRA2_msgs[] = { static const ap_message STREAM_EXTRA3_msgs[] = { MSG_AHRS, MSG_WIND, +#if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, +#endif MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, #if AP_BATTERY_ENABLED From d0a7b54e6184926de345fe949e07a937962841dc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Mar 2024 11:06:20 +1100 Subject: [PATCH 29/49] AP_ESC_Telem: move defaulting of HAL_WITH_ESC_TELEM for periph --- libraries/AP_ESC_Telem/AP_ESC_Telem_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h index 616b6c99bca642..fdd9cea2feba10 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_config.h @@ -4,5 +4,5 @@ #include #ifndef HAL_WITH_ESC_TELEM -#define HAL_WITH_ESC_TELEM ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS) && !defined(HAL_BUILD_AP_PERIPH))) +#define HAL_WITH_ESC_TELEM ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS))) #endif From b09dd7b866021bdc3c7c2d910b7e1f85b6eb465c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 1 Mar 2024 11:06:20 +1100 Subject: [PATCH 30/49] AP_HAL_ChibiOS: move defaulting of HAL_WITH_ESC_TELEM for periph --- libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index 00906d54108351..1ac7ab88ec58ea 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -317,6 +317,10 @@ #define AP_SCRIPTING_ENABLED 0 #endif +#ifndef HAL_WITH_ESC_TELEM +#define HAL_WITH_ESC_TELEM 0 +#endif + #ifndef AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED #define AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED 0 #endif From 2d5e6a5f98d95f50af76f72ff3862fe6c78e63da Mon Sep 17 00:00:00 2001 From: Pulak Gautam Date: Sun, 3 Mar 2024 18:26:08 +0530 Subject: [PATCH 31/49] AP_ExternalAHRS: added missing #if AHRS logging --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index cbcc394fb042ff..dc3ff2a34b268c 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -301,6 +301,7 @@ void AP_ExternalAHRS::update(void) state.have_origin = true; } } +#if HAL_LOGGING_ENABLED const uint32_t now_ms = AP_HAL::millis(); if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) { last_log_ms = now_ms; @@ -334,6 +335,7 @@ void AP_ExternalAHRS::update(void) state.location.lat, state.location.lng, state.location.alt*0.01, filterStatus.value); } +#endif // HAL_LOGGING_ENABLED } // Get model/type name From 8a478abce99cb3409d45b812ff36e7263e8257a6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Mar 2024 09:55:48 +1100 Subject: [PATCH 32/49] RC_Channel: allow customisation of position text in aux switch announcement this means that we get "EKFPosSource 1" rather than "EKFPosSource LOW" --- libraries/RC_Channel/RC_Channel.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index b0b4787909134d..21928b0fcfeac2 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -753,7 +753,6 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"}, { AUX_FUNC::VISODOM_ALIGN,"VisOdomAlign"}, { AUX_FUNC::AIRMODE, "AirMode"}, - { AUX_FUNC::EKF_POS_SOURCE,"EKFPosSource"}, { AUX_FUNC::CAM_MODE_TOGGLE,"CamModeToggle"}, { AUX_FUNC::GENERATOR,"Generator"}, { AUX_FUNC::BATTERY_MPPT_ENABLE,"Battery MPPT Enable"}, @@ -1434,22 +1433,26 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch break; #endif - case AUX_FUNC::EKF_POS_SOURCE: + case AUX_FUNC::EKF_POS_SOURCE: { + uint8_t source_set = 0; switch (ch_flag) { case AuxSwitchPos::LOW: // low switches to primary source - AP::ahrs().set_posvelyaw_source_set(0); + source_set = 0; break; case AuxSwitchPos::MIDDLE: // middle switches to secondary source - AP::ahrs().set_posvelyaw_source_set(1); + source_set = 1; break; case AuxSwitchPos::HIGH: // high switches to tertiary source - AP::ahrs().set_posvelyaw_source_set(2); + source_set = 2; break; } + AP::ahrs().set_posvelyaw_source_set(source_set); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Using EKF Source Set %u", source_set+1); break; + } #if AP_OPTICALFLOW_CALIBRATOR_ENABLED case AUX_FUNC::OPTFLOW_CAL: { From 4d2c7f3d64a88bd92b22a3f168bf6f12321c8fbb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Feb 2024 11:28:59 +1100 Subject: [PATCH 33/49] Plane: don't special case tilt rotors for motors shutdown this allows for Q_M_SPOOL_TIME to work correctly for back transition --- ArduPlane/quadplane.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c2cff5682e8171..b284e5289bef34 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1761,10 +1761,8 @@ void SLT_Transition::update() } case TRANSITION_DONE: - if (!quadplane.tiltrotor.motors_active()) { - quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); - motors->output(); - } + quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); + motors->output(); set_last_fw_pitch(); in_forced_transition = false; return; @@ -1851,7 +1849,7 @@ void QuadPlane::update(void) plane.control_mode == &plane.mode_acro || plane.control_mode == &plane.mode_training) { // in manual modes quad motors are always off - if (!tiltrotor.motors_active() && !tailsitter.enabled()) { + if (!tailsitter.enabled()) { set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); } From 2b784e01f9a07effe9db96310515aa567a4aca22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 21 Feb 2024 16:38:09 +1100 Subject: [PATCH 34/49] AP_Motors: allow output_motor_mask() to work properly with slew limits this fixes tilt quadplanes with slew limits when we set motors state to SHUT_DOWN --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 4 ++++ libraries/AP_Motors/AP_MotorsMulticopter.h | 10 ++++++++++ libraries/AP_Motors/AP_MotorsTri.cpp | 8 +++++--- 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index e2e71392621cfe..70e07103be6ff8 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -148,7 +148,7 @@ void AP_MotorsMatrix::output_to_motors() case SpoolState::SHUT_DOWN: { // no output for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { - if (motor_enabled[i]) { + if (motor_enabled_mask(i)) { _actuator[i] = 0.0f; } } diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e68cf85c8208ab..e4a75ef3303eed 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -272,6 +272,8 @@ void AP_MotorsMulticopter::output() // check for any external limit flags update_external_limits(); + // clear mask of overridden motors + _motor_mask_override = 0; }; void AP_MotorsMulticopter::update_external_limits() @@ -728,6 +730,8 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float const int16_t pwm_min = get_pwm_output_min(); const int16_t pwm_range = get_pwm_output_max() - pwm_min; + _motor_mask_override = mask; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { if ((mask & (1U << i)) && armed() && get_interlock()) { diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 3f97e5105a3bda..d6fdf6a3092b55 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -192,4 +192,14 @@ class AP_MotorsMulticopter : public AP_Motors { // array of motor output values float _actuator[AP_MOTORS_MAX_NUM_MOTORS]; + + /* motor enabled, checking the override mask + _motor_mask_override is only set for tilt quadplanes + */ + bool motor_enabled_mask(uint8_t i) const { + return motor_enabled[i] && (_motor_mask_override & (1U << i)) == 0; + } + + // mask of overridden motors (used by quadplane tiltrotors) + uint16_t _motor_mask_override; }; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 43730335fd584b..c109ade0936164 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -90,9 +90,11 @@ void AP_MotorsTri::output_to_motors() switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors - _actuator[AP_MOTORS_MOT_1] = 0.0; - _actuator[AP_MOTORS_MOT_2] = 0.0; - _actuator[AP_MOTORS_MOT_4] = 0.0; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + if (motor_enabled_mask(i)) { + rc_write(AP_MOTORS_MOT_1+i, output_to_pwm(0)); + } + } rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); break; case SpoolState::GROUND_IDLE: From 171da3dd08e3c70fa8b00e8355da504b8c7e9330 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Mar 2024 08:04:53 +1100 Subject: [PATCH 35/49] AP_Motors: smoother ramp down in output_motor_mask stop changing motors outside the given mask in output_motor_mask, which gives smoother ramp down in tilt quadplanes when we are transitioning to forward flight thanks to Pete for the suggestion --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 11 ++++++----- libraries/AP_Motors/AP_MotorsTri.cpp | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index e4a75ef3303eed..cece6dd8cde09a 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -733,8 +733,8 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float _motor_mask_override = mask; for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { - if (motor_enabled[i]) { - if ((mask & (1U << i)) && armed() && get_interlock()) { + if (motor_enabled[i] && (mask & (1U << i)) != 0) { + if (armed() && get_interlock()) { /* apply rudder mixing differential thrust copter frame roll is plane frame yaw as this only @@ -742,11 +742,12 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint16_t mask, float */ float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f; set_actuator_with_slew(_actuator[i], thrust + diff_thrust); - int16_t pwm_output = pwm_min + pwm_range * _actuator[i]; - rc_write(i, pwm_output); } else { - rc_write(i, pwm_min); + // zero throttle + _actuator[i] = 0.0; } + int16_t pwm_output = pwm_min + pwm_range * _actuator[i]; + rc_write(i, pwm_output); } } } diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index c109ade0936164..ab566944aeb967 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -92,7 +92,7 @@ void AP_MotorsTri::output_to_motors() // sends minimum values out to the motors for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled_mask(i)) { - rc_write(AP_MOTORS_MOT_1+i, output_to_pwm(0)); + _actuator[AP_MOTORS_MOT_1+i] = 0; } } rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0); From 155f31a3a2876c5b149cf4d0c58ba6b39123d903 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 4 Mar 2024 14:34:29 -0800 Subject: [PATCH 36/49] Plane: allow quadplanes to scale ESC CAN like normal --- ArduPlane/radio.cpp | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 6f5ea8daeb62bb..60688c0ecefc27 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -49,16 +49,9 @@ void Plane::set_control_channels(void) quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR); #endif - bool set_throttle_esc_scaling = true; -#if HAL_QUADPLANE_ENABLED - set_throttle_esc_scaling = !quadplane.enable; -#endif - if (set_throttle_esc_scaling) { - // setup correct scaling for ESCs like the UAVCAN ESCs which - // take a proportion of speed. For quadplanes we use AP_Motors - // scaling - g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); - } + // setup correct scaling for ESCs like the UAVCAN ESCs which + // take a proportion of speed. + g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle); } /* From e5f092482dfd0de66715aed091b5f9b3800037bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Feb 2024 19:17:58 +1100 Subject: [PATCH 37/49] AC_PrecLand: added get_target_location and get_target_velocity --- libraries/AC_PrecLand/AC_PrecLand.cpp | 42 +++++++++++++++++++++++++++ libraries/AC_PrecLand/AC_PrecLand.h | 14 +++++++++ 2 files changed, 56 insertions(+) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 49c7a11060a90a..ece865343688d9 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -727,6 +727,9 @@ void AC_PrecLand::run_output_prediction() _target_vel_rel_out_NE.x -= vel_ned_rel_imu.x; _target_vel_rel_out_NE.y -= vel_ned_rel_imu.y; + // remember vehicle velocity + UNUSED_RESULT(_ahrs.get_velocity_NED(_last_veh_velocity_NED_ms)); + // Apply land offset Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f; _target_pos_rel_out_NE.x += land_ofs_ned_m.x; @@ -742,6 +745,45 @@ void AC_PrecLand::run_output_prediction() _last_valid_target_ms = AP_HAL::millis(); } +/* + get target location lat/lon. Note that altitude in returned + location is not reliable + */ +bool AC_PrecLand::get_target_location(Location &loc) +{ + if (!target_acquired()) { + return false; + } + if (!AP::ahrs().get_origin(loc)) { + return false; + } + loc.offset(_last_target_pos_rel_origin_NED.x, _last_target_pos_rel_origin_NED.y); + loc.alt -= _last_target_pos_rel_origin_NED.z*100; + return true; +} + +/* + get the absolute velocity of the target in m/s. + return false if we cannot estimate target velocity or if the target is not acquired +*/ +bool AC_PrecLand::get_target_velocity(Vector2f& target_vel) +{ + if (!(_options & PLND_OPTION_MOVING_TARGET)) { + // the target should not be moving + return false; + } + if ((EstimatorType)_estimator_type.get() == EstimatorType::RAW_SENSOR) { + return false; + } + Vector2f target_vel_rel_cms; + if (!get_target_velocity_relative_cms(target_vel_rel_cms)) { + return false; + } + // return the absolute velocity + target_vel = (target_vel_rel_cms*0.01) + _last_veh_velocity_NED_ms.xy(); + return true; +} + #if HAL_LOGGING_ENABLED // Write a precision landing entry void AC_PrecLand::Write_Precland() diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index a25b30c71f4f42..e11a51f81ddb01 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -17,6 +17,7 @@ class AC_PrecLand_Companion; class AC_PrecLand_IRLock; class AC_PrecLand_SITL_Gazebo; class AC_PrecLand_SITL; +class Location; class AC_PrecLand { @@ -116,6 +117,18 @@ class AC_PrecLand bool allow_precland_after_reposition() const { return _options & PLND_OPTION_PRECLAND_AFTER_REPOSITION; } bool do_fast_descend() const { return _options & PLND_OPTION_FAST_DESCEND; } + /* + get target location lat/lon. Note that altitude in returned + location is not reliable + */ + bool get_target_location(Location &loc); + + /* + get the absolute velocity of the target in m/s. + return false if we cannot estimate target velocity or if the target is not acquired + */ + bool get_target_velocity(Vector2f& ret); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -215,6 +228,7 @@ class AC_PrecLand Vector2f _target_pos_rel_out_NE; // target's position relative to the camera, fed into position controller Vector2f _target_vel_rel_out_NE; // target's velocity relative to the CG, fed into position controller + Vector3f _last_veh_velocity_NED_ms; // AHRS velocity at last estimate TargetState _current_target_state; // Current status of the landing target From a4109c6cf27b7f5a831c25050c72f1f0492922ce Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Feb 2024 19:18:14 +1100 Subject: [PATCH 38/49] Plane: compile in AC_PrecLand for scripting --- ArduPlane/ArduPlane.cpp | 12 ++++++++++++ ArduPlane/Parameters.cpp | 8 +++++++- ArduPlane/Parameters.h | 4 ++++ ArduPlane/Plane.h | 9 +++++++++ ArduPlane/system.cpp | 4 ++++ ArduPlane/wscript | 2 ++ 6 files changed, 38 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 53e265b4fae38c..98a1dda0176b9d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -135,6 +135,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { #if AP_LANDINGGEAR_ENABLED SCHED_TASK(landing_gear_update, 5, 50, 159), #endif +#if AC_PRECLAND_ENABLED + SCHED_TASK(precland_update, 400, 50, 160), +#endif }; void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, @@ -947,4 +950,13 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const return g2.flight_options & flight_option; } +#if AC_PRECLAND_ENABLED +void Plane::precland_update(void) +{ + // alt will be unused if we pass false through as the second parameter: + return g2.precland.update(rangefinder_state.height_estimate*100, + rangefinder_state.in_range && rangefinder_state.in_use); +} +#endif + AP_HAL_MAIN_CALLBACKS(&plane); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 53ad53307e061c..166b141c87bf86 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1242,7 +1242,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw // @User: Standard AP_GROUPINFO("AUTOTUNE_AXES", 34, ParametersG2, axis_bitmask, 7), - + +#if AC_PRECLAND_ENABLED + // @Group: PLND_ + // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp + AP_SUBGROUPINFO(precland, "PLND_", 35, ParametersG2, AC_PrecLand), +#endif + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index dacc015496d97f..ba89c457728112 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -520,6 +520,10 @@ class ParametersG2 { AP_LandingGear landing_gear; #endif +#if AC_PRECLAND_ENABLED + AC_PrecLand precland; +#endif + // crow flaps weighting AP_Int8 crow_flap_weight_outer; AP_Int8 crow_flap_weight_inner; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3d08a2bca9add5..189189e9dcb844 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -87,6 +87,11 @@ #include "AP_ExternalControl_Plane.h" #endif +#include +#if AC_PRECLAND_ENABLED + # include +#endif + #include "GCS_Mavlink.h" #include "GCS_Plane.h" #include "quadplane.h" @@ -251,6 +256,10 @@ class Plane : public AP_Vehicle { AP_Rally rally; #endif +#if AC_PRECLAND_ENABLED + void precland_update(void); +#endif + // returns a Location for a rally point or home; if // HAL_RALLY_ENABLED is false, just home. Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 593dfb1c16e8c7..17d57395d92f57 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -132,6 +132,10 @@ void Plane::init_ardupilot() optflow.init(-1); } #endif + +#if AC_PRECLAND_ENABLED + g2.precland.init(scheduler.get_loop_rate_hz()); +#endif } //******************************************************************************** diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 4dd670ccc34a52..32bbdb0a5aabd9 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -29,6 +29,8 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_Follow', + 'AC_PrecLand', + 'AP_IRLock', ], ) From 8c33e2d2894893176112a752641bc4ec349d3d71 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Mar 2024 08:01:53 +1100 Subject: [PATCH 39/49] Plane: allow QLAND to use precision landing override --- ArduPlane/ArduPlane.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 98a1dda0176b9d..56cd867f692ef6 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -863,6 +863,18 @@ bool Plane::update_target_location(const Location &old_loc, const Location &new_ next_WP_loc = new_loc; next_WP_loc.change_alt_frame(old_loc.get_alt_frame()); +#if HAL_QUADPLANE_ENABLED + if (control_mode == &mode_qland) { + /* + support precision landing controlled by lua in QLAND mode + */ + Vector2f rel_origin; + if (new_loc.get_vector_xy_from_origin_NE(rel_origin)) { + quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); + } + } +#endif + return true; } From 4f9b66c267afbc5888553680217ac578043275f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 25 Feb 2024 17:30:36 +1100 Subject: [PATCH 40/49] SITL: fixed precland simulator fixed units of height, was mixed cm and m, now just m AMSL --- libraries/SITL/SIM_Precland.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp index 8406d1696c25c5..7bb40f8ea36230 100644 --- a/libraries/SITL/SIM_Precland.cpp +++ b/libraries/SITL/SIM_Precland.cpp @@ -50,9 +50,9 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = { AP_GROUPINFO("LON", 2, SIM_Precland, _device_lon, 0), // @Param: HEIGHT - // @DisplayName: Precland device center's height above sealevel - // @Description: Precland device center's height above sealevel assume a 2x2m square as station base - // @Units: cm + // @DisplayName: Precland device center's height SITL origin + // @Description: Precland device center's height above SITL origin. Assumes a 2x2m square as station base + // @Units: m // @Increment: 1 // @Range: 0 10000 // @User: Advanced @@ -115,7 +115,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = { AP_GROUPEND }; -void SIM_Precland::update(const Location &loc, const Vector3d &position) +void SIM_Precland::update(const Location &loc, const Vector3d &position_relhome) { if (!_enable) { _healthy = false; @@ -127,21 +127,21 @@ void SIM_Precland::update(const Location &loc, const Vector3d &position) } const Location device_center(static_cast(_device_lat * 1.0e7f), - static_cast(_device_lon * 1.0e7f), - static_cast(_device_height), - Location::AltFrame::ABOVE_HOME); - Vector2f centerf; - if (!device_center.get_vector_xy_from_origin_NE(centerf)) { + static_cast(_device_lon * 1.0e7f), + static_cast(_device_height*100), + Location::AltFrame::ABOVE_ORIGIN); + Vector3f centerf; + if (!device_center.get_vector_from_origin_NEU(centerf)) { _healthy = false; return; } centerf = centerf * 0.01f; // cm to m - Vector3d center(centerf.x, centerf.y, -_device_height); // convert to make the further vector operations easy + centerf.z *= -1; // NEU to NED // axis of cone or cylinder inside which the vehicle receives signals from simulated precland device Vector3d axis{1, 0, 0}; axis.rotate((Rotation)_orient.get()); // unit vector in direction of axis of cone or cylinder - Vector3d position_wrt_device = position - center; // position of vehicle with respect to preland device center + Vector3d position_wrt_device = position_relhome - centerf.todouble(); // position of vehicle with respect to preland device center // longitudinal distance of vehicle from the precland device // this is the distance of vehicle from the plane which is passing through precland device center and perpendicular to axis of cone/cylinder From 350215eb70fe12b6707e2dcf433f0f23a753b4f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Mar 2024 07:48:03 +1100 Subject: [PATCH 41/49] autotest: fixed got and want ordering in text wait --- Tools/autotest/vehicle_test_suite.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4d51a6a9b9c65a..4aee1199e39667 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7953,7 +7953,7 @@ def statustext_in_collections(self, text, regex=False): if "STATUSTEXT" not in c.collections: raise NotAchievedException("Asked to check context but it isn't collecting!") for x in c.collections["STATUSTEXT"]: - self.progress(" statustext want=(%s) got=(%s)" % (x.text, text)) + self.progress(" statustext got=(%s) want=(%s)" % (x.text, text)) if regex: if re.match(text, x.text): return x From e3df084b96eacc6b7df66516f436eefe852276e8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Mar 2024 18:33:52 +1100 Subject: [PATCH 42/49] autotest: fixed SIM_PLD_HEIGHT for AUTO_LAND_TO_BRAKE --- Tools/autotest/arducopter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d8dd4b23156834..528f9851fe05b4 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9617,7 +9617,7 @@ def AUTO_LAND_TO_BRAKE(self): "SIM_PLD_ALT_LMT": 15.000000, "SIM_PLD_DIST_LMT": 10.000000, "SIM_PLD_ENABLE": 1, - "SIM_PLD_HEIGHT": 942.0000000, + "SIM_PLD_HEIGHT": 0, "SIM_PLD_LAT": -20.558929, "SIM_PLD_LON": -47.415035, "SIM_PLD_RATE": 100, From de786932a64755b129dad1a63f8b88afd8c34a7d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Mar 2024 08:02:23 +1100 Subject: [PATCH 43/49] autotest: added PrecisionLanding test --- Tools/autotest/quadplane.py | 59 +++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 1207691fc76650..8a64abe577a7cc 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1335,6 +1335,64 @@ def VTOLQuicktune(self): self.remove_installed_script(applet_script) self.reboot_sitl() + def PrecisionLanding(self): + '''VTOL precision landing''' + applet_script = "plane_precland.lua" + + self.install_applet_script(applet_script) + + target = mavutil.location(-27.27440414, 151.28984285, 342.8, 0) + + self.set_parameters({ + "SCR_ENABLE": 1, + "SIM_SPEEDUP": 4, + "PLND_ENABLED": 1, + "PLND_TYPE": 4, + "SIM_PLD_ENABLE": 1, + "SIM_PLD_LAT" : target.lat, + "SIM_PLD_LON" : target.lng, + "SIM_PLD_HEIGHT" : 0, + "SIM_PLD_ALT_LMT" : 50, + "SIM_PLD_DIST_LMT" : 30, + "RNGFND1_TYPE": 100, + "RNGFND1_PIN" : 0, + "RNGFND1_SCALING" : 12.2, + "RNGFND1_MAX_CM" : 5000, + "RNGFND_LANDING" : 1, + }) + + self.reboot_sitl() + + self.context_push() + self.context_collect('STATUSTEXT') + self.set_parameters({ + "PLND_ALT_CUTOFF" : 5, + }) + + self.scripting_restart() + self.wait_text("PLND: Loaded", check_context=True) + + self.wait_ready_to_arm() + self.change_mode("GUIDED") + self.arm_vehicle() + self.takeoff(60, 'GUIDED') + self.wait_altitude(58, 62, relative=True) + self.drain_mav() + self.change_mode("QRTL") + + self.wait_text("PLND: Target Acquired", check_context=True, timeout=60) + + self.wait_disarmed(timeout=180) + loc2 = self.mav.location() + error = self.get_distance(target, loc2) + self.progress("Target error %.1fm" % error) + if error > 2: + raise NotAchievedException("too far from target %.1fm" % error) + + self.context_pop() + self.remove_installed_script(applet_script) + self.reboot_sitl() + def RCDisableAirspeedUse(self): '''check disabling airspeed using RC switch''' self.set_parameter("RC9_OPTION", 106) @@ -1578,6 +1636,7 @@ def tests(self): self.LoiterAltQLand, self.VTOLLandSpiral, self.VTOLQuicktune, + self.PrecisionLanding, Test(self.MotorTest, kwargs={ # tests motors 4 and 2 "mot1_servo_chan": 8, # quad-x second motor cw from f-r "mot4_servo_chan": 6, # quad-x third motor cw from f-r From bac99dc7ace92a7e4d67290d5f7358ad4dec7837 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Feb 2024 19:19:01 +1100 Subject: [PATCH 44/49] AP_Scripting: added bindings for AC_PrecLand --- .../AP_Scripting/generator/description/bindings.desc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 8e01c48ebbecf5..c02040c7098d29 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -719,6 +719,15 @@ singleton AP_Follow method get_target_location_and_velocity boolean Location'Nul singleton AP_Follow method get_target_location_and_velocity_ofs boolean Location'Null Vector3f'Null singleton AP_Follow method get_target_heading_deg boolean float'Null +include AC_PrecLand/AC_PrecLand.h +singleton AC_PrecLand depends AC_PRECLAND_ENABLED && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) +singleton AC_PrecLand rename precland +singleton AC_PrecLand method healthy boolean +singleton AC_PrecLand method target_acquired boolean +singleton AC_PrecLand method get_last_valid_target_ms uint32_t +singleton AC_PrecLand method get_target_velocity boolean Vector2f'Null +singleton AC_PrecLand method get_target_location boolean Location'Null + include AC_AttitudeControl/AC_AttitudeControl.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AC_AttitudeControl depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AC_AttitudeControl method get_rpy_srate void float'Ref float'Ref float'Ref From fa1d965224acb8ebb6c0a964976f72e16b6b8215 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Mar 2024 18:38:54 +1100 Subject: [PATCH 45/49] AP_Scripting: added docs for precland --- libraries/AP_Scripting/docs/docs.lua | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7375e0a0d273a5..0807c406d1dfcf 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3263,6 +3263,30 @@ AR_PosControl = {} ---@return number -- velocity slew rate function AR_PosControl:get_srate() end +-- precision landing access +---@class precland +precland = {} + +-- get Location of target or nil if target not acquired +---@return Location_ud|nil +function precland:get_target_location() end + +-- get NE velocity of target or nil if not available +---@return Vector2f_ud|nil +function precland:get_target_velocity() end + +-- get the time of the last valid target +---@return uint32_t_ud +function precland:get_last_valid_target_ms() end + +-- return true if target is acquired +---@return boolean +function precland:target_acquired() end + +-- return true if precland system is healthy +---@return boolean +function precland:healthy() end + -- desc ---@class follow follow = {} From c210675e9577caaa69fb331b7aa7d593ba49ff83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Mar 2024 18:14:09 +1100 Subject: [PATCH 46/49] AP_Scripting: added plane_precland lua script --- .../AP_Scripting/applets/plane_precland.lua | 212 ++++++++++++++++++ .../AP_Scripting/applets/plane_precland.md | 49 ++++ 2 files changed, 261 insertions(+) create mode 100644 libraries/AP_Scripting/applets/plane_precland.lua create mode 100644 libraries/AP_Scripting/applets/plane_precland.md diff --git a/libraries/AP_Scripting/applets/plane_precland.lua b/libraries/AP_Scripting/applets/plane_precland.lua new file mode 100644 index 00000000000000..4450fdea303c3c --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_precland.lua @@ -0,0 +1,212 @@ +--[[ + support precision landing on quadplanes + + This is a very simple implementation intended to act as a framework + for development of a custom solution +--]] + +local PARAM_TABLE_KEY = 12 +local PARAM_TABLE_PREFIX = "PLND_" + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local MODE_QLAND = 20 + +-- bind a parameter to a variable +function bind_param(name) + return Parameter(name) +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- setup precland specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table') + +--[[ + // @Param: PLND_ALT_CUTOFF + // @DisplayName: Precland altitude cutoff + // @Description: The altitude (rangefinder distance) below which we stop using the precision landing sensor and continue landing + // @Range: 0 20 + // @Units: m + // @User: Standard +--]] +PLND_ALT_CUTOFF = bind_add_param('ALT_CUTOFF', 1, 0) + +--[[ + // @Param: DIST_CUTOFF + // @DisplayName: Precland distance cutoff + // @Description: The distance from target beyond which the target is ignored + // @Range: 0 100 + // @Units: m + // @User: Standard +--]] +PLND_DIST_CUTOFF = bind_add_param('DIST_CUTOFF', 2, 0) + +-- other parameters +PLND_ENABLED = bind_param("PLND_ENABLED") +PLND_XY_DIST_MAX = bind_param("PLND_XY_DIST_MAX") +PLND_OPTIONS = bind_param("PLND_OPTIONS") + +if PLND_ENABLED:get() == 0 then + gcs:send_text(MAV_SEVERITY.INFO, "PLND: Disabled") + return +end + +local have_target = false + +local rangefinder_orient = 25 -- downward + +--[[ + update the have_target variable +--]] +function update_target() + if not precland:healthy() then + have_target = false + return + end + local ok = precland:target_acquired() + + if PLND_ALT_CUTOFF:get() > 0 then + -- require rangefinder as well + if not rangefinder:has_data_orient(rangefinder_orient) then + ok = false + end + end + + if ok ~= have_target then + have_target = ok + if have_target then + gcs:send_text(MAV_SEVERITY.INFO, "PLND: Target Acquired") + else + gcs:send_text(MAV_SEVERITY.INFO, "PLND: Target Lost") + end + end +end + +--[[ + return true if we are in an automatic landing and should + try to use precision landing +--]] +function in_auto_land() + return quadplane:in_vtol_land_descent() or vehicle:get_mode() == MODE_QLAND +end + +-- main update function +function update() + if PLND_ENABLED:get() < 1 then + return + end + + --[[ + get the current navigation target. Note that we must get this + before we check if we are in a landing descent to prevent a race condition + with vehicle:update_target_location() + --]] + local next_WP = vehicle:get_target_location() + if not next_WP then + -- not in a flight mode with a target location + return + end + + -- see if we are landing + if not in_auto_land() then + return + end + + update_target() + if not have_target then + return + end + + --[[ ask precland for the target. Note that we ignore the altitude + in the return as it is unreliable + --]] + local loc = precland:get_target_location() + if not loc then + return + end + + --[[ get rangefinder distance, and if PLND_ALT_CUTOFF is set then + stop precland operation if below the cutoff + --]] + local rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_orient) * 0.01 + if PLND_ALT_CUTOFF:get() > 0 and rngfnd_distance_m < PLND_ALT_CUTOFF:get() then + return + end + + --[[ + update the vehicle target to match the precland target + --]] + local new_WP = next_WP:copy() + new_WP:lat(loc:lat()) + new_WP:lng(loc:lng()) + vehicle:update_target_location(next_WP, new_WP) + + veh_loc = ahrs:get_location() + + local xy_dist = veh_loc:get_distance(new_WP) + + --[[ + get target velocity and if velocity matching is enabled in + PLND_OPTIONS then ask vehicle to match + --]] + local target_vel = precland:get_target_velocity() + if target_vel and (PLND_OPTIONS:get():toint() & 1) ~= 0 then + vehicle:set_velocity_match(target_vel) + end + if not target_vel then + target_vel = Vector2f() + end + + --[[ + log the target and distance + --]] + logger.write("PPLD", 'Lat,Lon,Alt,HDist,TDist,RFND,VN,VE', + 'LLffffff', + 'DUmmmmmm', + 'GG------', + new_WP:lat(), + new_WP:lng(), + new_WP:alt(), + xy_dist, + next_WP:get_distance(next_WP), + rngfnd_distance_m, + target_vel:x(), + target_vel:y()) + + --[[ + stop using precland if too far away + --]] + if PLND_DIST_CUTOFF:get() > 0 and xy_dist > PLND_DIST_CUTOFF:get() then + return + end + + if xy_dist > PLND_XY_DIST_MAX:get() then + -- pause descent till we are within the given radius + vehicle:set_land_descent_rate(0) + end +end + +gcs:send_text(MAV_SEVERITY.INFO, "PLND: Loaded") + +-- wrapper around update(). This calls update() at 20Hz, +-- and if update faults then an error is displayed, but the script is not +-- stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(0, "Internal Error: " .. err) + -- when we fault we run the update function again after 1s, slowing it + -- down a bit so we don't flood the console with errors + return protected_wrapper, 1000 + end + return protected_wrapper, 100 +end + +-- start running update loop +return protected_wrapper() + diff --git a/libraries/AP_Scripting/applets/plane_precland.md b/libraries/AP_Scripting/applets/plane_precland.md new file mode 100644 index 00000000000000..95f7660cef651f --- /dev/null +++ b/libraries/AP_Scripting/applets/plane_precland.md @@ -0,0 +1,49 @@ +# Plane Precision Landing + +This script implements a precision landing system for VTOL fixed wing +aircraft (quadplanes). It is a simple script which is intended to be +modified by vendors for their specific aircraft. + +# Parameters + +Beyond the normal PLND parameters the script adds 2 additional parameters to +control it's behaviour. The parameters are: + +## PLND_ALT_CUTOFF + +This is an optional altitude in meters below which the precision +landing system will stop correcting the landing position. Many +precision landing sensors have poor performance at low altitudes, so +setting this to around 5 meters is advisable. A value of zero disables +this cutoff. + +## PLND_DIST_CUTOFF + +This is a maximum horizontal distance in meters that will be accepted +for a landing corrections. If this parameter is greater than zero and +the precision landing subsystem gives a distance beyond this distance +then precision landing correction will stop and the last landing +position will be used. + +# Operation + +You should first install and configure a precision landing sensor as described here: + + https://ardupilot.org/copter/docs/precision-landing-with-irlock.html + +then you should enable the precision subsystem and install the lua +script in the APM/scripts folder of your flight controller. + +The script will start adjusting the landing position only when in the +descent phase of an automatic VTOL landing. The PPLD log message can +be used to analyse the performance of the precision landing. + +It is advisable to have a manual pilot able to take over control in a +mode such as QLOITER for instances where the precision landing system +may malfunction. + +## Moving Target + +If the PLND_OPTIONS bit for a moving target is enabled then the +vehicle will be set to track the estimated target velocity during +descent From 6a678305566282f975e74feb48a8a129274d6b2e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Mar 2024 08:53:17 +1100 Subject: [PATCH 47/49] autotest: added quadplane ShipLanding test --- .../ShipLanding/takeoff100.txt | 3 + Tools/autotest/quadplane.py | 61 ++++++++++++++++++- Tools/autotest/vehicle_test_suite.py | 4 +- 3 files changed, 64 insertions(+), 4 deletions(-) create mode 100644 Tools/autotest/ArduPlane_Tests/ShipLanding/takeoff100.txt diff --git a/Tools/autotest/ArduPlane_Tests/ShipLanding/takeoff100.txt b/Tools/autotest/ArduPlane_Tests/ShipLanding/takeoff100.txt new file mode 100644 index 00000000000000..2ab5bdbbe7ff12 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/ShipLanding/takeoff100.txt @@ -0,0 +1,3 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 37.625620 -122.332354 0.100000 1 +1 0 3 84 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 1 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 8a64abe577a7cc..12b7cfda70db03 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1341,11 +1341,11 @@ def PrecisionLanding(self): self.install_applet_script(applet_script) - target = mavutil.location(-27.27440414, 151.28984285, 342.8, 0) + here = self.mav.location() + target = self.offset_location_ne(here, 20, 0) self.set_parameters({ "SCR_ENABLE": 1, - "SIM_SPEEDUP": 4, "PLND_ENABLED": 1, "PLND_TYPE": 4, "SIM_PLD_ENABLE": 1, @@ -1367,6 +1367,7 @@ def PrecisionLanding(self): self.context_collect('STATUSTEXT') self.set_parameters({ "PLND_ALT_CUTOFF" : 5, + "SIM_SPEEDUP" : 10, }) self.scripting_restart() @@ -1393,6 +1394,61 @@ def PrecisionLanding(self): self.remove_installed_script(applet_script) self.reboot_sitl() + def ShipLanding(self): + '''ship landing test''' + applet_script = "plane_ship_landing.lua" + + self.install_applet_script(applet_script) + + self.set_parameters({ + "SCR_ENABLE": 1, + "SIM_SHIP_ENABLE": 1, + "SIM_SHIP_SPEED": 5, + "SIM_SHIP_DSIZE": 10, + "FOLL_ENABLE": 1, + "FOLL_SYSID": 17, + "FOLL_OFS_TYPE": 1, + "SIM_TERRAIN" : 0, + "TERRAIN_ENABLE" : 0, + }) + + self.load_mission("takeoff100.txt") + + self.reboot_sitl(check_position=False) + + self.context_push() + self.context_collect('STATUSTEXT') + self.set_parameters({ + "SHIP_ENABLE" : 1, + "SIM_SPEEDUP" : 10, + }) + + self.scripting_restart() + self.wait_text("ShipLanding: loaded", check_context=True) + + self.wait_ready_to_arm() + self.change_mode("AUTO") + self.arm_vehicle() + self.wait_altitude(95, 105, relative=True, timeout=90) + self.drain_mav() + + self.wait_text("Mission complete, changing mode to RTL", check_context=True, timeout=60) + self.wait_text("Descending for approach", check_context=True, timeout=60) + self.wait_text("Reached target altitude", check_context=True, timeout=120) + self.wait_text("Starting approach", check_context=True, timeout=120) + self.wait_text("Land complete", check_context=True, timeout=120) + + self.wait_disarmed(timeout=180) + + # we confirm successful landing on the ship from our ground speed. The + # deck is just 10m in size, so we must be within 10m if we are moving + # with the deck + self.wait_groundspeed(4.8, 5.2) + + self.context_pop() + self.remove_installed_script(applet_script) + self.reboot_sitl(check_position=False) + def RCDisableAirspeedUse(self): '''check disabling airspeed using RC switch''' self.set_parameter("RC9_OPTION", 106) @@ -1637,6 +1693,7 @@ def tests(self): self.VTOLLandSpiral, self.VTOLQuicktune, self.PrecisionLanding, + self.ShipLanding, Test(self.MotorTest, kwargs={ # tests motors 4 and 2 "mot1_servo_chan": 8, # quad-x second motor cw from f-r "mot4_servo_chan": 6, # quad-x third motor cw from f-r diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4aee1199e39667..2b1bd1ff962c29 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2002,14 +2002,14 @@ def send_cmd_enter_cpu_lockup(self): 0, 0) - def reboot_sitl(self, required_bootcount=None, force=False): + def reboot_sitl(self, required_bootcount=None, force=False, check_position=True): """Reboot SITL instance and wait for it to reconnect.""" if self.armed() and not force: raise NotAchievedException("Reboot attempted while armed") self.progress("Rebooting SITL") self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force) self.do_heartbeats(force=True) - if self.frame != 'sailboat': # sailboats drift with wind! + if check_position and self.frame != 'sailboat': # sailboats drift with wind! self.assert_simstate_location_is_at_startup_location() def reboot_sitl_mavproxy(self, required_bootcount=None): From dc863d878a3ee469d718f5ad06fc7b3d5ae21255 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Mar 2024 08:55:02 +1100 Subject: [PATCH 48/49] AP_Scripting: fixed race condition in ship landing and fixed lua warnings --- .../applets/plane_ship_landing.lua | 50 +++++++++++-------- 1 file changed, 30 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Scripting/applets/plane_ship_landing.lua b/libraries/AP_Scripting/applets/plane_ship_landing.lua index 07975cb4c922ba..45bc9be2d42c4e 100644 --- a/libraries/AP_Scripting/applets/plane_ship_landing.lua +++ b/libraries/AP_Scripting/applets/plane_ship_landing.lua @@ -1,5 +1,10 @@ --- support takeoff and landing on moving platforms for VTOL planes --- luacheck: only 0 +--[[ + support takeoff and landing on moving platforms for VTOL planes + + See this post for details: https://discuss.ardupilot.org/t/ship-landing-support +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local PARAM_TABLE_KEY = 7 local PARAM_TABLE_PREFIX = "SHIP_" @@ -120,7 +125,7 @@ function check_parameters() assert(current, string.format("Parameter %s not found", p)) if math.abs(v-current) > 0.001 then param:set_and_save(p, v) - gcs:send_text(0,string.format("Parameter %s set to %.2f was %.2f", p, v, current)) + gcs:send_text(MAV_SEVERITY.INFO, string.format("Parameter %s set to %.2f was %.2f", p, v, current)) end end end @@ -145,11 +150,11 @@ function update_throttle_pos() reached_alt = false if landing_stage == STAGE_HOLDOFF and tpos <= THROTTLE_MID then landing_stage = STAGE_DESCEND - gcs:send_text(0, string.format("Descending for approach (hd=%.1fm h=%.1f th=%.1f)", + gcs:send_text(MAV_SEVERITY.INFO, string.format("Descending for approach (hd=%.1fm h=%.1f th=%.1f)", get_holdoff_distance(), current_pos:alt()*0.01, get_target_alt())) end if landing_stage == STAGE_DESCEND and tpos == THROTTLE_HIGH then - gcs:send_text(0,"Climbing for holdoff") + gcs:send_text(MAV_SEVERITY.INFO, "Climbing for holdoff") landing_stage = STAGE_HOLDOFF end end @@ -263,7 +268,7 @@ function check_approach_tangent() local distance = current_pos:get_distance(target_pos) local holdoff_dist = get_holdoff_distance() if landing_stage == STAGE_HOLDOFF and throttle_pos <= THROTTLE_MID and distance < 4*holdoff_dist then - gcs:send_text(0, string.format("Descending for approach (hd=%.1fm)", holdoff_dist)) + gcs:send_text(MAV_SEVERITY.INFO, string.format("Descending for approach (hd=%.1fm)", holdoff_dist)) landing_stage = STAGE_DESCEND end if reached_alt and landing_stage == STAGE_DESCEND then @@ -274,8 +279,6 @@ function check_approach_tangent() local target_bearing_deg = wrap_180(math.deg(current_pos:get_bearing(target_pos))) local ground_bearing_deg = wrap_180(math.deg(ahrs:groundspeed_vector():angle())) local margin = 10 - local distance = current_pos:get_distance(target_pos) - local holdoff_dist = get_holdoff_distance() local error1 = math.abs(wrap_180(target_bearing_deg - ground_bearing_deg)) local error2 = math.abs(wrap_180(ground_bearing_deg - (target_heading + SHIP_LAND_ANGLE:get()))) logger.write('SLND','TBrg,GBrg,Dist,HDist,Err1,Err2','ffffff',target_bearing_deg, ground_bearing_deg, distance, holdoff_dist, error1, error2) @@ -284,7 +287,7 @@ function check_approach_tangent() distance > 0.7*holdoff_dist and error2 < 2*margin) then -- we are on the tangent, switch to QRTL - gcs:send_text(0, "Starting approach") + gcs:send_text(MAV_SEVERITY.INFO, "Starting approach") landing_stage = STAGE_APPROACH vehicle:set_mode(MODE_QRTL) end @@ -298,7 +301,7 @@ function check_approach_abort() local alt = current_pos:alt() * 0.01 local target_alt = get_target_alt() if alt > target_alt then - gcs:send_text(0, "Aborting landing") + gcs:send_text(MAV_SEVERITY.NOTICE, "Aborting landing") landing_stage = STAGE_HOLDOFF vehicle:set_mode(MODE_RTL) end @@ -324,14 +327,14 @@ end function update_target() if not follow:have_target() then if have_target then - gcs:send_text(0,"Lost beacon") + gcs:send_text(MAV_SEVERITY.WARNING, "Lost beacon") arming:set_aux_auth_failed(auth_id, "Ship: no beacon") end have_target = false return end if not have_target then - gcs:send_text(0,"Have beacon") + gcs:send_text(MAV_SEVERITY.INFO, "Have beacon") arming:set_aux_auth_passed(auth_id) end have_target = true @@ -358,7 +361,7 @@ function update_alt() if landing_stage == STAGE_HOLDOFF or landing_stage == STAGE_DESCEND then if math.abs(alt - target_alt) < 3 then if not reached_alt then - gcs:send_text(0,"Reached target altitude") + gcs:send_text(MAV_SEVERITY.INFO, "Reached target altitude") end reached_alt = true end @@ -382,7 +385,7 @@ function update_auto_offset() local new = target_no_ofs:get_distance_NED(current_pos) new:rotate_xy(-math.rad(target_heading)) - gcs:send_text(0,string.format("Set follow offset (%.2f,%.2f,%.2f)", new:x(), new:y(), new:z())) + gcs:send_text(MAV_SEVERITY.INFO, string.format("Set follow offset (%.2f,%.2f,%.2f)", new:x(), new:y(), new:z())) FOLL_OFS_X:set_and_save(new:x()) FOLL_OFS_Y:set_and_save(new:y()) FOLL_OFS_Z:set_and_save(new:z()) @@ -407,6 +410,16 @@ function update() end current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + --[[ + get target location before we check vehicle state to prevent a + race condition with the user changing mode or target + --]] + local next_WP = vehicle:get_target_location() + if not next_WP then + -- not in a flight mode with a target location + return + end + update_throttle_pos() update_mode() update_alt() @@ -414,11 +427,6 @@ function update() ahrs:set_home(target_pos) - local next_WP = vehicle:get_target_location() - if not next_WP then - -- not in a flight mode with a target location - return - end next_WP:change_alt_frame(ALT_FRAME_ABSOLUTE) if vehicle_mode == MODE_RTL then @@ -463,13 +471,15 @@ end check_parameters() +gcs:send_text(MAV_SEVERITY.INFO, "ShipLanding: loaded") + -- wrapper around update(). This calls update() at 20Hz, -- and if update faults then an error is displayed, but the script is not -- stopped function protected_wrapper() local success, err = pcall(update) if not success then - gcs:send_text(0, "Internal Error: " .. err) + gcs:send_text(MAV_SEVERITY.ERROR, "Internal Error: " .. err) -- when we fault we run the update function again after 1s, slowing it -- down a bit so we don't flood the console with errors return protected_wrapper, 1000 From cbe5cf8c81bad5768ffde07e1877524a8e14d0b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Mar 2024 09:32:24 +1100 Subject: [PATCH 49/49] Plane: support precland in QLAND for pos, velocity and descent rate allow full override in QLAND --- ArduPlane/ArduPlane.cpp | 20 ++++++++++---------- ArduPlane/GCS_Mavlink.cpp | 10 ++++++++++ ArduPlane/GCS_Mavlink.h | 1 + ArduPlane/mode.h | 2 ++ ArduPlane/mode_qland.cpp | 31 +++++++++++++++++++++++++++++++ 5 files changed, 54 insertions(+), 10 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 56cd867f692ef6..60a22e44fe4124 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -857,21 +857,20 @@ bool Plane::get_target_location(Location& target_loc) */ bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) { - if (!old_loc.same_loc_as(next_WP_loc)) { + /* + by checking the caller has provided the correct old target + location we prevent a race condition where the user changes mode + or commands a different target in the controlling lua script + */ + if (!old_loc.same_loc_as(next_WP_loc) || + old_loc.get_alt_frame() != new_loc.get_alt_frame()) { return false; } next_WP_loc = new_loc; - next_WP_loc.change_alt_frame(old_loc.get_alt_frame()); #if HAL_QUADPLANE_ENABLED if (control_mode == &mode_qland) { - /* - support precision landing controlled by lua in QLAND mode - */ - Vector2f rel_origin; - if (new_loc.get_vector_xy_from_origin_NE(rel_origin)) { - quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); - } + mode_qland.last_target_loc_set_ms = AP_HAL::millis(); } #endif @@ -895,7 +894,8 @@ bool Plane::set_velocity_match(const Vector2f &velocity) bool Plane::set_land_descent_rate(float descent_rate) { #if HAL_QUADPLANE_ENABLED - if (quadplane.in_vtol_land_descent()) { + if (quadplane.in_vtol_land_descent() || + control_mode == &mode_qland) { quadplane.poscontrol.override_descent_rate = descent_rate; quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis(); return true; diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index dec67d5a2beddb..dc97584962ad18 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -748,6 +748,16 @@ void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &c } +/* + handle a LANDING_TARGET command. The timestamp has been jitter corrected +*/ +void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) +{ +#if AC_PRECLAND_ENABLED + plane.g2.precland.handle_msg(packet, timestamp_ms); +#endif +} + MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { plane.in_calibration = true; diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 5920a354028bf0..b3b59cb2d8790d 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -50,6 +50,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void send_pid_tuning() override; void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; + void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; private: diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 597399cc21676d..076d51f87026f3 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -669,6 +669,7 @@ friend class ModeQLand; class ModeQLand : public Mode { public: + friend class Plane; Number mode_number() const override { return Number::QLAND; } const char *name() const override { return "QLAND"; } @@ -686,6 +687,7 @@ class ModeQLand : public Mode bool _enter() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; } + uint32_t last_target_loc_set_ms; }; class ModeQRTL : public Mode diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index e646c807468174..681f7cb26cfee3 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -19,6 +19,10 @@ bool ModeQLand::_enter() #if AP_FENCE_ENABLED plane.fence.auto_disable_fence_for_landing(); #endif + + // clear precland timestamp + last_target_loc_set_ms = 0; + return true; } @@ -29,6 +33,33 @@ void ModeQLand::update() void ModeQLand::run() { + /* + see if precision landing is active with an override of the + target location + */ + const uint32_t last_pos_set_ms = last_target_loc_set_ms; + const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms; + const uint32_t now_ms = AP_HAL::millis(); + + if (last_pos_set_ms != 0 && now_ms - last_pos_set_ms < 500) { + // we have an active landing target override + Vector2f rel_origin; + if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) { + quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y); + } + } + + // allow for velocity override as well + if (last_vel_set_ms != 0 && now_ms - last_vel_set_ms < 500) { + // we have an active landing velocity override + Vector2f target_accel; + Vector2f target_speed_xy_cms{quadplane.poscontrol.velocity_match.x*100, quadplane.poscontrol.velocity_match.y*100}; + quadplane.pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel); + } + + /* + use QLOITER to do the main control + */ plane.mode_qloiter.run(); }