From 884450868e919d9f10a63d7543cd15f727d2efda Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Feb 2022 14:04:57 +1100 Subject: [PATCH 0001/3101] AP_NavEKF: use length_squared in place of length --- libraries/AP_NavEKF/EKFGSF_yaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 290e65eaf4c..bfc424ac7ab 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -226,8 +226,8 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx) // Gyro bias estimation const ftype gyro_bias_limit = radians(5.0f); - const ftype spinRate = ang_rate_delayed_raw.length(); - if (spinRate < 0.175f) { + const ftype spinRate_squared = ang_rate_delayed_raw.length_squared(); + if (spinRate_squared < sq(0.175f)) { AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt); for (uint8_t i = 0; i < 3; i++) { From 850227ffdb96136bab0a4ee85678a7bd46232980 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 13:23:57 +1100 Subject: [PATCH 0002/3101] autotest: add test for THR_FAILSAFE==2 and throttle output --- Tools/autotest/arduplane.py | 25 +++++++++++++++++++++++-- Tools/autotest/common.py | 25 +++++++++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 8a250572ac7..2ddc9fa0a58 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -898,7 +898,7 @@ def test_rc_option_camera_trigger(self): raise NotAchievedException("Bad absalt (want=%f vs got=%f)" % (original_alt+30, x.alt_msl)) self.fly_home_land_and_disarm() - def test_throttle_failsafe(self): + def ThrottleFailsafe(self): self.change_mode('MANUAL') m = self.mav.recv_match(type='SYS_STATUS', blocking=True) receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER @@ -1021,6 +1021,27 @@ def test_throttle_failsafe(self): if ex is not None: raise ex + self.start_subtest("Not use RC throttle input when THR_FAILSAFE==2") + self.takeoff(100) + self.set_rc(3, 1800) + self.set_rc(1, 2000) + self.wait_attitude(desroll=45, timeout=1) + self.context_push() + self.set_parameters({ + "THR_FAILSAFE": 2, + "SIM_RC_FAIL": 1, # no pulses + }) + self.delay_sim_time(1) + self.wait_attitude(desroll=0, timeout=5) + self.assert_servo_channel_value(3, self.get_parameter("RC3_MIN")) + self.set_parameters({ + "SIM_RC_FAIL": 0, # fix receiver + }) + self.zero_throttle() + self.disarm_vehicle() + self.context_pop() + self.reboot_sitl() + def test_throttle_failsafe_fence(self): fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE @@ -3504,7 +3525,7 @@ def tests(self): ("ThrottleFailsafe", "Fly throttle failsafe", - self.test_throttle_failsafe), + self.ThrottleFailsafe), ("NeedEKFToArm", "Ensure we need EKF to be healthy to arm", diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 358d6b09e96..0e86a51f454 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -5822,6 +5822,21 @@ def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operato if comparator(m_value, value): return m_value + def assert_servo_channel_value(self, channel, value, comparator=operator.eq): + """assert channel value (default condition is equality)""" + channel_field = "servo%u_raw" % channel + opstring = ("%s" % comparator)[-3:-1] + m = self.assert_receive_message('SERVO_OUTPUT_RAW', timeout=1) + m_value = getattr(m, channel_field, None) + if m_value is None: + raise ValueError("message (%s) has no field %s" % + (str(m), channel_field)) + self.progress("assert SERVO_OUTPUT_RAW.%s=%u %s %u" % + (channel_field, m_value, opstring, value)) + if comparator(m_value, value): + return m_value + raise NotAchievedException("Wrong value") + def get_rc_channel_value(self, channel, timeout=2): """wait for channel to hit value""" channel_field = "chan%u_raw" % channel @@ -5854,6 +5869,16 @@ def wait_rc_channel_value(self, channel, value, timeout=2): if value == m_value: return + def assert_rc_channel_value(self, channel, value): + channel_field = "chan%u_raw" % channel + + m_value = self.get_rc_channel_value(channel, timeout=1) + self.progress("RC_CHANNELS.%s=%u want=%u" % + (channel_field, m_value, value)) + if value != m_value: + raise NotAchievedException("Expected %s to be %u got %u" % + (channel, value, m_value)) + def wait_location(self, loc, accuracy=5.0, From b19bfba0ec8197ea8961e3165d9b04e8bfc3e260 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 26 Jul 2020 10:52:15 +1000 Subject: [PATCH 0003/3101] Plane: use has_valid_input in place of checking throttle counter --- ArduPlane/control_modes.cpp | 5 ++--- ArduPlane/quadplane.cpp | 6 +++--- ArduPlane/servos.cpp | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index d97ce4212a5..62689c78a28 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -107,9 +107,8 @@ void Plane::read_control_switch() // If we get this value we do not want to change modes. if(switchPosition == 255) return; - if (failsafe.rc_failsafe || failsafe.throttle_counter > 0) { - // when we are in rc_failsafe mode then RC input is not - // working, and we need to ignore the mode switch channel + if (!rc().has_valid_input()) { + // ignore the mode switch channel if there is no valid RC input return; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 491d89d79f2..60ce00ce941 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1224,7 +1224,8 @@ float QuadPlane::get_desired_yaw_rate_cds(bool should_weathervane) // get pilot desired climb rate in cm/s float QuadPlane::get_pilot_desired_climb_rate_cms(void) const { - if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) { + if (!rc().has_valid_input()) { + // no valid input means no sensible pilot desired climb rate. // descend at 0.5m/s for now return -50; } @@ -1735,8 +1736,7 @@ void QuadPlane::update(void) // disable throttle_wait when throttle rises above 10% if (throttle_wait && (plane.get_throttle_input() > 10 || - plane.failsafe.rc_failsafe || - plane.failsafe.throttle_counter>0)) { + !rc().has_valid_input())) { throttle_wait = false; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 41b6f8ff45c..2e52219587e 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -561,7 +561,7 @@ void Plane::set_servos_controlled(void) control_mode == &mode_fbwa || control_mode == &mode_autotune) { // a manual throttle mode - if (failsafe.throttle_counter) { + if (!rc().has_valid_input()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } else if (g.throttle_passthru_stabilize) { // manual pass through of throttle while in FBWA or @@ -615,7 +615,7 @@ void Plane::set_servos_flaps(void) int8_t manual_flap_percent = 0; // work out any manual flap input - if (channel_flap != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) { + if (channel_flap != nullptr && rc().has_valid_input()) { manual_flap_percent = channel_flap->percent_input(); } From d7ff10163c270a926c26f54d4fb7e43725cac4b6 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 9 Feb 2022 00:32:46 +0000 Subject: [PATCH 0004/3101] AP_Button: trigger low on invalid PWM input --- libraries/AP_Button/AP_Button.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp index 1349341abb8..2b993f40b5a 100644 --- a/libraries/AP_Button/AP_Button.cpp +++ b/libraries/AP_Button/AP_Button.cpp @@ -74,27 +74,27 @@ const AP_Param::GroupInfo AP_Button::var_info[] = { // @Param: OPTIONS1 // @DisplayName: Button Pin 1 Options - // @Description: Options for Pin 1. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 1. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS1", 6, AP_Button, options[0], 0), // @Param: OPTIONS2 // @DisplayName: Button Pin 2 Options - // @Description: Options for Pin 2. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 2. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS2", 7, AP_Button, options[1], 0), // @Param: OPTIONS3 // @DisplayName: Button Pin 3 Options - // @Description: Options for Pin 3. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 3. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS3", 8, AP_Button, options[2], 0), // @Param: OPTIONS4 // @DisplayName: Button Pin 4 Options - // @Description: Options for Pin 4. PWM input detects PWM above or below 1800/1200us instead of logic level. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. + // @Description: Options for Pin 4. PWM input detects PWM above or below 1800/1200us instead of logic level. If PWM is not detected or is less than 800us or above 2200us the button will interpreted as low. Invert changes HIGH state to be logic low voltage on pin, or below 1200us, if PWM input. // @User: Standard // @Bitmask: 0:PWM Input,1:InvertInput AP_GROUPINFO("OPTIONS4", 9, AP_Button, options[3], 0), @@ -183,6 +183,13 @@ void AP_Button::update(void) continue; } const uint16_t pwm_us = pwm_pin_source[i].get_pwm_us(); + if (pwm_us < RC_Channel::RC_MIN_LIMIT_PWM || pwm_us > RC_Channel::RC_MAX_LIMIT_PWM) { + // invalid pulse width, trigger low + if (pwm_state & mask) { + new_pwm_state &= ~mask; + } + continue; + } // these values are the same as used in RC_Channel: if (pwm_state & mask) { // currently asserted; check to see if we should de-assert From 96c47dadcbdfbdea4cb744390a36940026580fda Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:23 +0000 Subject: [PATCH 0005/3101] Copter: motor_test: use PWM min and max from RC_Channel --- ArduCopter/motor_test.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 2248aa3e169..b222e24f493 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -6,8 +6,6 @@ */ // motor test definitions -#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test -#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test #define MOTOR_TEST_TIMEOUT_SEC 600 // max timeout is 10 minutes (600 seconds) static uint32_t motor_test_start_ms; // system time the motor test began @@ -84,7 +82,7 @@ void Copter::motor_test_output() } // sanity check throttle values - if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) { + if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { // turn on motor to specified pwm value motors->output_test_seq(motor_test_seq, pwm); } else { From c26ffed47ffe57fbcb2f262d4e4880a0d8cee297 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:33 +0000 Subject: [PATCH 0006/3101] Plane: motor_test: use PWM min and max from RC_Channel --- ArduPlane/motor_test.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp index 8774aa18336..cfc640cab98 100644 --- a/ArduPlane/motor_test.cpp +++ b/ArduPlane/motor_test.cpp @@ -7,8 +7,6 @@ */ // motor test definitions -#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test -#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test #define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds // motor_test_output - checks for timeout and sends updates to motors objects @@ -68,7 +66,7 @@ void QuadPlane::motor_test_output() } // sanity check throttle values - if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) { + if (pwm >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) { // turn on motor to specified pwm vlaue motors->output_test_seq(motor_test.seq, pwm); } else { From a47445bde9db964734b3dea8fc580b3fce113f3c Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 15 Feb 2022 00:35:51 +0000 Subject: [PATCH 0007/3101] RC_Channel: update RC_MIN_LIMIT_PWM from 900 to 800 --- libraries/RC_Channel/RC_Channel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 0c2df648aaf..bec21d8c18f 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -274,7 +274,7 @@ class RC_Channel { const char *string_for_aux_function(AUX_FUNC function) const; #endif // pwm value under which we consider that Radio value is invalid - static const uint16_t RC_MIN_LIMIT_PWM = 900; + static const uint16_t RC_MIN_LIMIT_PWM = 800; // pwm value above which we consider that Radio value is invalid static const uint16_t RC_MAX_LIMIT_PWM = 2200; From 41a5afdf6434d147b433e77cb161e488f6a76976 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sat, 19 Feb 2022 22:18:10 +0530 Subject: [PATCH 0008/3101] AP_Common: use default location alt frame while sanitizing location alt We should also set alt frame along with copying altitude value while sanitizing it --- libraries/AP_Common/Location.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 92a86ebc235..4ae650312f0 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -355,9 +355,11 @@ bool Location::sanitize(const Location &defaultLoc) // convert relative alt=0 to mean current alt if (alt == 0 && relative_alt) { - relative_alt = false; - alt = defaultLoc.alt; - has_changed = true; + int32_t defaultLoc_alt; + if (defaultLoc.get_alt_cm(get_alt_frame(), defaultLoc_alt)) { + alt = defaultLoc_alt; + has_changed = true; + } } // limit lat/lng to appropriate ranges From 560e24385af47ef4cd027022820862c7f9c31ac6 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 20 Feb 2022 09:48:37 +0530 Subject: [PATCH 0009/3101] AP_Common: fix location sanitize unit test --- libraries/AP_Common/tests/test_location.cpp | 31 +++++++++++++-------- 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index fddbe3804e3..399806b9e65 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -317,24 +317,31 @@ TEST(Location, Distance) TEST(Location, Sanitize) { + // we will sanitize test_loc with test_default_loc + // test_home is just for reference const Location test_home{-35362938, 149165085, 100, Location::AltFrame::ABSOLUTE}; + EXPECT_TRUE(vehicle.ahrs.set_home(test_home)); + const Location test_default_loc{-35362938, 149165085, 200, Location::AltFrame::ABSOLUTE}; Location test_loc; test_loc.set_alt_cm(0, Location::AltFrame::ABOVE_HOME); - EXPECT_TRUE(test_loc.sanitize(test_home)); - EXPECT_TRUE(test_loc.same_latlon_as(test_home)); - EXPECT_EQ(test_home.alt, test_loc.alt); + EXPECT_TRUE(test_loc.sanitize(test_default_loc)); + EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc)); + int32_t default_loc_alt; + // we should compare test_loc alt and test_default_loc alt in same frame , in this case, ABOVE HOME + EXPECT_TRUE(test_default_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, default_loc_alt)); + EXPECT_EQ(test_loc.alt, default_loc_alt); test_loc = Location(91*1e7, 0, 0, Location::AltFrame::ABSOLUTE); - EXPECT_TRUE(test_loc.sanitize(test_home)); - EXPECT_TRUE(test_loc.same_latlon_as(test_home)); - EXPECT_NE(test_home.alt, test_loc.alt); + EXPECT_TRUE(test_loc.sanitize(test_default_loc)); + EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc)); + EXPECT_NE(test_default_loc.alt, test_loc.alt); test_loc = Location(0, 181*1e7, 0, Location::AltFrame::ABSOLUTE); - EXPECT_TRUE(test_loc.sanitize(test_home)); - EXPECT_TRUE(test_loc.same_latlon_as(test_home)); - EXPECT_NE(test_home.alt, test_loc.alt); + EXPECT_TRUE(test_loc.sanitize(test_default_loc)); + EXPECT_TRUE(test_loc.same_latlon_as(test_default_loc)); + EXPECT_NE(test_default_loc.alt, test_loc.alt); test_loc = Location(42*1e7, 42*1e7, 420, Location::AltFrame::ABSOLUTE); - EXPECT_FALSE(test_loc.sanitize(test_home)); - EXPECT_FALSE(test_loc.same_latlon_as(test_home)); - EXPECT_NE(test_home.alt, test_loc.alt); + EXPECT_FALSE(test_loc.sanitize(test_default_loc)); + EXPECT_FALSE(test_loc.same_latlon_as(test_default_loc)); + EXPECT_NE(test_default_loc.alt, test_loc.alt); } TEST(Location, Line) From 80f8f8b14e32b3bfb9241a872f1cfb5c5a48d825 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 19 Feb 2022 22:21:52 +0000 Subject: [PATCH 0010/3101] Plane: quadplane: double log QPOS state change --- ArduPlane/quadplane.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 60ce00ce941..f405fc2c8bc 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2139,6 +2139,7 @@ void QuadPlane::log_QPOS(void) */ void QuadPlane::PosControlState::set_state(enum position_control_state s) { + const uint32_t now = AP_HAL::millis(); if (state != s) { auto &qp = plane.quadplane; pilot_correction_done = false; @@ -2163,14 +2164,17 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // reset throttle descent control qp.thr_ctrl_land = false; } + // double log to capture the state change + qp.log_QPOS(); + state = s; qp.log_QPOS(); + last_log_ms = now; } - state = s; - last_state_change_ms = AP_HAL::millis(); + last_state_change_ms = now; // we consider setting the state to be equivalent to running to // prevent code from overriding the state as stale - last_run_ms = last_state_change_ms; + last_run_ms = now; } /* From 75045057b759e90ebbe1f30c5db5d8188129eaac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:10 +1100 Subject: [PATCH 0011/3101] AntennaTracker: add RebootRequred to stream rate parameters --- AntennaTracker/GCS_Mavlink.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 62ca06772fe..7d176865643 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -179,6 +179,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 1), @@ -188,6 +189,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 1), @@ -197,6 +199,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 1), @@ -206,6 +209,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 1), @@ -215,6 +219,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1), @@ -224,6 +229,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1), @@ -233,6 +239,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1), @@ -242,6 +249,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1), @@ -251,6 +259,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10), AP_GROUPEND From c80cd1daa57793d2cee55a42e5b65126425ffd08 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:11 +1100 Subject: [PATCH 0012/3101] ArduCopter: add RebootRequred to stream rate parameters --- ArduCopter/GCS_Mavlink.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 12d88004a78..b02b1be8791 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -380,6 +380,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 0), @@ -389,6 +390,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 0), @@ -398,6 +400,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0), @@ -407,6 +410,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 0), @@ -416,6 +420,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 0), @@ -425,6 +430,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 0), @@ -434,6 +440,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 0), @@ -443,6 +450,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 0), @@ -452,6 +460,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 0), @@ -461,6 +470,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 0), AP_GROUPEND From 452a5df0cc4235d45555758e062a79001f7e38d2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:11 +1100 Subject: [PATCH 0013/3101] ArduPlane: add RebootRequred to stream rate parameters --- ArduPlane/GCS_Mavlink.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 591ec224de5..948b0b49483 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -428,6 +428,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 1), @@ -437,6 +438,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 1), @@ -446,6 +448,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 1), @@ -455,6 +458,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 1), @@ -464,6 +468,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1), @@ -473,6 +478,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1), @@ -482,6 +488,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1), @@ -491,6 +498,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1), @@ -500,6 +508,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10), @@ -509,6 +518,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 5), AP_GROUPEND From c33590a63baa0a38932da1a3e2bd6b27a0d9b753 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:11 +1100 Subject: [PATCH 0014/3101] ArduSub: add RebootRequred to stream rate parameters --- ArduSub/GCS_Mavlink.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index d3976fb84f3..0f645a8ca9b 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -255,6 +255,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RAW_SENSORS], 0), @@ -264,6 +265,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTENDED_STATUS], 0), @@ -273,6 +275,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_RC_CHANNELS], 0), @@ -282,6 +285,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_POSITION], 0), @@ -291,6 +295,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA1], 0), @@ -300,6 +305,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA2], 0), @@ -309,6 +315,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_EXTRA3], 0), @@ -318,6 +325,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[GCS_MAVLINK::STREAM_PARAMS], 0), AP_GROUPEND From 5ae96b46563c86bd74e9ca249baa71a5ed7a0387 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:11 +1100 Subject: [PATCH 0015/3101] Blimp: add RebootRequred to stream rate parameters --- Blimp/GCS_Mavlink.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 30ae53ed830..0f29d59ca57 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -217,6 +217,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 0), @@ -226,6 +227,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 0), @@ -235,6 +237,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 0), @@ -244,6 +247,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 0), @@ -253,6 +257,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 0), @@ -262,6 +267,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 0), @@ -271,6 +277,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 0), @@ -280,6 +287,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 0), @@ -289,6 +297,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 10 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 0), AP_GROUPEND From bb44cca7edfea3ec86aeecc90487d84f03e1105c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 20 Feb 2022 12:08:12 +1100 Subject: [PATCH 0016/3101] Rover: add RebootRequred to stream rate parameters --- Rover/GCS_Mavlink.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index af1dc6a4e57..35790deb80e 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -384,6 +384,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 1), @@ -393,6 +394,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 1), @@ -402,6 +404,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 1), @@ -411,6 +414,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 1), @@ -420,6 +424,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1), @@ -429,6 +434,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1), @@ -438,6 +444,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1), @@ -447,6 +454,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1), @@ -456,6 +464,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10), @@ -465,6 +474,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Units: Hz // @Range: 0 50 // @Increment: 1 + // @RebootRequired: True // @User: Advanced AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 0), From 0c32e71272ec71d55c1b0411abcae5acab285269 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 26 Feb 2022 07:40:12 -0600 Subject: [PATCH 0017/3101] ArduPlane: clarify and simplify RC failsafe messages --- ArduPlane/events.cpp | 20 +++++++++++++++----- ArduPlane/radio.cpp | 4 ++-- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index e0a420a4857..3b900f4a358 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -21,7 +21,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso failsafe.state = fstype; failsafe.short_timer_ms = millis(); failsafe.saved_mode_number = control_mode->mode_number(); - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, static_cast(reason)); + gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On"); switch (control_mode->mode_number()) { case Mode::Number::MANUAL: @@ -102,7 +102,12 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason) { // This is how to handle a long loss of control signal failsafe. - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, static_cast(reason)); + if (reason == ModeReason:: GCS_FAILSAFE) { + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe On"); + } + else { + gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe On"); + } // If the GCS is locked up we allow control to revert to RC RC_Channels::clear_overrides(); failsafe.state = fstype; @@ -186,7 +191,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason void Plane::failsafe_short_off_event(ModeReason reason) { // We're back in radio contact - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast(reason)); + gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared"); failsafe.state = FAILSAFE_NONE; //restore entry mode if desired but check that our current mode is still due to failsafe if ( _last_reason == ModeReason::RADIO_FAILSAFE) { @@ -197,8 +202,13 @@ void Plane::failsafe_short_off_event(ModeReason reason) void Plane::failsafe_long_off_event(ModeReason reason) { - // We're back in radio contact - gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", static_cast(reason)); + // We're back in radio contact with RC or GCS + if (reason == ModeReason:: GCS_FAILSAFE) { + gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off"); + } + else { + gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe Cleared"); + } failsafe.state = FAILSAFE_NONE; } diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 5e534f66539..eb5ea21dd2e 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -280,7 +280,7 @@ void Plane::control_failsafe() // throttle has dropped below the mark failsafe.throttle_counter++; if (failsafe.throttle_counter == 10) { - gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on"); + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "on"); failsafe.rc_failsafe = true; AP_Notify::flags.failsafe_radio = true; } @@ -295,7 +295,7 @@ void Plane::control_failsafe() failsafe.throttle_counter = 3; } if (failsafe.throttle_counter == 1) { - gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off"); + gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "off"); } else if(failsafe.throttle_counter == 0) { failsafe.rc_failsafe = false; AP_Notify::flags.failsafe_radio = false; From b3af8221be8a055a71cb9a17d363f1c7e9128295 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 26 Feb 2022 07:40:12 -0600 Subject: [PATCH 0018/3101] Tools: clarify and simplify RC failsafe messages --- Tools/autotest/arduplane.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2ddc9fa0a58..1668fc401bc 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1000,11 +1000,11 @@ def ThrottleFailsafe(self): "FS_SHORT_ACTN": 3, # 3 means disabled "SIM_RC_FAIL": 1, }) - self.wait_statustext("Long event on", check_context=True) + self.wait_statustext("Long failsafe on", check_context=True) self.wait_mode("RTL") # self.context_clear_collection("STATUSTEXT") self.set_parameter("SIM_RC_FAIL", 0) - self.wait_text("Long event off", check_context=True) + self.wait_text("Long Failsafe Cleared", check_context=True) self.change_mode("MANUAL") self.progress("Trying again with THR_FS_VALUE") @@ -1012,7 +1012,7 @@ def ThrottleFailsafe(self): "THR_FS_VALUE": 960, "SIM_RC_FAIL": 2, }) - self.wait_statustext("Long event on", check_context=True) + self.wait_statustext("Long Failsafe on", check_context=True) self.wait_mode("RTL") except Exception as e: self.print_exception_caught(e) From fdaaa589e5e18277ad6930c9ed2a169d348b50bb Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Feb 2022 20:13:38 +0000 Subject: [PATCH 0019/3101] waf: move external flash binaries to regular name to aid publishing --- Tools/ardupilotwaf/chibios.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 7143ac1ae36..f6c49019298 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -266,6 +266,18 @@ def keyword(self): def __str__(self): return self.outputs[0].path_from(self.generator.bld.bldnode) +class build_normalized_bins(Task.Task): + '''Move external flash binaries to regular location if regular bin is zero length''' + color='CYAN' + always_run = True + def run(self): + if self.env.HAS_EXTERNAL_FLASH_SECTIONS and os.path.getsize(self.inputs[0].abspath()) == 0: + os.remove(self.inputs[0].abspath()) + shutil.move(self.inputs[1].abspath(), self.inputs[0].abspath()) + + def keyword(self): + return "bin cleanup" + class build_intel_hex(Task.Task): '''build an intel hex file for upload with DFU''' color='CYAN' @@ -302,12 +314,15 @@ def chibios_firmware(self): abin_task = self.create_task('build_abin', src=link_output, tgt=abin_target) abin_task.set_run_after(generate_apj_task) + cleanup_task = self.create_task('build_normalized_bins', src=bin_target) + cleanup_task.set_run_after(generate_apj_task) + bootloader_bin = self.bld.srcnode.make_node("Tools/bootloaders/%s_bl.bin" % self.env.BOARD) if self.bld.env.HAVE_INTEL_HEX: if os.path.exists(bootloader_bin.abspath()): hex_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.hex').name) hex_task = self.create_task('build_intel_hex', src=[bin_target[0], bootloader_bin], tgt=hex_target) - hex_task.set_run_after(generate_bin_task) + hex_task.set_run_after(cleanup_task) else: print("Not embedding bootloader; %s does not exist" % bootloader_bin) From 2398d16a5f4bdf0941c9bdf403e4853b26206bbc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:26 +1100 Subject: [PATCH 0020/3101] AC_Fence: include cleanups --- libraries/AC_Fence/AC_Fence.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index dd13c8ab579..7aead1c0e79 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -4,9 +4,7 @@ #include #include #include -#include #include -#include // bit masks for enabled fence types. Used for TYPE parameter #define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL @@ -105,7 +103,7 @@ class AC_Fence uint8_t check(); // returns true if the destination is within fence (used to reject waypoints outside the fence) - bool check_destination_within_fence(const Location& loc); + bool check_destination_within_fence(const class Location& loc); /// get_breaches - returns bit mask of the fence types that have been breached uint8_t get_breaches() const { return _breached_fences; } From 2ab2555e0bd046be012fb9e783de38e31a4c3f0f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:26 +1100 Subject: [PATCH 0021/3101] AC_PrecLand: include cleanups --- libraries/AC_PrecLand/AC_PrecLand_StateMachine.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h index 7993202dbe4..b72b0e8d72e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h @@ -1,7 +1,6 @@ #pragma once #include -#include // This class constantly monitors what the status of the landing target is // If it is not in sight, depending on user parameters, it decides what measures can be taken to bring the target back in sight @@ -17,8 +16,7 @@ class AC_PrecLand_StateMachine { }; // Do not allow copies - AC_PrecLand_StateMachine(const AC_PrecLand_StateMachine &other) = delete; - AC_PrecLand_StateMachine &operator=(const AC_PrecLand_StateMachine&) = delete; + CLASS_NO_COPY(AC_PrecLand_StateMachine); // Initialize the state machine. This is called everytime vehicle switches mode void init(); From 24099f9a2dfd0cbb7605f4b5ee8d1c88540840da Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:27 +1100 Subject: [PATCH 0022/3101] AP_AHRS: include cleanups --- libraries/AP_AHRS/AP_AHRS.cpp | 2 ++ libraries/AP_AHRS/AP_AHRS.h | 6 +----- libraries/AP_AHRS/AP_AHRS_Backend.cpp | 3 +++ libraries/AP_AHRS/AP_AHRS_Backend.h | 7 ++----- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 1 + 5 files changed, 9 insertions(+), 10 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index ab414a545e4..39932b3bcde 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -22,9 +22,11 @@ #include "AP_AHRS.h" #include "AP_AHRS_View.h" #include +#include #include #include #include +#include #include #include #include diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e2d6cd08636..f62b2c2cf99 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -16,7 +16,7 @@ */ /* - * NavEKF based AHRS (Attitude Heading Reference System) interface for + * AHRS (Attitude Heading Reference System) frontend interface for * ArduPilot * */ @@ -36,14 +36,10 @@ #define AP_AHRS_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif -#include "AP_AHRS.h" - #if AP_AHRS_SIM_ENABLED #include #endif -#include - #include #include #include // definitions shared by inertial and ekf nav filters diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 4f95e988d7b..202605c955c 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -16,10 +16,13 @@ */ #include "AP_AHRS.h" #include "AP_AHRS_View.h" + +#include #include #include #include #include +#include extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index a39f66ba45f..cc59c553772 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -22,11 +22,8 @@ #include #include -#include #include #include -#include -#include class OpticalFlow; #define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees @@ -204,10 +201,10 @@ class AP_AHRS_Backend } // - virtual bool set_origin(const Location &loc) { + virtual bool set_origin(const struct Location &loc) { return false; } - virtual bool get_origin(Location &ret) const = 0; + virtual bool get_origin(struct Location &ret) const = 0; // return a position relative to origin in meters, North/East/Down // order. This will only be accurate if have_inertial_nav() is diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 0cd43a84119..7ffd3c75456 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include extern const AP_HAL::HAL& hal; From 7ac33af97bff985c4def2eb458f0354fcb6723f1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:27 +1100 Subject: [PATCH 0023/3101] AP_Baro: include cleanups --- libraries/AP_Baro/AP_Baro.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 27e61467170..c4f35520283 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -2,7 +2,6 @@ #include #include -#include #include #include #include From 824ced4b3db27e7791b4ffe9e369cebd36fff116 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:27 +1100 Subject: [PATCH 0024/3101] AP_Button: include cleanups --- libraries/AP_Button/AP_Button.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Button/AP_Button.cpp b/libraries/AP_Button/AP_Button.cpp index 2b993f40b5a..38cf84f23c2 100644 --- a/libraries/AP_Button/AP_Button.cpp +++ b/libraries/AP_Button/AP_Button.cpp @@ -20,6 +20,7 @@ #include #include +#include // very crude debounce method #define DEBOUNCE_MS 50 From 1b970545c084d02445063af23959101fa3a4e847 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:27 +1100 Subject: [PATCH 0025/3101] AP_Camera: include cleanups --- libraries/AP_Camera/AP_Camera.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 64f070477ae..1acb74c2fe6 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -3,7 +3,7 @@ #pragma once #include -#include +#include #include #define AP_CAMERA_TRIGGER_DEFAULT_DURATION 10 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) From 7d3f3b68886727d0e19e2eda57212f0a69e5b55c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:27 +1100 Subject: [PATCH 0026/3101] AP_Compass: include cleanups --- libraries/AP_Compass/AP_Compass_Backend.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 95b5976a18f..8636f982585 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -19,12 +19,13 @@ */ #pragma once -#include "AP_Compass.h" - +#include #ifndef HAL_MSP_COMPASS_ENABLED #define HAL_MSP_COMPASS_ENABLED HAL_MSP_SENSORS_ENABLED #endif +#include "AP_Compass.h" + class Compass; // forward declaration class AP_Compass_Backend { From f9b33317b6d1d27fb8cf5ba4c09b47165d296897 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:28 +1100 Subject: [PATCH 0027/3101] AP_EFI: include cleanups --- libraries/AP_EFI/AP_EFI.h | 4 +--- libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp | 5 ++++- libraries/AP_EFI/AP_EFI_Serial_MS.cpp | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/AP_EFI/AP_EFI.h b/libraries/AP_EFI/AP_EFI.h index d5ceadaeb52..fb5eef065cf 100644 --- a/libraries/AP_EFI/AP_EFI.h +++ b/libraries/AP_EFI/AP_EFI.h @@ -15,11 +15,9 @@ #pragma once - - #include #include -#include +#include #ifndef HAL_EFI_ENABLED #define HAL_EFI_ENABLED !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 diff --git a/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp index 1d0141d6bef..459711ea2d2 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_Lutan.cpp @@ -16,9 +16,12 @@ #include #include "AP_EFI_Serial_Lutan.h" #include -#include #if HAL_EFI_ENABLED + +#include + +#include #include extern const AP_HAL::HAL &hal; diff --git a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp index f5820fa272e..753c8e1c710 100644 --- a/libraries/AP_EFI/AP_EFI_Serial_MS.cpp +++ b/libraries/AP_EFI/AP_EFI_Serial_MS.cpp @@ -17,6 +17,7 @@ #include "AP_EFI_Serial_MS.h" #if HAL_EFI_ENABLED +#include #include extern const AP_HAL::HAL &hal; From 45c6591f0369455967d3c9a998e9ca5eda46371e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:28 +1100 Subject: [PATCH 0028/3101] AP_Follow: include cleanups --- libraries/AP_Follow/AP_Follow.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index fc5bc8eba6d..a7b7f0d6701 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -15,10 +15,11 @@ #pragma once #include +#include #include #include #include -#include +#include #include #include From 88b8a7d64ef5015408e8109d95c9a4b922b73024 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:28 +1100 Subject: [PATCH 0029/3101] AP_Generator: include cleanups --- libraries/AP_Generator/AP_Generator.cpp | 2 ++ libraries/AP_Generator/AP_Generator.h | 3 +-- libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp | 4 +++- libraries/AP_Generator/AP_Generator_IE_FuelCell.h | 3 --- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Generator/AP_Generator.cpp b/libraries/AP_Generator/AP_Generator.cpp index 78309a80e98..4c735d7c656 100644 --- a/libraries/AP_Generator/AP_Generator.cpp +++ b/libraries/AP_Generator/AP_Generator.cpp @@ -21,6 +21,8 @@ #include "AP_Generator_IE_2400.h" #include "AP_Generator_RichenPower.h" +#include + const AP_Param::GroupInfo AP_Generator::var_info[] = { // @Param: TYPE diff --git a/libraries/AP_Generator/AP_Generator.h b/libraries/AP_Generator/AP_Generator.h index 9e5a269e59e..e4b44d4dd33 100644 --- a/libraries/AP_Generator/AP_Generator.h +++ b/libraries/AP_Generator/AP_Generator.h @@ -9,7 +9,6 @@ #if HAL_GENERATOR_ENABLED #include -#include #include class AP_Generator_Backend; @@ -62,7 +61,7 @@ class AP_Generator bool idle(void); bool run(void); - void send_generator_status(const GCS_MAVLINK &channel); + void send_generator_status(const class GCS_MAVLINK &channel); // Parameter block static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index 24eb6605dd0..b1dfddfdcf6 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -14,10 +14,12 @@ */ #include "AP_Generator_IE_FuelCell.h" -#include #if HAL_GENERATOR_ENABLED +#include +#include + // Initialize the fuelcell object and prepare it for use void AP_Generator_IE_FuelCell::init() { diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h index 7ba662ec214..a1666adbe29 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -4,9 +4,6 @@ #if HAL_GENERATOR_ENABLED -#include -#include - class AP_Generator_IE_FuelCell : public AP_Generator_Backend { From 8d6e4429979b7229ec30eeeb9fcacd427eee4fd1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:28 +1100 Subject: [PATCH 0030/3101] AP_GPS: include cleanups --- libraries/AP_GPS/AP_GPS.cpp | 1 + libraries/AP_GPS/AP_GPS.h | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c93206719c0..3c9ede363e2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include "AP_GPS_NOVA.h" #include "AP_GPS_ERB.h" diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 3bca1e13c98..89411ac9b57 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -20,7 +20,6 @@ #include #include #include "GPS_detect_state.h" -#include #include #include @@ -219,7 +218,7 @@ class AP_GPS }; /// Startup initialisation. - void init(const AP_SerialManager& serial_manager); + void init(const class AP_SerialManager& serial_manager); /// Update GPS state based on possible bytes received from the module. /// This routine must be called periodically (typically at 10Hz or From 677466e60b7dd6d0ac6c64bb969b4fd834ba26d6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:29 +1100 Subject: [PATCH 0031/3101] AP_ICEngine: include cleanups --- libraries/AP_ICEngine/AP_ICEngine.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 7600fdb2b1b..de05647a08e 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -19,6 +19,8 @@ #include #include #include +#include + #include "AP_ICEngine.h" extern const AP_HAL::HAL& hal; From fda4a65e3eb29de837641e8973b637a5551b5e1d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:29 +1100 Subject: [PATCH 0032/3101] AP_InertialSensor: include cleanups --- libraries/AP_InertialSensor/AP_InertialSensor_NONE.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.h b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.h index effe33920f1..54a82d6f7d9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.h @@ -1,8 +1,5 @@ #pragma once -#include -#include - /* This is a 'mock' implementation of an INS that does nothing and gives a level HUD, but does it successfully. Its useful for boards that don't have any form of IMU accel/gyro etc connected just yet, but where u want to boot-up "successfully" anyway, From 5d35d8a02c33d3584beaf1e3f61e735e811b859d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:29 +1100 Subject: [PATCH 0033/3101] AP_Motors: include cleanups --- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 1 - libraries/AP_Motors/AP_MotorsTri.cpp | 3 +++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index c568413caa9..eb4bc823444 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -4,7 +4,6 @@ #include // ArduPilot Mega Vector/Matrix math Library #include #include -#include // default main rotor speed (ch8 out) as a number from 0 ~ 100 #define AP_MOTORS_HELI_RSC_SETPOINT 70 diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 9641cda727b..1fffd7bec4b 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -14,8 +14,11 @@ */ #include +#include + #include #include + #include "AP_MotorsTri.h" extern const AP_HAL::HAL& hal; From 7a0c3dc911e1a410e1f9c03a5237a16b89399eb3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:29 +1100 Subject: [PATCH 0034/3101] AP_MSP: include cleanups --- libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index cda9fc86ecf..a2fc596a9c2 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include From 5f2e0159b041f58203e442604b0eb94d14296d75 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:29 +1100 Subject: [PATCH 0035/3101] AP_Radio: include cleanups --- libraries/AP_Radio/AP_Radio.h | 3 +-- libraries/AP_Radio/AP_Radio_backend.h | 2 ++ libraries/AP_Radio/AP_Radio_cc2500.cpp | 2 +- libraries/AP_Radio/AP_Radio_cypress.cpp | 2 +- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Radio/AP_Radio.h b/libraries/AP_Radio/AP_Radio.h index 231f11504fc..0095457de26 100644 --- a/libraries/AP_Radio/AP_Radio.h +++ b/libraries/AP_Radio/AP_Radio.h @@ -18,9 +18,8 @@ * base class for direct attached radios */ -#include #include -#include +#include class AP_Radio_backend; diff --git a/libraries/AP_Radio/AP_Radio_backend.h b/libraries/AP_Radio/AP_Radio_backend.h index e6c3bda92b6..a1fd9e168e8 100644 --- a/libraries/AP_Radio/AP_Radio_backend.h +++ b/libraries/AP_Radio/AP_Radio_backend.h @@ -21,6 +21,8 @@ #include #include "AP_Radio.h" +#include + class AP_Radio_backend { public: diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index 999ed188f7d..a4e43744a4c 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include #include #include diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index a9f08f7375b..a66de153587 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -11,7 +11,7 @@ #include #include "telem_structure.h" #include -#include +#include /* driver for CYRF6936 radio From ade6b3ddfea4319aaca169cbd51e0bbc6be0e9b5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:30 +1100 Subject: [PATCH 0036/3101] AP_RangeFinder: include cleanups --- libraries/AP_RangeFinder/AP_RangeFinder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 7474c4cadfa..4eba3c2cc1d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include #include "AP_RangeFinder_Params.h" From 546961eff4433914485e914ea639a954b00ee34b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:30 +1100 Subject: [PATCH 0037/3101] AP_Vehicle: include cleanups --- libraries/AP_Vehicle/AP_Vehicle.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index a93746e2320..2d0b0bf7861 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -25,6 +25,7 @@ #include // board configuration library #include #include +#include #include #include #include From 7a19719a5ea8eb3c18fdbdde45162b75e60efd70 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:30 +1100 Subject: [PATCH 0038/3101] AP_Volz_Protocol: include cleanups --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp | 3 +++ libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 6 +----- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 5fb5b6c69a9..e0084e47981 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -9,6 +9,9 @@ #include "AP_Volz_Protocol.h" #if NUM_SERVO_CHANNELS + +#include + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_Volz_Protocol::var_info[] = { diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index c654ff4fb7f..adbf5edf13f 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -34,11 +34,8 @@ #pragma once #include -#include #include -//#include - #define VOLZ_SCALE_VALUE (uint16_t)(VOLZ_EXTENDED_POSITION_MAX - VOLZ_EXTENDED_POSITION_MIN) // Extended Position Data Format defines 100 as 0x0F80, which results in 1920 steps for +100 deg and 1920 steps for -100 degs meaning if you take movement a scaled between -1 ... 1 and multiply by 1920 you get the travel from center #define VOLZ_SET_EXTENDED_POSITION_CMD 0xDC #define VOLZ_SET_EXTENDED_POSITION_RSP 0x2C @@ -56,8 +53,7 @@ class AP_Volz_Protocol { AP_Volz_Protocol(); /* Do not allow copies */ - AP_Volz_Protocol(const AP_Volz_Protocol &other) = delete; - AP_Volz_Protocol &operator=(const AP_Volz_Protocol&) = delete; + CLASS_NO_COPY(AP_Volz_Protocol); static const struct AP_Param::GroupInfo var_info[]; From e93be25ed4d9f43337a4dfa53a5c3dbb38087d2f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:30 +1100 Subject: [PATCH 0039/3101] AP_WindVane: include cleanups --- libraries/AP_WindVane/AP_WindVane.cpp | 6 ++++++ libraries/AP_WindVane/AP_WindVane.h | 8 ++++---- libraries/AP_WindVane/AP_WindVane_Analog.cpp | 5 +++++ libraries/AP_WindVane/AP_WindVane_Analog.h | 7 +------ libraries/AP_WindVane/AP_WindVane_Backend.cpp | 2 ++ libraries/AP_WindVane/AP_WindVane_Backend.h | 3 --- libraries/AP_WindVane/AP_WindVane_Home.cpp | 2 ++ libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp | 2 ++ libraries/AP_WindVane/AP_WindVane_ModernDevice.h | 1 - libraries/AP_WindVane/AP_WindVane_SITL.cpp | 3 +++ 10 files changed, 25 insertions(+), 14 deletions(-) diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index dba378495cc..186394a5e45 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -23,7 +23,9 @@ #include "AP_WindVane_SITL.h" #include "AP_WindVane_NMEA.h" +#include #include +#include const AP_Param::GroupInfo AP_WindVane::var_info[] = { @@ -369,6 +371,10 @@ void AP_WindVane::update() } +void AP_WindVane::record_home_heading() +{ + _home_heading = AP::ahrs().yaw; +} // to start direction calibration from mavlink or other bool AP_WindVane::start_direction_calibration() diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index c4cbfaae129..ebc9adb4768 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -15,8 +15,8 @@ #pragma once #include -#include -#include +#include +#include #ifndef WINDVANE_DEFAULT_PIN #define WINDVANE_DEFAULT_PIN -1 // default wind vane sensor analog pin @@ -58,7 +58,7 @@ class AP_WindVane bool wind_speed_enabled() const; // Initialize the Wind Vane object and prepare it for use - void init(const AP_SerialManager& serial_manager); + void init(const class AP_SerialManager& serial_manager); // update wind vane void update(); @@ -88,7 +88,7 @@ class AP_WindVane Sailboat_Tack get_current_tack() const { return _current_tack; } // record home heading for use as wind direction if no sensor is fitted - void record_home_heading() { _home_heading = AP::ahrs().yaw; } + void record_home_heading(); // start calibration routine bool start_direction_calibration(); diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.cpp b/libraries/AP_WindVane/AP_WindVane_Analog.cpp index ecf38c5dad8..6e78c8c5615 100644 --- a/libraries/AP_WindVane/AP_WindVane_Analog.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Analog.cpp @@ -15,6 +15,11 @@ #include "AP_WindVane_Analog.h" +#include +#include + +extern const AP_HAL::HAL& hal; + #define WINDVANE_CALIBRATION_VOLT_DIFF_MIN 1.0f // calibration routine's min voltage difference required for success // constructor diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.h b/libraries/AP_WindVane/AP_WindVane_Analog.h index eed943835c3..017266b0be9 100644 --- a/libraries/AP_WindVane/AP_WindVane_Analog.h +++ b/libraries/AP_WindVane/AP_WindVane_Analog.h @@ -16,11 +16,6 @@ #include "AP_WindVane_Backend.h" -#include -#include - -extern const AP_HAL::HAL& hal; - class AP_WindVane_Analog : public AP_WindVane_Backend { public: @@ -33,7 +28,7 @@ class AP_WindVane_Analog : public AP_WindVane_Backend private: // pin for reading analog voltage - AP_HAL::AnalogSource *_dir_analog_source; + class AP_HAL::AnalogSource *_dir_analog_source; float _current_analog_voltage; uint32_t _cal_start_ms = 0; diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp index 02e041a2526..39f9ac9ed79 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Backend.cpp @@ -16,6 +16,8 @@ #include "AP_WindVane.h" #include "AP_WindVane_Backend.h" +#include + // base class constructor. AP_WindVane_Backend::AP_WindVane_Backend(AP_WindVane &frontend) : _frontend(frontend) diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h index c2319acfba7..e347de49723 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.h +++ b/libraries/AP_WindVane/AP_WindVane_Backend.h @@ -16,9 +16,6 @@ #include "AP_WindVane.h" -#include -#include - class AP_WindVane_Backend { public: diff --git a/libraries/AP_WindVane/AP_WindVane_Home.cpp b/libraries/AP_WindVane/AP_WindVane_Home.cpp index 64ed706402b..042d4ca0c18 100644 --- a/libraries/AP_WindVane/AP_WindVane_Home.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Home.cpp @@ -15,6 +15,8 @@ #include "AP_WindVane_Home.h" +#include + void AP_WindVane_Home::update_direction() { float direction_apparent_ef = _frontend._home_heading; diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp index 402f3b9f079..e273601a436 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp @@ -17,6 +17,8 @@ // read wind speed from Modern Device rev p wind sensor // https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/ +#include + // constructor AP_WindVane_ModernDevice::AP_WindVane_ModernDevice(AP_WindVane &frontend) : AP_WindVane_Backend(frontend) diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h index 66b2e8c5c20..bb05cb215a1 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h @@ -16,7 +16,6 @@ #include "AP_WindVane_Backend.h" -#include #include extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp index 936ad2c63e2..c853185b97f 100644 --- a/libraries/AP_WindVane/AP_WindVane_SITL.cpp +++ b/libraries/AP_WindVane/AP_WindVane_SITL.cpp @@ -17,6 +17,9 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#include + void AP_WindVane_SITL::update_direction() { if (_frontend._direction_type == _frontend.WindVaneType::WINDVANE_SITL_TRUE) { From 7638cbf00170b9cc92da20f6fc3203571dfca9f4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:31 +1100 Subject: [PATCH 0040/3101] GCS_MAVLink: include cleanups --- libraries/GCS_MAVLink/GCS.cpp | 6 ++++++ libraries/GCS_MAVLink/GCS.h | 19 ++++++------------- libraries/GCS_MAVLink/GCS_Common.cpp | 8 ++++++++ .../GCS_MAVLink/MissionItemProtocol_Fence.cpp | 2 ++ 4 files changed, 22 insertions(+), 13 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 2495ed40acf..f25883b5f2c 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -7,8 +7,14 @@ #include #include #include +#include #include #include +#include + +#include "MissionItemProtocol_Waypoints.h" +#include "MissionItemProtocol_Rally.h" +#include "MissionItemProtocol_Fence.h" extern const AP_HAL::HAL& hal; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index bdbf31acc00..487aa232ecc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -17,21 +17,14 @@ #include #include #include "MAVLink_routing.h" -#include -#include #include #include #include #include -#include #include #include -#include #include -#include "MissionItemProtocol_Waypoints.h" -#include "MissionItemProtocol_Rally.h" -#include "MissionItemProtocol_Fence.h" #include "ap_message.h" #define GCS_DEBUG_SEND_MESSAGE_TIMINGS 0 @@ -611,7 +604,7 @@ class GCS_MAVLINK static constexpr const float magic_force_arm_value = 2989.0f; static constexpr const float magic_force_disarm_value = 21196.0f; - void manual_override(RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false); + void manual_override(class RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false); uint8_t receiver_rssi() const; @@ -1051,10 +1044,10 @@ class GCS ap_var_type param_type, float param_value); - static MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints; - static MissionItemProtocol_Rally *_missionitemprotocol_rally; - static MissionItemProtocol_Fence *_missionitemprotocol_fence; - MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const; + static class MissionItemProtocol_Waypoints *_missionitemprotocol_waypoints; + static class MissionItemProtocol_Rally *_missionitemprotocol_rally; + static class MissionItemProtocol_Fence *_missionitemprotocol_fence; + class MissionItemProtocol *get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const; void try_send_queued_message_for_type(MAV_MISSION_TYPE type) const; void update_send(); @@ -1075,7 +1068,7 @@ class GCS bool out_of_time() const; // frsky backend - AP_Frsky_Telem *frsky; + class AP_Frsky_Telem *frsky; #if !HAL_MINIMIZE_FEATURES // LTM backend diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 1decefa92b0..80dab24442b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -49,6 +50,13 @@ #include #include #include +#include +#include +#include + +#include "MissionItemProtocol_Waypoints.h" +#include "MissionItemProtocol_Rally.h" +#include "MissionItemProtocol_Fence.h" #include diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp index ea72bdc3bfc..294c28e3872 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -1,6 +1,8 @@ #include "MissionItemProtocol_Fence.h" #include +#include +#include /* public function to format mission item as mavlink_mission_item_int_t From bfb8e7df81e539ce815f02488ec2ceaae3725078 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:31 +1100 Subject: [PATCH 0041/3101] ArduCopter: include cleanups --- ArduCopter/Copter.h | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 48b1ca783f7..f6035075d3a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -34,7 +34,6 @@ #include // library for Management for hal.storage to allow for backwards compatible mapping of storage offsets to available storage // Application dependencies -#include // Library for Interface definition for the various Ground Control System #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Vector/Matrix math Library #include // interface and maths for accelerometer calibration From c0aa20b1d02f04c15458c0cf2938ef5be7313e5c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:31 +1100 Subject: [PATCH 0042/3101] ArduPlane: include cleanups --- ArduPlane/transition.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index 00167800779..30c91d9615b 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -13,7 +13,7 @@ along with this program. If not, see . */ #pragma once -#include +#include class QuadPlane; class AP_MotorsMulticopter; From 0a9481a5576dd18748d7409b38bfef847810a4be Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:31 +1100 Subject: [PATCH 0043/3101] ArduSub: include cleanups --- ArduSub/Sub.h | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 36b1819bcd2..28b021a2932 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -32,7 +32,6 @@ #include #include #include -#include #include // interface and maths for accelerometer calibration #include // ArduPilot Mega Vector/Matrix math Library #include // ArduPilot Mega Declination Helper Library From e277c3bd4967b08576e0ed79e17ebe4d14d72cd9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:06:31 +1100 Subject: [PATCH 0044/3101] Blimp: include cleanups --- Blimp/Blimp.h | 1 - 1 file changed, 1 deletion(-) diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 5aece1b6747..5af0ecf5cfc 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -34,7 +34,6 @@ #include // Application dependencies -#include #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Vector/Matrix math Library // #include // interface and maths for accelerometer calibration From 465f56254a78fcf11f8e96d45dcb961d3581eed3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Feb 2022 16:09:06 +1100 Subject: [PATCH 0045/3101] AP_Scripting: include cleanups --- libraries/AP_Scripting/AP_Scripting.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index 943fd0a0fbc..3b55f3345a9 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -18,7 +18,8 @@ #include #include -#include +#include +#include #include #include #include "AP_Scripting_CANSensor.h" @@ -48,7 +49,7 @@ class AP_Scripting MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet); - void handle_mission_command(const AP_Mission::Mission_Command& cmd); + void handle_mission_command(const class AP_Mission::Mission_Command& cmd); // User parameters for inputs into scripts AP_Float _user[6]; From 1321a66d97d39a9bdfc72f9532a5e3bc3cd705f3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 26 Feb 2022 11:51:07 +1100 Subject: [PATCH 0046/3101] Replay: include cleanups --- Tools/Replay/Replay.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index c42085e5b75..f9a7e3ea21c 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include From 7f747141075a7da329cbc3f932038ff119e0195b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 26 Feb 2022 14:35:21 +1100 Subject: [PATCH 0047/3101] AP_Arming: include cleanups --- libraries/AP_Arming/AP_Arming.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 1646180b53c..af7dc98d1ec 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include From e01ca792512c9177e61a3a881e4588a594a501a7 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 6 Feb 2022 22:07:10 +0530 Subject: [PATCH 0048/3101] AC_WPNav: rename circle_nav.set_radius to circle_nav.set_radius_cm --- libraries/AC_WPNav/AC_Circle.cpp | 2 +- libraries/AC_WPNav/AC_Circle.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 000d8dae586..1b6fd33d635 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -138,7 +138,7 @@ void AC_Circle::set_rate(float deg_per_sec) } /// set_circle_rate - set circle rate in degrees per second -void AC_Circle::set_radius(float radius_cm) +void AC_Circle::set_radius_cm(float radius_cm) { _radius = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX); } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 59d4b0acd71..f0ce1237535 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -44,8 +44,8 @@ class AC_Circle /// get_radius - returns radius of circle in cm float get_radius() const { return is_positive(_radius)?_radius:_radius_parm; } - /// set_radius - sets circle radius in cm - void set_radius(float radius_cm); + /// set_radius_cm - sets circle radius in cm + void set_radius_cm(float radius_cm); /// get_rate - returns target rate in deg/sec held in RATE parameter float get_rate() const { return _rate; } From 8fe9a8e9f6446ea5e4f0c5989c9e6d34b5529daa Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 6 Feb 2022 22:06:07 +0530 Subject: [PATCH 0049/3101] Sub: change circle_nav.set_radius to circle_nav.set_radius_cm --- ArduSub/control_auto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 8114a220fd3..4ee719f7c69 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -180,7 +180,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi // set circle radius if (!is_zero(radius_m)) { - circle_nav.set_radius(radius_m * 100.0f); + circle_nav.set_radius_cm(radius_m * 100.0f); } // check our distance from edge of circle From 7765399e497f9a95726cf41264c9fee9e48b0afc Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sun, 6 Feb 2022 22:05:17 +0530 Subject: [PATCH 0050/3101] Copter: change circle_nav.set_radius to circle_nav.set_radius_cm --- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9d449ef7700..9d2d1a590ab 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -378,7 +378,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi // set circle radius if (!is_zero(radius_m)) { - copter.circle_nav->set_radius(radius_m * 100.0f); + copter.circle_nav->set_radius_cm(radius_m * 100.0f); } // check our distance from edge of circle diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 6b31b95c768..eca217bc058 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -52,7 +52,7 @@ void ModeCircle::run() const float radius_new = MAX(radius_current + radius_pilot_change,0); // new radius target if (!is_equal(radius_current, radius_new)) { - copter.circle_nav->set_radius(radius_new); + copter.circle_nav->set_radius_cm(radius_new); } // update the orbicular rate target based on pilot roll stick inputs From a4da65ea05b48e27355caea3b74768ca7e8c50ff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Mar 2022 11:24:44 +1100 Subject: [PATCH 0051/3101] AP_NavEKF3: fixed constrain indexing bug fixes #20180 thanks to @liyue75 for spotting this! --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a4650192170..e80533cf505 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1945,7 +1945,7 @@ void NavEKF3_core::ConstrainVariances() zeroCols(P,13,15); zeroRows(P,13,15); for (uint8_t i=0; i<=2; i++) { - const uint8_t stateIndex = 1 + 13; + const uint8_t stateIndex = i + 13; P[stateIndex][stateIndex] = fmaxF(P[stateIndex][stateIndex], minStateVarTarget); } } From ad53ee7f0e2a8e4a2b08a68155644d45d3b5a752 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Mar 2022 07:42:48 +1100 Subject: [PATCH 0052/3101] Plane: added release notes for 4.2.0beta1 --- ArduPlane/ReleaseNotes.txt | 66 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index e45c3c3247b..2a1f7621456 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,69 @@ +Release 4.2.0beta1 2nd March 2022 +--------------------------------- + +This is a major release with a lot of changes. We expect at least 4 +weeks of beta testing and several beta updates before 4.2.0 final is +released. + +Changes include: + + - EKF startup messages reduced + - LORD Microstrain CX5/GX5 external AHRS support + - Auto mode supports up to 100 DO_JUMP commands on high memory boards + - Auto support for NAV_SCRIPT_TIME commands (Lua within Auto) + - aerobatic scripting from any mode via RC switches + - new boards AirLink, BeastF7v2, BeastH7v2, JHEMCU GSF405A, KakuteH7, KakuteH7Mini, MambaF405US-I2C, MatekF405-TE, ModalAI fc-v1, PixC4-Jetson, Pixhawk5X, QioTekZealotH743, RPI-Zero2W, SPRacingH7 Extreme, Swan-K1 + - Parachute option to leave servo in open position (see CHUTE_OPTIONS parameter) + - Parachute released arming check added + - Pre-arm check of IMU heater temp + - Pre-arm check of rangefinder health + - Pre-arm check of throttle position skipped if PILOT_THR_BHV is "Feedback from mid stick" + - ADIS16470, ADIS16507 and BMI270 IMU support + - AK09918 compass support + - Battery monitor supports voltage offset (see BATTx_VLT_OFFSET) + - Benewake TFMiniPlus I2C address defaults correctly + - Buzzer can be connected to any PWM on any board + - Compass calibration (in-flight) uses GSF for better accuracy + - CRSFv3 support, CSRF telemetry link reports link quality in RSSI + - Cybot D1 Lidar support + - DroneCan (aka UAVCAN) battery monitors support scaling (see BATTx_CURR_MULT) + - DroneCan (aka UAVCAN) GPS-for-yaw support + - Electronic Fuel Injection support incl Lutan EFI + - FETtecOneWire resyncs if EMI causes lost bytes + - IMU heater params renamed to BRD_HEAT_xxx + - Landing gear enable parameter added (see LGR_ENABLE) + - Lightware SF40C ver 0.9 support removed (ver 1.0 and higher still supported) + - Maxbotix serial sonar driver support RNGFNDx_SCALING parameter to support for varieties of sensor + - MPPT solar charge controller support + - MTK GPS driver removed + - Optical flow in-flight calibration + - Ping200x ADSB support + - Proximity sensor min and max range (see PRX_MIN, PRX_MAX) + - QSPI external flash support + - uLanding (aka USD1) radar provides average of last few samples + - Unicore NMEA GPS support for yaw and 3D velocity + - Board ID sent in AUTOPILOT_VERSION mavlink message + - DO_SET_CAM_TRIG_DIST supports instantly triggering camera + - DJI FPV OSD multi screen and stats support + - GPIO pins configured by setting SERVOx_FUNCTION to -1 (also see SERVO_GPIO_MASK. BRD_PWM_COUNT removed) + - GPIO pin support on main outputs on boards with IOMCU + - GyroFlow logging (see LOG_BITMASK's "VideoStabilization" option) + - Firmware version logged in VER message + - SD card format via MAVLink + - Serial port option to disable changes to stream rate (see SERIALx_OPTIONS) + - VIBE logging units to m/s/s + - BLHeli passthrough reliability improvements + - Compass learning (inflight) fixed to ignore unused compasses (e.g. those with COMPASS_USE = 0) + - EKF optical flow terrain alt reset fixed (large changes in rangefinder alt might never be fused) + - EKF resets due to bad IMU data occur at most once per second + - GPIO pin fix on CubeOrange, F4BY, mRoControlZeroF7, R9Pilot + - MAVlink2 serial ports always send MAVLink2 messages (previously waited until other side sent MAVLink2) + - Omnibusf4pro bi-directional dshot fix + - Real-Time-Clock (RTC) oldest possible date updated to Jan-2022 + +Happy flying! + + Release 4.1.7 12th February 2022 -------------------------------- From 08d53c2d64971f0d2edb8435cb1b8a40c1b84959 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Mar 2022 07:44:36 +1100 Subject: [PATCH 0053/3101] Plane: change master version to 4.3.0dev --- ArduPlane/version.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/version.h b/ArduPlane/version.h index ead1f4b9a5e..8cb9ae50b4e 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.2.0dev" +#define THISFIRMWARE "ArduPlane V4.3.0dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 2 +#define FW_MINOR 3 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV From 7470920dc6d7556ec93806d7291f0b361ec8369b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Mar 2022 08:34:56 +1100 Subject: [PATCH 0054/3101] Tools: switch compiler for new releases to 10.2 only sub stable and beta on old compiler --- Tools/scripts/build_binaries.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/Tools/scripts/build_binaries.py b/Tools/scripts/build_binaries.py index ea658a8fa60..d13e4fcd071 100755 --- a/Tools/scripts/build_binaries.py +++ b/Tools/scripts/build_binaries.py @@ -47,7 +47,7 @@ def is_chibios_build(board): return False -def get_required_compiler(tag, board): +def get_required_compiler(vehicle, tag, board): '''return required compiler for a build tag. return format is the version string that waf configure will detect. You should setup a link from this name in $HOME/arm-gcc directory pointing at the @@ -56,11 +56,11 @@ def get_required_compiler(tag, board): if not is_chibios_build(board): # only override compiler for ChibiOS builds return None - if tag == 'latest': - # use 10.2.1 compiler for master builds - return "g++-10.2.1" - # for all other builds we use the default compiler in $PATH - return None + if vehicle == 'Sub' and tag in ['stable', 'beta']: + # sub stable and beta is on the old compiler + return "g++-6.3.1" + # use 10.2.1 compiler for all other builds + return "g++-10.2.1" class build_binaries(object): @@ -454,8 +454,9 @@ def build_vehicle(self, tag, vehicle, boards, vehicle_binaries_subdir, "--board", board, "--out", self.buildroot, "clean"] - gccstring = get_required_compiler(tag, board) - if gccstring is not None: + gccstring = get_required_compiler(vehicle, tag, board) + if gccstring is not None and gccstring.find("g++-6.3") == -1: + # versions using the old compiler don't have the --assert-cc-version option waf_opts += ["--assert-cc-version", gccstring] waf_opts.extend(self.board_options(board)) From 0b29d0243d2edafaa76f5d7448b095f609eebcee Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 2 Mar 2022 09:57:14 +0900 Subject: [PATCH 0055/3101] Copter: version to 4.3.0-dev --- ArduCopter/version.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 34092026aa5..110cf18f0f3 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.2.0-dev" +#define THISFIRMWARE "ArduCopter V4.3.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 2 +#define FW_MINOR 3 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV From 2ede298e8cc105157505394f2a698088afc7a8bf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 2 Mar 2022 09:58:12 +0900 Subject: [PATCH 0056/3101] Rover: version to 4.3.0-dev --- Rover/version.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Rover/version.h b/Rover/version.h index 6ef44e62bd9..34aa6b150b2 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.2.0-dev" +#define THISFIRMWARE "ArduRover V4.3.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 2 +#define FW_MINOR 3 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV From 32afbbdec4cadea43603507ba4227629579e568e Mon Sep 17 00:00:00 2001 From: Evgeniy Date: Sun, 9 Jan 2022 17:47:26 +0300 Subject: [PATCH 0057/3101] AP_HAL_ChibiOS: BeastH7v2 board added --- .../AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md | 87 ++++++++++++++++++ .../hwdef/BeastH7v2/beast_h7v2_pinout.jpg | Bin 0 -> 75912 bytes .../hwdef/BeastH7v2/defaults.parm | 3 + .../hwdef/BeastH7v2/hwdef-bl.dat | 3 + .../AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat | 47 ++++++++++ 5 files changed, 140 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/beast_h7v2_pinout.jpg create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md b/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md new file mode 100644 index 00000000000..90def7f831d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md @@ -0,0 +1,87 @@ +# iFlight Beast H7 v2 55A AIO Flight Controller + +https://shop.iflight-rc.com/Beast-H7-V2-55A-AIO-MPU6000-25.5-25.5-pro1588 + +The Beast H7 AIO is a flight controller produced by [iFlight](https://shop.iflight-rc.com/). + +## Features + + - MCU: BGA-STM32H743 + - Gyro: BMI270 + - 16Mb Onboard Flash + - BEC output: 5V 2.5A + - No Barometer + - OSD: AT7456E + - 5 UARTS: (UART1, UART2, UART3, UART4, UART7) + - I2C for external compass. UART3 pins are used for I2C (BRD_ALT_CONFIG=1) + - 5 PWM outputs (4 motors and 1 LED) + +## Pinout + +![Beast H7 v2 AIO Board](beast_h7v2_pinout.jpg "Beast H7 v2 AIO") + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the +receive pin for UARTn. The Tn pin is the transmit pin for UARTn. +|Name|Pin|Function| +|:-|:-|:-| +|SERIAL0|COMPUTER|USB| +|SERIAL1|RX1/TX1|UART1 (DJI connector)| +|SERIAL2|TX2/RX2|UART2 (DJI connector, TX is on the back side of board)| +|SERIAL3|TX3/RX3|UART3| +|SERIAL4|TX4/RX4|UART4| +|SERIAL7|TX7/RX7|UART7 (GPS)| + +All UARTS support DMA. + +## RC Input + +RC input is configured on the (UART2_RX/UART2_TX) pins which forms part of the DJI connector. It supports all RC protocols. + +## OSD Support + +The Beast H7 v2 AIO supports OSD using OSD_TYPE 1 (MAX7456 driver). + +## PWM Output + +The Beast H7 AIO supports up to 4 PWM outputs. The pads for motor output ESC1 to ESC4 on the above diagram are for the 4 outputs. All 4 outputs support DShot as well as all PWM types. + +The PWM are in in two groups. + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in the group need +to use DShot. + +## Battery Monitoring + +The board has a builtin voltage sensor. The voltage sensor can handle up to 6S +LiPo batteries. + +The correct battery setting parameters are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 12 + - BATT_VOLT_MULT around 10.9 + - BATT_CURR_PIN 13 + - BATT_CURR_MULT around 28.5 + +These are set by default in the firmware and shouldn't need to be adjusted + +## Compass + +The Beast H7 v2 AIO does not have a builtin compass, but you can attach an external compass to I2C pins. Default configuration does not have I2C bus enabled. You need to set BRD_ALT_CONFIG=1 to make I2C use RX3/TX3 pins. + +## NeoPixel LED + +The board includes a NeoPixel LED on the underside which is pre-configured to output ArduPilot sequences. This is the fifth PWM output. + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the +bootloader button pressed. Then you should load the "with_bl.hex" +firmware, using your favourite DFU loading tool. + +Once the initial firmware is loaded you can update the firmware using +any ArduPilot ground station software. Updates should be done with the +*.apj firmware files. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/beast_h7v2_pinout.jpg b/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/beast_h7v2_pinout.jpg new file mode 100644 index 0000000000000000000000000000000000000000..95c40066b4d56d41cd1a1f07ecc82c5dfdba899f GIT binary patch literal 75912 zcmc$_Wn3Ih(>J=fLvWV>L4&(Xg1fuB2X_y?xGx^ugS$JyWl3-g?(UquuKSheea@G2 z&X+U$>zSRI?w>(seFg9Z00#pL3kw4WIl;lf z!6Tp|B0vrd6ci*>Obje6Obkp+Y&;TtY#bt7OiY501Vp5y|O?WYi>NK3Y z4Fd~lB_a|s3M50*2LLn_3=A}+{SW|<-2RaF09Z^otdDGB@YpIQ2$aq^?7@izh*aWr zeYmQ#7t|c4E+I(Bc=!Z_M4xDA>F60axwv_F`S`zlm5`K@mXTFc*U;3`*3mUHx3ILb zwy||}bNBG{^7aY+9u^)E85NzBoRXTBo{tx9Pz}0x;`W-4%G`>Q2Dz=;` z9F800F+JM9ZThF^|Nkk3E*}CMC+7gl2901ZEeN}qiPSnDFdIk5nR^51qATQtyt4iQ zbQOiC`vIJRlA!nzgnm;rN3Ltw2=L7R2iZS~2m&Q;Asj-ve+aq$Kajz2W)v3|18`Cr z|E(k%K+iGB^a;QOom|ZHpK>XU$N+jcCh{-z7zc9oL>i4XxG zl!(OvkfmhL&CUfOD>gPlIJStB2tdc#n5PTSrRRe%Xn0z*{0!7h;p1F1n~{=`d@krrU}w#Q2cXqZpHr$4mc(N zX*n9x#~}c-H>5?Lw*R!AFdvq)F|&{pP!dO93~4j^cC?AD!VT0X!m@i;$tOVaWnnas zw|GqyqcGa>NMQl+#38>XZN3*oy&WCzRlH6Ib))8~>WBk)qa&s72)A(+MUhzt&LHLk z=H{H`M3HWPq~l~OL&v2kLogHw!E_U_Ftq1kET4 z`bKM6FO}juwMvg$G<7}V9h@DoO=zfJ)&k3JYxZ@GN!62^7F$qv1hqtpmc6ufxMr;v z)y_GSNie{jPq#(I<8dIj9lM5fr^vU@%Y)jwyL7x)D-?;hZ|bAM)DIKhQ`Q7RBSHshk1zmf9n)rnaJpebY9&sp8U7`E$c})F-8;H-WSHcALR0MKTZGMZf)HMoc7j*cV*2 z<$`gE?6Zn-j;x6rvsEruLAm8G!Lp1{jfaBoLPt~KU*HQ8yd7Ep(Pz3YRSj4)YQ#8K zecS=Zu+~|vr&WI5%x%1+6FNl9n6VXM^_lJTnSo7F6T^WF)bO-~8ED8zaKrJPfu5BC z@hd}v=*~qU%>U0+EhY;Q1*)pEqEPuNQk;MsIw5)+sC+om|AYJfl?fnGYrqiwkK}3q z4tgUp2Za4>I3%qBBb;p|dl`m7WV#@Ip9Ln`uCG^{w=V!SfF~UXPT_B+v~RG%Nv$2>B`p#s@f)jH!QmkFoa%?DP3L6kg5&hTBb;2 z>9T#6avx$w()d4q{{=h$IX^);PK3Xw@S%BLd2U`=-D}5KkvKW{ru5XN*f+BzdyT7I zK!x?TnEiR_(>nl3D6*F&#GC9UuwTPi`Da1`{|?A-Ju$ZN^7&YLe}&MJOqI5a0A`;SWCG8Wgw;AW0A*+IkV&VZs>Tj$-bq zTG{2) zI)&H7c9gbT<7~2E`p13__NGx^pGLRfe0S{- ziFKb|*6U(bp9jNVxCLkCWfl`}|9)~kpHB2ZL7vJg=<$pjrL3gARW zgUtWA*8fu%`5#w~Nr;RV{YRRM{UhE1)0~Z_z!_*FL;9jQynF;p;DSYK4e@XuR%0x@ zWK+{SK&jJ^5VQO1Q_D@kfrM;gNiB_MN0{Us^5fm}wi3|YE|e75E$JiSfW8wyLX%+l zBA1!|N~@}Al0X=;>yJi#=Hl;zVgBcQnpt2$f$kRfdaAJM;jOo>y?v1iV4lP-BAlA? z#&aq94#-$s7g;ZzNXRvT>(sgFOYxbPuRJzlmD!obDdPp#^qd!H`f=gSB6pX(jJO~n0XIm{!krYd}y`yv`UTpvb5Z}B(qS}*n=`sA!|y!=Vs~tT*Ys8 zF=Xc_yvW?-fOlf3Kjt#CrbLir*de;zCW2+HQ0o=ok2P%2@@+Lr*}H%paW_PIitTAB zCO)QoqOk33bFWQ3clffae$madCsOQ68k(KNqD?UF1=%7B<+BTxOYcsP3Ak-dM6&bZ zJbe3Sj~YqIX5H&SGD-_^9KWkB>%=ckXYY<~`Q5znx_-43`eLTZO5Qd@0pc^%_&P-T zzQa+n#>f5e%O?$v7(+LmI`PG`{1JL$x8!I9<^)MOK8CV)x&J7K7X^YC|NK z^r5^7cQ9RAU_TkNLc7Dl2B|IsW#??5bMnm)_ahW@g(z&qCqOBU zz7)~sYA(1f4kOKoTmvx$44oCsBN9Z3VUKZB+|5Lf&GoS=SSynjYF8lxKTnSha5x$y zktmjUHe=`{s&9T63FQOD_I7htw32gg`@Cugi3Yis-vNJaWj6I>erTrx2+$U+s(rNa ze~Bda0m}+da%khYB82&8+#Z;<=~KMRYQDMnkoYtDIA~(N1F#WqrPKPMXQv8ap#O}l zpGIFOK^ZByUcxBabwngYEu48w({#-#X#S2rMRX_rG-tw`PF*X|H-f-Ra$6eZvbQ?G zQdT<2=uVwu^dxOmbxS;s#FJLgpP`_& zm4zwduItAUwxyoFL{go(JM8ssDbr~IOF(;d#o>%o+^GjMYG{a@ReZ6l@7lD~muX=t zyXCN+n9fQe$-fjyoX&qI#IxfHK{vikk~(f#dU_6$yysz!E-e{6Ii2Q-+1JxtyGD4_ zv7LS;+*)d==D31pBw7{P`bcg9(wr+DUl-ttl%n8IXh{;#otG|aKuhX7<$sOe0qYg| zoy)z#hWYK;<} z^&yXbEH6zPac#pKE8uMNE+o>-g{r`&to?P9VbirKe7?gGa2sFg>TOk=-S$!>v%}2_ zV=RYSO2F%OVnIfn=)pH%qOOa1J&+Lw=Tms;$27fo1|E>A%iuS71=k*8%ldS&dOJQ7 zW9BuZaVp}Vecp3j5hP}NvkUOAOk*v(d`c|LU#UBU=Y2ILe^0R6b{RkWcgD@=AE6QH z#M-j7h->Uq3YX`d?E~X($}6SIKOKTUa;u)oxrPqxlVo2{YRNdsQd1r)I!F*I%pyvi z)%#je#bs_M*8UJVLZ;5F`A_SP#t-@oou-G^%-$-y)13l~ z28R?mVtcKM-G$-7^J*7Eu_HNqqm&vc$;qwguKKh3E>H?6vmTCDl;nJAh3!@H8nf%tKa&Bxz(mra}J6oRza} z(hr=6-Dy}VvTPghv-Cl8FXl2{CphHDBkewFwRCi2>q-4OKh8wvyx^Q7nlbIdGuF)r zTOx+5JlYTl% z{W_6R-=$Em%3=O<;H}8XDVt^VLNiqhaP#M_*_(u1l9Z6&R~wrSp=7no`0&On&3&CF zbY6z|&d?jes`Q7%Es~vQ`FFrZ`a1yma%rAZh&IPe#jYa}44w&XeB#IzVfcP*NGjGO z$X?Z5>-P9c@5l)^;%rCDwkqxt`-bp){x0d@%ZSFB_K~3bPR8wX z7Ndamr)apgFX<$8lWq!B-vP3sjAJNE7n5FV1N;OfqH&GIR-l|| zY^C`Ga63H)U1N~j+Lamlmt%}|*!lx%?)yNU{AuwB$=(=QM2PFmU&>*61Vc{^xF}vz zl&GLIF)&q1+Zw39k5X~atz3WWeAC$ubxu`gmeh;u3M9F{DfcTk*H9DmQkx$C7KnQJ zG{@;!Gch+TcD!=U_m(>0%PiM6`l4nfN37OC4c`YtmRh*Ux37e}GH$V)PnP4Hydg5z z))NKF*21i=mm|p)-^vwfml1-q@nc`CK6A4^!8_y3-QWZ6a-iPAx9c0#NhOL`k#Re| z1Hcig_pnL1jxeaXB6*zNyNMKSqyv|Uv4rQASS<8bjKUF)Cl zY%O{FZ(NgROYLW-4@YO#ZXI@cteB+ZE#qv_E3*NVHas5V%m$}LFiGmEP6fFAFY*gJ zaxkS5&LmSsqbmq&J3)$ycGz3$ApgB4(&mb&&ouBK4eo&Z(Psnem~C6z43pcsMfY`u zmo9RFSVx+c;B7u*Nh8*5Ai(?B(ET!IAie#q$c{qRsR%IMRx*6eb`_ZJTI zyibeODKgGKGsZsmVVH{X3ODT=VG#Z4lqf&wW@w04S9m@-Gy0H5*au@nNcQK)m>g(d zp^lkJfDGGW(+N{=S>G{kY+2&Z-_G8h*Mcc1T+c5LuKuprvEon5lPM;Wo@Xzb-8Jd` z!dP+3Hd>;dl*_-f9X&Z)zS!^NQhmx=QQl`aMokIphT zf+E@%nuYigoAXtcA%Bx3EkE8}-f~T1M66R%?C}PZlwGIn-_}+MI&I~l9CqU@Fgtv< zjQjhMg=Ct_L6z@tu5(~(GU$?Ps9(6!!O|Fz$9aqJ4pCaqQtr+a>r{+qmK$QD+x0+8Q^Q;$poV9sd+raNqFR8 zG=?p>jkF~es!Q$Ax7Lk(IaT3v{is|UnW}}PLl(~y-^tzVF$zjb*MNd(aq-2UjXu2a z^ZJVhZZt#YjEic2w4SJQreT2m2mE~a0#)awVjIF%Mv|rmq?4!jQXnz=T5J`AT4z-<1sQMfD`UXh(O;v-zBqm1*0WGv9JgMER3OMZGT-SB6IN zEY&LZHhI|PR9VmcX-|J?%r<@)(!LhZU!|wNQ>ey|sx&E<^sa2@VE{4V6@ezW%1hh> zvpdG$QJ~XPFY*Pk!o}j%$KT!`qxhjvY$4-QTBRLhZd50{Nbp`W%+f^pk3PRzkq9nK zkJ;EgB!qNUvVpe@r@<`_F{U=zj*5+lCZI)ggMLOd6-VU%!@K?$k^N02EWeF3se_TN_zVQ?S|Ljc8; zt}=+0|3#9XUO!VnQH3ATRt*UVeUSv^NEc${rmerdlfNUw68brY>VW9(}@GWV8AzZzH$s^gjgHTbD_1yNe}HDy`)jwE`|BDT=jquw=yxFT^ug(yiW&34!eNK!TeV!mWN0;hBH zxab;4&i*2Hun>xs+YH*c$UO8qz6(3mTi$q4e4!_+kaV&q5p(HvxlFubyC-EN?yXP) z-ux^@I7OMXY2Y)iruJ#+i(ql88dO^AN{2bt_UWv-pBnq%L_a+?C5pDhRefO)?C&l{ zh#x<_ufvnZ<5Z@=HCUZajleV>RHeKEy2v42X!m&P?NVW+D+jf9JDsZj( z`C^1~MH|nu?;T)n{+6vFAn?iumNspbXp8+R-uml07jR$3!liC^I`yE>3YX;wQU9M* zustSogo={9jm{0CpxqsyS@vQM zkNSLJJ&)>i9W;l<3M}ekJo+(+yT#=$P0Lw<-6C=oX`~i6rIDU^oy0s;Hi^1-92@95 z*C{a6K;H1sGJnd7d{5v|5~4%=ZP`pnICV3CHQ6CAM)Cqh&w~EY&|7!RKhvcbW#vp5ZCv@&7@gI3uj8*QyOtFmS;~*LLPq_1VU9nJ zt8Q+v&UqsFFBOcnK#zJo8619jWPS1;3X8E@g1ru6h@tb`#{T^nVnHrW8Tyk*)x_6Gq=kCP5&gJ@-Tdw1KS(^SwC{Qhtr z-FOfAoO;eqLY+L$6{nJGO$$DbeWQ1^G_y`asZ9@m2-$m;@k9sxIp#Vx{@nKwNyTxU zim&ME#W$?zA>n+X8EE}U@M}cy$8&hTJ^J-_c-C_Kb`V;93bmiD{=xp zIXmpg8f+S4^G*r3)~vq3U10o?r(A#bHtsiS%<1*vbHQxWG0eOg=h?mJ;mM5y$H`#zt~*jZOF7{R&TZ^NqRQ+nTXk4_k(%jq? zbVU1LS3|8plUE_ZVV1E%r?=Aja-5!qQYnzh#KJ_~+!h&d#Z54JOaAJJX8xD1LqQw# zX!hMZ|4eVb*3sT^r@b+A3&gz|R|-q?^kpkPdTEG=Jd;hME9iTx%EoL&U2VqVmDi!c zQlG!h%{htgP-|o@RPeAD-`_ypW%Q{LVf2nt71{>jM72rjl6?5$FGIG|e#xAw$hxBc zdV?VA`GDDFh^Oq|yZ`BR_+MACd>DvJC^H|nsF*Vapt2k-c_>Ood%2{ad#xWaa5o{Z z449)6@%b`Co{uj^_KVbWi^VNHzd&%t!IR6jAMN6G&Rtu`>1{ z%|SR|jA{ot$f+lpP26*QL#QMgyB8#Ca7j+rJmFqm_HEPTE%dY$ki@B@oq<|d62l*W z*c)>A8SNUQq^{Iu5#(okT`|3HIQ182>DUB|=Q{9*1TNhJbIp#_FkR}_8=BLqap);j z=Zp>eq;dmGPmY>LgvJSOGU?}m?E|4I2kirgRd@F?cd^1|QJantt3Ir?ev1@MJ3P z7Y=#ERYQMKqh^y_9_tko6K$1=D|bb`16mxrlyGjqXKQ!Eq#1Y6)e@UZgmPF4_~A`5 zREaJ?ri>7bfRsOeEZeX2v8&zW3!B=*`&Lc%0RH{B6qm0XM~xlm;fGv^wD{@lx=rnA z!yH{57`Bsd^VX5vr_}ac#Da^yiy0mACzX4Nx&F5Icbt~mJ9^=pw?;; zdItm>DRN&NK4u3YjP4^`*P!(4qJ@13sgvwhVUYADoaU>c>#NKiykvF2 z{{&LA7PVw-yiAVXdLv)eFY<{Q;*8vW5=${f=dE>ob_zZ>wr09%`yQ7;`~`sFdS4m- zpiyK^cHIFZzZd`e02mFzS^TL+gV{T`k_!Uje->9r<$*&k$zHmF*Y_^E514r zAcX}_Sm;uqCy9G?Ji4{d{!k!JpN~U+Lw%z`gT54c-`EuYm0iz6#6^o#sKhl>{*`42 zE8ZbHidk6#I=a6Pcwx)>_ol;mW5p1eLYdOtH0IiB2!VsTX(=$SwKO=6pcZc1(ik*{3JR(J$iiiUit@0SKk^2~G1Pefv7rP?Zd-WptM49(*imPOJK zvCoFPg2zM zmK7QZr3-TJwTE`i{ZaW_z|j}8Z$@^tMK~~t@1#^_GX&3d?)uVJ9ex^Vi zy*B^xyg}@l{~avxpZ}<{QaH*0A2ovchZ4Sg#A*E{rb|4#j2p9Ed_>nhhr@WEBcisv zNF}0&fX&A&^OdU_pX)>YjNLUPYCFZ%kuX0YSBN``>9v2~zpL?zMx+yG=W&T!Dw7?a zZ-ZibU(XP}D|&vYVm`1UXC2{mAhriwe=8?b#YKNgfSZQIJ6$HI+0jSN3YvY)ZQDBK zN5dD7h83kr{F>b3;lek4Rcx2o1M>)$%&Hx#uwk3*xvwa&Fx(2N*s!P6$5L?bAE&1} z*!lFy%^f_41X#5TeW)$!bBrn%htAYS0JxiMT-AX34;%<}h;>LoJf%kELfur4{DWTIBe9|-)tWY@erol`ox}@Kf|dzv@I@&s@Qw>?J$^@q zyXX|jc$eRt$nA68)+6?`^s$=q9bh$~$QI|S-BPzrua`10wYi$Ma;MfTh?@=jysifon#(ujj&I05GkQ&ZaXPoo1GmrO8Ur%el>icDL2nRNkFHw{$22<$cGjni<>KyPPIwwZd3{u z$5M1@$%D}#4;kv>ZHAb&gGCx2N!fTR6}cjVfAX3A@vpkY)rTIjju|ciy;o#C*1IIc zY<+}*(??0DGdtof{5Vbp@}WZ2?q_)uhg`be2(Ql*tq8gBP!+ih7jea<4;&IbRt)KL zCHhd|nm`ir5w6M$&wgFfk*LN8cjBnsgw7Ieu%L-}Y*lq`+qC22x6(7-4Oc?bQxzIt zy!1;esYT}Q-p`H`NfrpHFm)t8rkx!lDE!*CS|K0r40LzcuzLBut#W=XXtjUYM8liA z8AK6e;{{S;vd}%6CitK-lMe__49(sfoyh`+3=!jaltkZ zV&%o+gFsJHJnzn{c5PXpmG+`Ami4omd`Goc;b(y^PXCd(-?dS1bkg$=(}S=Hn74dTRL$hL z6+L9al<10u_TTu4)km&sXwD6CAOC_bVPV%!nmIvfa#3VuJJu=V)R-XLjH5G2B#q%6+lf94w9;I;bds6k$dnQ}(Uj$Hq>yOV(PFAxe+I*cKy_fJ#~pR#fBEq<5WCiXDJ@{m z-l$h|h5H=8{PEQG@}OJD>y{(4@qW6xZ0CwAX1gakm;O>HMB8>=j{r-InesdRXDAqv z9E;KMw$atEL9%BfUM{SBgp=+(BuDh`0DaSUzzklSjN@QJd#XTA)HE*GfSgu6gb#l1 zZl?Zzl&xC5)-05)7S^YB08My}bGB@}plA$p|3T4PWW0U9gUomr)6jp@KwHfA$?tod z=g^Iigrm^?IRGO3z!gPFap4a(^s1TylMH`#f^em_l_EA5Z^0m7RPDHGrpNewfluDbO81bN7j$`0EjaE5XpqO= zJe;~~wl@=3u}Ed1uuvgX>@N_FogVnzbGMvK|Jco;4l|d8({VvOV`M*bY^&vd@Z4!N zo}ZTnpCNO4>3OddvJ?nK*350+A(>rM=1?o0GeR@|z)}@jb~jU{&(A&I7d7?1(<}liEfhW#>-v{l3~or3n|mWhha7gx^g821Xa>N+OY%B_Wu|7A0#+ zF>`mCp-XfS&9wV*E-`oGoW3eUZF@K2>Q&A-gH35oJ4FC4r-2ok=!s( zLdd+nFNW0c^UzSPXg!9HAmJS#GA^4NlE)nWpg>>be_Q})#|?SXlX?f_M_2BMjICD+6entXhv%WCs=|wbWv{xia zBHAuiMsb`t{TUbEK&g7uhvMvcBCM65r2txXVr(i61|l8=z=RBFgOlbCEfaHTVq3>X zsbGqIg!YG=BJL~%&#{)t*nv4(yX<-oBkn7_R?x7!#$QvhHSAj1@*8oiT}cmLyiz6kIs{~FI(Fm z&qGR!2jX8buv}wCt^T3ca*o?;lW~FZMR$LA8rElTjb@y;Ndf@OTQxRz;2Qr+1#HL$ z@X?g%E^m0H>o(7EK02Jf>`}-FoJzoCCu{fdaYW-Dt0&3iq=yN5m6W3b)HIGj(QJ*+}#^lQ{A*@E$I=#YAc8w9miL;-#C})%+R1lyS%H z#Z*L)$MVSo7li9zZR;^sF-?3{3HLH(*e7&!Fw`jTSUuVp0%IQV_&ConwM=5h@r9BM z9f;mmSZ3}Z*MMmo66k#+Q^N8pAr-!}YOY6cn+tmfjJW;= zJq?J@-MmuChY!fN#GzQHz^aqiwv~rCtS(_Fx`T40@;A_}+XI4&21;zwj^R2Pg8;jE zYDc!GlbxQJoT~Y5CLxIBNJGZQ0TgyZs`H9<^~uV)G6>69G~}(ocL1?f{6KlNn-Fye znZl(~uR|$cXQKi5vkWZ8E$p4h$vc4I>OM}+8>otkX=JP~0k_sMz)+Rg$}sK?s}fxtfjC$-EDiC=)+4V2ASM%{PATq5%R(^=qEVnioROn zNmi{zpmu?nF)RbD(@)Rg@KXV)*MBRBr!mE;0GZ4I`o2w`WIUHuR0<9(a&1_G`Bii1 z(cM8$VEln!ygQxAhFu2DK0cXHj`6e?Ww(4)v{j9QKzHqhY!fm_koq#cFm>xFBJ-^F z-Dt1Kcz4+RAd-Cy^p6seh+@A13jrgqIO44E0Xa9)=@Lb|!ww`$FPyIj>U6`)!)U&8 zS%1$n6&};o_e$a_M>Mh82@kN4;h@tJ{(B%WBE1b9vo+wKKx7ykRnKBb$m$Z#$+h%A zG$8nCiS!afwza$A`bVw=CcS{zR##-ruxzDYaJ7XYy7KLlrhB<$71PHiceCAkRF_dH zSiT{*`%6gtS}AjZLulnnnOug{dZy^sf+z10qqq`4)NL_YG=kzLn zo>d1M20L$ckjjGb7FpaLnQ42&kKnJC)z0|Y?yCqp0jhS9JK$=aErZh!uu!?$)kLWFHrb;Hb=7u4&Zo@!AVA^-WF zCe^)Qfs7PM7sca)o#d>3o8z&sEM|veV`Eyn+Z+)_w!iQUTA9qowH^0a@ceYCSnlL@ zarpGjeBQoK8;XEgNlSPa{5jABTk5r>o+}NaF}+>NU&s3>0w&cLF<#{ULYxXE_m&I_ z<0l7A);Ae1JkGU$Y?oj7?a@uYb2YF7-3)qn=31Y#ax`rWZXUUiPKjUjcT19YEW0mN#IzlcOc$2VGNdV zDo?Y1w3-THkw4eY8@L@+4+A4wEz5?L!U|JKE2sLRJ+v?_0XfH7nh(W+2+&?2t;?)o0%&{_?--_G^4y z7{nL*gVQ*=56#bm7=IxvPI`!rWOBx=p(LATb0zbBT9t?0xC<{F#Cnj?d{oaFPn(|h z%S>=TF16{$;S6~w(OwY~k73h!+BNDX1xg&hC)aQv_NO0Ms0sXkms;;}Z{#nX9o0b% ztBZ?*8dXQ#g!LCCHQ-Y#vtavQ9}ob>uG)3pcS_fSLA9%?NYgXhrB^#wU_QkMf|n#u z-EYtOX1|cB#b##Brc-5P)^ZRcSlsMlOvp;N6-?(&^N`C?q`!L{xPri!s6_k^rl#pW zr)&ElD#?!TBSh#su(^ti=`+OY{)UWpdcI2hfomhw`zYAvCQr8n<$9`@Om`9G3W1(* z6L>BC^rmJ_Ia8wUy@W8i{s#>QapTHrMZ5Qe!eRdY7fZ_Anj8kd@Ct}qQnI2;hR2?? zvJ9jI987HJ2nYwwJ-54%=QTP*_$Wg5wr4H%*^9m-kVW81RDW>id|cIt3g?=Sj*Vlo zcWSBNO6_kbaDV-?i);?Q$Gb;!VyZf12-1-y0RrRa+MmnjP;5S_vKI>3vEMPOF}203 zH@10SkWes~2*)NMY@_L0{+U(6=c_DB@{;Q3--u*7qn*Er$@(tws5d^!bbwK9x%T^r z)v2y+zBFtr**@$CpJw0Yl8Hm*5kGa}Ve=cUT$Y={WmDKdinv$bVW|4VzRCiu?fja7 z{3@V0XXP>ZYY})xLPg7sh~@37 z!9mCZ*&Zb}7k2BkU2-Vi%(u>f)r5vRVv}}`FmMNIVVI^HHB+*h_tT{WC*QBAK z%nyt_dFKnH8xvqGyu68!T8ztR7hHC$BauzV#wK5*>5Pd`7Je!=ac%t;x zlRHj}HJNuJE%J|q{kpru`_fkP4j2UEYqo>)&{pA*lE8q20XXScpXWuR?@#Cr+u)>? zuX2%2r+*1M@!?dd4@Zw5i9kEDi{_V(1tsgYa^BwNbB?((NLs&(a+QS2oW7)EPt*M= z6vsyjAt*N#LrAcbBsEkCW;q8OK%!R|uD#ulo2dbXWZ{Jo%8M3b5fcgM#cI4|DoN~x z;e%>_a{KV3i(2IMR+83$K;G(U=;glcl`c^`)m423+?oR{N`RuI^rV-_N|T!i`-E^g z#K@@@L3>CxY*`4)$(dj7+I$wM;~lR%@UN&^SpcgakFEO@9>_tmaHA0knJTgGD7CaD z(OV>J6$`=v(0ljlUR3gzy#j{<2wPet)EF5%Dy;&~c|}sai%V*2Wr_nKQ7+b&6LNdr z6n-7Ac86A2&H18}iYR0dbgs|k(^Zp|&LKGt$yoDXKvG{kcG z3Osu+23X>qs6Rhz<^Nuja<{>qpw9R!dL%spYpw=dXclT6lQX@_rN2ccO0_2VcWcpq zajg9Jr~t@4Cm^EuV{u6nfC~gtvL&vD7)mtxU9HjJb(#G{vsnDkgEu&!m`+XX(b%Rm zb}`a8#xX8C;#f-xC-)pl{mm10Ku;Q*jz%?+Z+w!A;da~KH|hn4BCu2OH2~w=3CDpF z&MW@VlS5Y#Nb={Ya2EY2%6);;Im?5V7E8!Yi6+%4`|hSOv{Db(;6sbVJK(#MZm1iZ zzEmf_U%nIIhAWkSCRxy@m8enZkqHjG6TTq$vy=$WM{M=}r< z`S{LxjTEz&(SXBr?e79H>#)auCc&1}0%*Ab%o&fNH%1yZ=n1NdR$r}M4I(QLQtGl5 zIXT|eXCK)nhVE)XLb$0Fd5GU6wd=*lgHda1&N|ft2FTS7&#|M6-r`8I9k7&rs=noH zd{vW+|FYN^t0+)T*=dnz9#r#%ehS?#oDh<*z-Tf@S%-#OoS zeAgJK&}|-FA<32?wPQHBd5n^U5uHmGG;$?TD);xVr1Le=x}F_zAnLI)ihGkWkj`SN6Xy+MP9qsu&!trL$>F-Fz-+g_6hV3inC2(# zsMIgiaGRUr8sy)NalQla9`sZiy=pjL`EQx5L@zV@&G&PMyRf&obFF3s)kvE6zM=y) z8ghDbx7*Jnx$m64G{MT)p&wx8@2z-W8EwExx+K4C1LjsI9>LRJ!B&}++-hlNK>)*^ z9GQwkY#W}EX;mz}DS#PqDX$RYk{>=K)}3bl>xy6l`x$)m+o$QGl@&YXAsgZ`K{jfa zu(2su;KF<*y`xQZ!Aa^u=B^=X{DAUWOpfOEMdw#eIb>+Uh;rIsUGNe&-Nfi7w9J7` zGoA`Vh^bP|tk%}td6O^W-Q3BS5eu^CC@*~svE0%_t*O#NK_o@5D3>$MrpE3|*lRtT zqb!rCBGa=E0D&Y$201;gOP7^DUPdzEi;cfj?_01HamIJ%vmyJ^EOS48r;V_mS8SO- ziLrIjH!xl$I|lBw5LAFR^2!f;mdVIw=0+6ex3z1CHmLXWa=$Fmf}7-i2++^P@~*fl zP7G`taL8Yr5IKa!mEsg9t0j)N2^-;NN5!+_17BmSQa4o=G z01p|L6OQ01b+huDO^tKdhla`4w~thV^d@$`gQ$GoXAjwdw{$E3u9kdAm`t_w~fx3Z)vTGiJs zvQL!{wMYvtf7x(DyE_jz_4NCE;Hd7~5mU{6f%e$(Dl{)lv73sWu4QVV#Sh&v$Fy(D z9G$sVm<+fYg!f}F<)KpeJ0gg$c__pLUmd!K58Q*_0qq+npzAKv{tf+(B*p@~ZiO$EPc%30 zC9k`RfuomZdC{@1dO>MJE;D;Qz&YL$H}BKh-(I9o6LYA>^e%Q~ogJ4f5zLmejx{ya z(HFCLpQMF`eaUT4Z~TSl1W1nQ_gM)F4G_Xmnt&Q~}5mCDtA2Y7eM)Ld;(%_)9r2wpHp z8xRPT4Rc%LOQDf7g$yMOR~CtVP0!~(C|`f(lJb3 zuoYC1zzO=lZ3ruZP76YopPLCt0YJ>pBFO$uT3uanYiL?S+olEc{w<$W3XO&(Q0xIc z3C3?H%})*&=P-i9iWvJUVzB2fWBlSU=-RaFPfttU4D<-W>W+mmBxNs^v76gdsgYKM z{#d#~oiIw+xCwAqJNjQmx=w^s;eNe6-LV;uy#cjXPYGN@ul8#sbT0)H(GEF~dM7yuR@<{h5txtB|pNdaBm4*M9 zemfYbLkKm%c_X|et1gk-)}hidN#U$zlrAiB9Wh)yrak-pFmyiD&$jTl=o?OwC`5Cr zJ=5J`XoKwf;FrtVXp%3Bus7fIeO~&qRSR!xNO_$uNB{9V^dIA{fRbtnDHr;=WDhyD z;>Kuc5i5amL#W;6g14W>W_u)gGg{QSJhPl@$SX=6J$Vz-Z%4xuIQu11xOv-rx09SQ zn=8lZSCrYjEjk=z*Kz(Bw!O`efY1eId&;_b<`Nx}G3tcRVR0wP9X(uqnSb3Hhz!Ro zZD{>53VU~G<)laSh^lu);DnpV8h>#dVKU=>XZo2t@e7Ep%bhDc3kfV*CRH@IF*Av$ z?>3cE%&3t(*3Ui3PasL_>ul2%v1W7)U9llhvSp z^jLa>TCQ8for_+$cLGGQbVJ+C+FPa=U_BGlXEOtY9*6CqM+WXA4cGUitHDUZQ*&(s zf)oC$Z@7E9=q=^ecOMSH_w4@R^zs{)tKuA&V=%R1224wXdXLLuG+7q)GT%=!{k((x z&$^B`c9mSm24_<3yh)?I7&YQdaQacqI8vgK^58AWS0S$MRQU2u13ksL(uEpBy6qNZ z^OymPiC$%u25#-D8@ucedk#~(N8UOF_{%g5RoF%YVG3jQwn zWP51GeKU_1OC7n!1ud`2?_`Samvh+-hlrdQkmeh$CUS#Cyk1`40kVIkd=6A3Q-1zT zQL~(Qs+X<`A|bnJd8*DCJc)}m*}*jzzSnJN?Xo%LjB#KVD)6iHuvClR7Kq~Gg6JHA z?PN{xb#}-KT%FMeanfk<0gExd$e(6fqWLfz8Y3vO-*8zQqX4LJFIySX@FK_xG~s0< zSy|;9Lzk6C*fLZHci*L}ioEw9>V<@kb1p`JMHhSJ%(Ye)IQRA=w$k5%Wm84sxhB>{ z9ZG&HiV?!+Phse0%th-GHSeBHi^p%*R3s>&`bcEkxe|td*=ryzC8UaL3hX7vY}`Qh zs=wEbSKu+Umtfj)ATTXEa%L$Tt;-HN-r6gNdD#oxDz1IIIaCRdMbT z;`y3r9L+QhRCWBt<_$#{JGlc6uUSGhS@B^Dw<+p2l?+ z3L4sc2R^d?*sA@*^XHCFed$zoI?%k{v zro{Pp?rODBMj61vEVljX#&e}A4_aWuCap+bFwVo%7*u9H@oARji_&FM6aPMKKw)E) zC*B)@qaVtgy(9ZMhr_(UvZ*15(-r}E^1-BAODb8ibZPbb8n{xI(+C3on8=lFNdzu~ zZ}x6Ixfx`y1R$Km1RdX3O`p=KyO*)8w&Zz?)2}k_B2q#G$)GjfM5(Vu0^|892s0MM zBy4o1Eji7+Ysa+bANPEo+(GhZqC9yIvL~3bFTQ0e_tM_;Dk9o49Z|Bydc?aZ2;5by zV>~gJYpzi@5@<>P0PpYe=HA5S9yQyrVmIqsn0(8ZVh-@RZ_3q5l~ay*BI66AnBM=N zH{uY?w86-$`k#dUe{m50*Ea@$j;mrgeI76%&BhyL^lhQ>HN*9IPvEMKGd|ijQ0#|N zI?*4A-CUMA_jc28U`OKTP;7pcZ382=)7l#9upNYC^6Gx+v0bBzmkV1kNaycKe+^Op zRP_6_6!cFT#}5$1%O36Bw<#-#B_`LSmcKYceNY}}`{T%Cpw4xCEVnE=0R|2FyGOy} z+4}tM8_V>-ce1fw2~xH^q3r@0F%s@1!K#K5 zZ%fAxE#j0Omd~FOzZN}sY`f<9L2t{!r|{JTMje=$?@t_b>-G!`FSB;|3!oWn)Lr$# zGj`FZSK~OV^$-d9^fKq}2yybiD|wNM_0}GvLOW7JiJR{(ux4c9sE+} zF=$6v5rDe2Y;Z!JQ$;)P0D-9RkC3p3(qY}_$TO9vL+$!M$C+ugH^LMIU8Z0Z z8y8o@qpTo>d9QHIPnZ~0*Md_5tx!)fK4 zhfj2+d3JB#CXq%-%lG-J`O~t(Yku!4!b$qlIlv z*1>c5M%yvV8=a*Kaz)G>(Po$EYNy5YniLt(8C&}M6qd$U(oix9$py!Bi3h0|tDYpg zOX`$p5tQ(l$ppzmgSm9Hcx=RJ=d`3|2$kj}8jW~VSjJ3nnQt^{UnW#$+lLp7e$^!} zt2yWSWZn;CHlIha-dM)0sJMRcT9Itxz6G}pF|xS}UPP4$ooKe$N)YH5u9W8GPug2e zI#HO^uW?^FsnoLY3~kVcd^K;G;hia3PRC)1C`gHDGk>GMtz4NZ@Mfi4!l30|)t=g; z`|#7_fOwP`RA0%BfU-A2#7Oxue&@b6C*C4Q#O9uPuu-9f&c!49hYFg`MZ@~e@G+&m z_cv*O-V^+9==OGZhhUqjpUEHb;Wz^zC$!Nc2+VPy#pokzMkW@Xq8W!C2w5(L zJ^O~gnr`{qXgLb!Wt-se*->DPoeDug*>M_yaCL)hjcywUbuy0w2;qZm%3K}ay3WSj zz>^PgY@_I}N9-VapS*Mz=cu)r+UkFRZGzOX0|l*x+#7}0;wi$+9S8`~ew2j#o)=jSU8(d~kImI9q@;KCPUQ}+t5fStwB zA1N(4B`uM7ALPy4*0_1j_40ncz?=Dfd86x?g(z&26#KYvGNNXOI|;JgKrbuAi5`}Y zY~1{9KiK^CD;A(Ix_yEazz<7X#VaX%g2z|*{!BEtpZJ(Sr7_-kqk62e;EpE7-EVvR zIJ>cd&w3^F=UtEa<$1h&US!9_6J>afBd&b2{UizXCg{TBfO%>72m=s2!BzFtC(?e`?LEOV+0oKFCZ1=z+z{olKkk77NJv$*nLzN zcj?Y~`;q4U{&eQ5N)s;A(q|5uOh|N{{6y5z!txhSEfDd|6^)I6`lbGeL}S=j9Z)jU*1MO zKp5`Vsb?CC;~&b*GRGS3%Dm6PSaMl;{Xwnv${Jlj9|wHIcB2y+4HONE{lYxGZtvab zlSGWjE;+(IM`N(gg+lL#I9xn!QIy3I}&+C zxGgvYo*!1;i*sEPM?&q@cB|gK=f=%U-`dSN(`$^f*Gy!y^9oVMpj206*FVbgvvTB_n)?&7$yN1k8t zyG;o->tHmg?e9T=pGQc(Ng6tG4S`-|yP(fWpSv8&$Kc^|mQ*^phRiJ--3+c+Qx_UO zRGEhmk_MeH6WiJ2A>yDXB6-FSE-+?jn5>T+z&lEer9-(#Lq&g!c--K9zeh{cTpZKX zj~Vkw?F$^wtg3iapbGoYsp_1tHm4(({5u8-IVfQjZ@lnl6%Esr7d#O@kc-Dta=us} zHr$=k(44Zllf5{?JIwYf1{stUKL`xC`U#~C3Wp9h=<{+bgr+kWp>I2HCK#BOTF(lE zpEP@&<*!*JLPp-G2+@hIr_Ur(eDv%3Yviv7SbSC{xF&f`zWjWrj$oO3`lyM&qHpIc z!>(a|w)uJ7&xXt^P?Eu6IL^KjOJ&2yqgKuxY;z*&>&Vs;TOiF^xs7A~r*D{czwOGO zrQXh7c1z~?=(oVK_#{f0wo}2VBTcS;Gi9hapj~jnqHo5L zn>`gRWg$!^+I^=iG{hC#bmy=I$ISHTe6+c(#L1HtBIC1|T1;y%j+7+QO7Z=Zn22e! z1!mKrKlU?kmBawi55#(J36=Y*oQKDsmu94`@UNLZfjwTerrVTJq6)=J~w`cu@*NYOrqQ_rR=2u;K=^VN(!DImK!s=SBlUYjE^7Y_PAijbv zeQq&Xl?K~x_9I&lLIxj3+F#wDzz)q2yqHP}w?E`84rpoos0l;~3eFIkc%1`w`#tsz zrJG8)EpDvKa_N?QiM(~DT) zX$#7{lH55*UzPbaT0x)^Q-uI4;~{!lgyY2{xb>&xBXyz*{5Az~mup7F3c44st~DGzv5A8YiAiqG2p zSl-+{SX}v;hRj3Vpkos;fC2pnm4k>ujmsPFMOjQ2=3Py#L#u9fwLRJP0OGJLr4FvN zNH(PZmHR62!@5o%dXh3O60e$E-?>4n^ER6Ynt(U&@*h-S{{fk=7RQC50_0rOyh886 zjXLy?fJlbG$NCWo0Rvf^`}a30M4QrdtvO^dsM}v62$9<#y=qaL>R57!{AAg9L|5)imJrt#MWe%k=L8Y`DSlV{$AX&NL648EV3rW#i}nC@j(b|y!ozcaf%V38A`$c@1X5h?T_K(Ipjzo zURqV~*YNL?l;Ed<>cK!#Umw)%GrI5rXAogTQ+P-4)p{-j*V|hxed~a@Hl2;t@6=bT z@)5OR6ghljUMX~q-)U?`$C7$Nz_Jqr)_q2R72n4QlC0!jGm_FusbadHi>P7|4>G*^Bxz+L5H zP;F`O?7OI|woemI+$v-cr5y~0uNTE3Qn{nliyt#Am6R8Evj@lTy`UROKpRKlDAW^& zBvumB6@lGt;_me(E$nn6S&jy<4?HQCaKXeA>84Zo89xKHSU zlRl820n{u05xTb5@7ZzKj+vetIVxfPiK%;!kT21y7rhZ~*9;M6?J?i=t4m&nM2%cy zd>3Lh{cV;$Z%_Ig5VIklCHxpcZrKJO>nKRO`EXbN9XfQ?+5yG$8Im2Hnp*c@kRVfk z1V8mlMAko{4$T@o%YF2xzq1v|=~NirQ^5Ls3M2!PgleFQA&OeG&v~{(mAm%DEvAZw z@;)#u?zS|xbo8{uvT3qJme+#wvZFM4~P%U`IBbJ=gvb zVLtPk?mpUxV&mL=i%O?h{bfk_@O&AA;L-9iXwZjVo44Vs}`2bs{>1+#r;uq%Ym<8RxiY>E)g6yHPJB+?~&1)CdUGT+g2Ja{CK^{P-BCDs| z1>y(yLjPQE;%3R+UAW)o7ZN{BGjHa$pCfkEI~+MVCM1a%dhy1^xUEVYo4of>wh{&F z_?bJ_Gezlfi&gg}E^2S+Z8fIqq7$+oO|hNx^5Rpg*8PNMNXAuD1?G2bKP_<&zw03s zQeV?5*(llBl*Mlk8PgK?_B&&VJ)m| zsrMPr>#B;rGvkkG{qV!!gZ#mgrs2y+9fb$V=WokLX`XYslLC2oZDIWIdqg0IvfSH^ zBLj*k33%P(VGnC|1}{bJEr9U+8|ebNA<~&!c@w$jg{-Y$=C&moY8b5pDF3GRW<8z~ zo&jm@#G7Nl5MB2uReiMbkRDfl?P`l%_#jDJ9B<*Ii)BaT zRrXnUXS=p&*@32wEBUcOa_bi5<2l_o5ualy}RxYOsu@K+LMs!fQ zGCN)gDAR9%m-06is?ZOEL(wx=-*U$hqF7kzbX%Se&Q7&C;JR1tw!^a|n@XSSrP7 ziD969B^pen^(+>vz|z}KCd2%(09R8u;DB!4iuCImj1YBu9QgjB#}TDy)mGQ-k?L9# zODKFi23q3X621eQjNcbNI4BTB`*-OLImu2DI=rV?9`ZcAWYC$r0rIvrrIWC{|DxU* zh8|KTo-(e(6NoZIA+4!*`V!B3St$JAD`{z>Aj^UL`m{UbAuhtDP-vyaH8Q9`*2^Rc z*IQ2B`?LU}xLs-yD7v7L+Gi}3E7ri+|81ss5aXIRT*iNQ?&=g*^+EP)iRA5DGh&x2 zXNx&^5sz^|!1~C|epWea;pJBa8b?$aU54v(H5(OA)91vCQzCXbwo$-c^{h%oO~h%* z+jQozZe&-AKImZBHL8*|Y21?Tht!UWiLkmNs=TuIB;(e8jjj&s;+qGw+0zkZhaTlW zl-OsKSRcswd&D{WFe$VbLz8qPev!aS=$wl{2 zTvGMAo{g|`Z*wBOMntwzw$ZB(#N+8K7Bi2=N>g+)?P$H(*flmxu^TFC4XSMP}foK;pv+TW#t6tRB_ZO(kUgW!^X$>0@I@et2 z1Vgt!^0R;!VgDQLMeO=pbIFjm@CMO*Cj}mJwv%j)-$TG7Nd77BPBxc%i{)WOdw}in z(FG1rxG?WS5iA0C5x>2G2|E!4Zi4;+cB&3G<2T4=yBMI)BK|lN-&eaUbVXW}mYhG6H9v?|ProX62_uA8I~H!^0jM@N*Y+Jg<3KggOa*jyLXt9Y*tAm1}z$N2py z$pCTZ z`xM3tm{W8}zE-WzN6Jl}leg|#fJ@oQGH;(Li;QBVITx_GK9Tt?NpZqlD}Xx+YODL& z87z|iot9Y75`VuDSxW;T*SBOx`_fj&LrfWP;6Qw6Dk`7QpMe^s9s4-NGWEuZ$hpJ~ zeYo)V(JCOyD3T#IDSpYnka!JPIOrwHP}>;VrD8uAJrUOYCQfUf{tp0`E4kC#gyG`s z&aA*r$eKRPALDj6cV}n=ZP~IxJ4X0KI)TG}A^37Le17gQJ85TId6lH_vjUcsaM*?*= zBMz1;gk&R2wK_>nu~x=Kg7i}&I`eLi)we7nu>!C~c`fa?u6T?OLPVNiNUiJF2Pd|! zBN7SP-;cWWPxC%vr=zyLSYSWu`gwLwp%PYQIui1+XC{T=+conHF@Mf^GolND~qf1(}t@o+^z*iQ=eaIZSTx_H$ zqN~lbVS5`jr_G0f29T%a5%hFZAAq&F!5zQ0mAFmiM z`Jh?hQbnW3h9d3C6VsOK-a;TfFi5zY5GiPAlb^_Jj2oEh-0f-V@BSuuA$A{LSJH6( zwpo_c%x+BSewkPVp;*%rs94hZLaoHVzjHe%eFm?a%!LgR%@sS!pQ%A8;xB10LJ!+h zL;J?1`-Ai3y^(J@AI4g}EHencA?PZD$eH-2QrHZFW3fzT2lF0Z^|VzHo|3mag-N8x z|AN0}B3J3{ChJ%_@A)|!BlSztPRGf!@|IzfhD?(z?veE$z{^9ds^lW2E8gk;s&%ZQ zp4%rk1uOFAScFN(G9eS#pvgzncNUPGJe$V2($SveYb+}4Mu;=g^KFwm)IoMz^Wz-5 z!HO98;L8y?e~TnVo=ycuqo|#OdE&C7bWQZ|khxL7-`ax#9gEgFnMhkQ$@~}=$43oh zBQ9y6OI0Iyh}|NYuuC^ZYf$II?-@TDdxR~SRzj}22@kgwEL0WP3RxIqXt5nB+6%^B zn(Gk|8x>rQS3REX_y<6G2vNQ`@g%XRC!5x%9m+`YgkOF7MsiwO02MR8wI_MkazPbG zLyf8n6X$jI?-KX#Q-6+OXMddQdZtK27Y5AGgexbaAvJzV{4{~b2!D=#3#RX?0*SK< zT(RertPL-U{2nJ3H;c~(tT~CqScYgn{p_LcO(n;XYhCYu(|O0a%lH<~)d*nkI@5G*uFCq?$MpE&fbN~w>9uI)H9L~*wErbY$WX73WnR&AoWvX&I4 zep{I~%^uT{5+2Qa$nFPps&CSg*-C9CLJTBx{;G4d=p22&Xvb0O2V|)n@?;I?vz zR<-B&@{S$5!A^IFw2V8f>Ki&N*xLsM6KQ!$4QJw|X?B|zl)n*+LgaO6!8zGX?8tof<~yRg0Ebve*?7cUd#Do;JN6f@{y>>{GAO#aike0#;X10s;#!y zHGFel<)g4nBU>6fd4^{O@!oNJT68nxkHtJ`SegDb#V!Xm>&N4-O2s$yl(Fe#@0K%b zl+RzQamX(X`v1?)ivK1BiOj$ttzoGlusRD=MnJLY5L5Zak9Rf#yu}N?%jlX7W0_<8@Rnwh2lucfc8FXYA=3&g4FN$SMp*l%%n^-`*Xj*{onxbq7RPwnjnr zxIi9*1J$_rTZ<)hlsFVMhc)L7-u<01Ag0l@+Kc%eO=T1YLaqZW>ff~3I@YfCvCU0U zb1Txas=9>3x&9$@!?$6nkyeUGI1!>eOn2=Ak&1cWRzB4X&CKvQwkTO?jVFy=?>0j!v*$w`YCOP(eD$`XbU&SAuFC)GWaquAVQwqJh>t{hTW4?Fwf3Rj9`)3}Y4<_bex zkwc8;SAf-YzP5}@Z*PYS8kZqWM(%z1v>O-=zf(7K+jrX=62!evJ$fLck8)6QG2a<5 z>5{>sXxA&czkqy{A=cqjN%vCHSKIv--A|wF5YdSO7|0e*jbCK>HX~=P1|6_vYB}4t zhk5=1-ht#h9SgN<+mY{R8)OM2F`WKzN(`G1&eTaL~S6v~s#v=$w`_Y38 zDLvBa6oT@Jw4Ing{2BD)+MXkV`^jDLLeZ|Gb1Z5xEfMa9QY8JIoSq#%u!##~vwLh_ zKLZn=>XwV+#}!~^Uc65jLCBV%qUch47g~%X)|epPM}M9Se{D}(QM+;` zXBy>FN~MLsKtm%jmdaRb-?W=k(>gqO;Jy;PC|t7@99;&Oqf70giZ3He&ZJ%$J>x z4E8Qb>un`W-{=$)zw+TfObGetlF(MZi6u!(Cq8;zaHA4pU3xu5D@Gee$Y$@hmU}v4 zTxhW#d)ZxDTdOt6T8mN@k_6L1uiv{Z4gS|8Y#I&~&(rHTZPpDs4U%z~X7lxRB5Tqcvg$I{oH zw(kT*9w}r_Pn%@wt8Iu$ZUwKkZ5%0l;8}uOOq5geq1b+w7>TYl7@mR`Yh&htAZOl; zd#w*;tq*4wM>`F%(@3knKR_nFfkr*Tj2~e|C7W!ML1tb{Cw3n9wbu#{;dR<8Y~4L6 zMhWkf7k-wH3W>rSnESDoundkP$Ct^FC5fr+&|j&a(mX4o1&?eoUdf8y9^kKdN$rr< zVXbs|kLcGV9U6)+zep|G?{E&4EcM}XnGoVKG=zqkXU*bR7P;w_laFG#nYLzW|I=FaAF9%S2WR}( zZ^=Sf6ADH0@PD){1lbzixk;H_xTJ50Y?^Ny!yo4q%jFZ{#6BGx7`s+m6^S(al1~67 zdWjDfVe?kRVikK5ALQ&yG`2LW@|i!&fZ0P}#kx!@>GtEW^Zqq>+rZwFc;3dL|E?{{ zaV>OaO}xGEGb#5`OZ1d7J#u`1SK1DLJ#s{Irve(4S!+a!`rL#E>~vh&O1V8;vd7yV zQ~3`tXZY6aq7#?sHmOP5!K2&d!o<)KbBkX}!JE|KDdg-`{RKL%CRz>^m9?>J&dRa~ z+Ts$@kK@#$Xxk}@@-l7Jz!CH>6Ld(K^HaJc9|A`?ETU!kW_^8#l8)paAP0hU`ez|} z3zk-H<4lgU@Xm5$0?X|^!4fx^T zq)!$80T54N>Bo7T^@r$zYuUR~p&H_0!c%@@AmDC|QIph89P2Rkm)F!b-}DTtGntI8 zvB&zHV4Y`s{e@kNv`L&KwL!290;RGoxCdXREA%t=C>o`N_TilX$>fbr_F1ML2MfwOhhNZq)gxP3 zg$4Hfi(()26?y+S-4IvA!D+mdXaBP;x1GG2QFMW)P*`FQ;2S<@`rv(-XS2|7R)v_`c;=>1ic0z@#NCJhM`$LfT;>V}lfL;!gKV$gYtKhW1R;q~hV6SFRY- z85?gu-7f`V#w|B+`Jv*>K_IT4j&%>+f`gr#t$WI8i8kOG)@aave*hmfQ@`jgcftlk z^;DWF3DAiyI`5(1n+V4bXU$;dpgJY)Tc#@1>DhF2Pu}hnYbD7|{b59P@x@ZH`P;=& zdMI!Q$z0edOid#&Li6HOn0oEqqWRA~NbH3ybMgAx8^vq8iXlHsjOVQ$pK2{~N;#O@ z99rGy9aw4;z>&(rqN|aO^o1H;h|(Fi4WO~{_oza(&YexE4!yPlDuMU?FpVS`oAGPW zgi%D*iQs;P->1g*X}(n$(^#93Kv^`kE%ZGOhdV>JO90a)A|z@rTBFs=Ugv!(<5rNz z$I7_e_Vdh}z_1OrnUV{cf-{zmT+^kQ_HQ`!PnkoUW2Snpq8KQ5b2^tE?LEiKktsP zc$?0UBnKSiLGvITmZbi~uv%kwNZ_m&9ckVbh8zT6C`dcudM_ga0g`=PKUQ1!4e~Ji*SygD)p=PJYtn$Qjd6MWN6b~w6@^aJlmf_(G{1k2aA`Lc;>#lBuFVZJT(t6 zU=p3}>vzyZKGg0ZnVb6tBfos@|B$umsD^B_blDqj*Q9IfI}SJ|OMIV$GBSF`66F%J z$r`68(Hdhw&(Ca|$#);a7u~HZ(?Smk)l3LJ|8vBn`NFUh?TE1?o>IB2C+m#&GqLF0 zvL31UqAdD^SB#03BfkxUSj2#Cjf^4?UN1aKREkf@W9RaV$7Ks=OhG#17Os?9UOQcZ zaElY@Eq3`P%+UF@^_{M7`u?ER z4@?x%1+!j-;9$~nk4D4|Q@}faTjl5e%;1MY*t5Tiu1X){QV)@wE8BICMYRs&(=E zZ%jlo6wERiYT&XE0%=?WnLx(6=2DKRa zsHIeV6nf)vF?qc*V9nP*N`8`HUO?r-EwmC?VoXJIPZdNg`kP#(MBLpp)HU@lp(vJ* zC!e#@_Z+RTo%;4pjGY?!r*UODvemUp!nN@kEio5=W?!v&@i{AwQ_B|MhjDlX;?K-( z=qzY*O%PEPww@5HD+5`Fxaq1E96ESc7avR(V|Rt;DpLOLo=ne1LuR5q|5Yt9B=7RX z>8s0cP@?5CN6Q$CPKogR9CXecRfGeon8bkgoi<8QxR+xU*-LqAlE7uNIUFZiZX|1D zgEeeD4Wm6$8qvyXt3{WrhWaDAy|8_z#RE8`i+ps7-dOhz!${soleLq}gg!(pol9Q{ zO6ykD3|z&P$pLea;4L(>5r+7yiAbP*8>g4Mw{zqnb%jvsj8QBm%~0%KwtaV`S*sEK zT&h47OK+Em%xLSwzh%i*a@eg5f(ZNtwa`Io=om6`H? z0Jk$8nMuXcDv&xz_ctA0(?Xfr2Uzk8@c(>Zi%Nj%r~GMv4gY^$rSD@a!#V23I9tW!rKD5>MsvxJoScdUPLtqonmMcYTmhYV5mT zqi{&8*dOs``AcrnI7z<0{{Wb0rQv-Z=IM`In>?J3DZZFxas9u2Xa@TvH7cwr0_zUJ zrhdO89aQz<%U!S0pWZYW!g;N46+k~#xV}Ud`?)xYN#HQUG|W}oSJz1n9B!HNvD){@ zNqsLi2LtO4V_>z6<)Va*^^^~7fb5W%yb|YU*~#`>75MNw&%OCQKUwo~YgawdR4oj1 zykW|wlYZR8Je_c@3(q^qz@>kwY*v+S1&JC%A9f0|NI+4oZNsH%WL8Pm zcLWETZ5e7nnjSWu_`DDbr*359r^Op_ypD_9-tC1Ms+8sd%tLrVrv~wFA7oXM-|&O( z@=#?>eW@1MQ%@~E&L~oPBP%^fFp1UYCU?ZFeEE7P1JO!V=#1p@>Xf^}r9r@k1gmUo z#;#T{(a3Y0b_c3M4p_+2gngUR)`F&XPz((uYeB$y_jM8Av|CmVK3$YZg0NtQ=kH&f z@K%TSje2~=0pv=```^1%oGEe>Pp+l-ACUz`d~)+@9F8bNW@tiyX~2cS`_vzUJ@yHt zc<5%A*Ayy-@~K61sGp%A4~@sNT=LP-R|Y@()!SRm4od+&L%*@k57#l}W=z5?6vk2Fa&S z8pH?Wg8N;{_9BBF$p%!J14^@+8{B4H)?F>`MBounFT$n1Ox7j))>NWRDp=rrqH{aD zh>;l&APj54;i=u!Hau$eBxux=M4Tvj$#)XE9Y!Rpj~iCO_w=lovl7RjDaep4R-TmV{ny@OAbR!$ocOWVLvw zqsvn%Y?YctlT2{^j_d%rR)|l1IDp*n0qBmV%Tg7qEcYW7h&(l!f*47&$$R4y<*IIQ z)KZ4KGe`JBLxNg95G3drm~GPT65Av`*IMeUrL}X9Yehm8Cb1sGKP#M~;QtiB)6s;CnJEvZmKY@pY`u#g-L48(^~#`*J6_h_8}<-R}B% zX>osHZ!+gK;+-W<|8w#fr%JP4;@hllWD$@iGLw~c;8Bwr;;5n#o_EJ2)9nu$M*%(K z6Od10SAeX`R)(4_;N;wp1!)Q@d#F334pf?$kIZ)443^*Jlbl|~seQy!EVXGVD4+Dh zQy{Y;EFBuQE+%BkwdQt>QHxPMVIG`{mW=u{9^hzS+%Z(0s~w+2mXrRB`nQ&FpwWs% zvY|eA><|XvQ&0Mc^DBOwsLZ;8-+T zuJe!y)v#J*8E}HyUI#BDP)>CyNujWjZ>I_%t&doxzf89}iB$nAfB(wG&!dIrS5qrV zw#1Y~=X@!AA|~W+d^;%&j`1cxUg@>i4BsHSj7zE1w_QwJ13GdS8=Er%XvC@pVF@*^ z7|0xDeP>dkZNlZypyfXRIp6f1*cXq1t+auJe*jK?3EglQ#rhI!g{SX{BSaKf^q(k* zl|6MP7T0{ck-lM__)=XB41hD$+k$^m3LSWILfP8^0zs46L66_)&#+9xAIysa`=zTJ z6bGfE%#-U7uBZKhbsF_Eydt`C9~XE#8QJNQK}52a7_eNzLoMzW7D@Bp(>n)m*gjvY z*jmlLHI%nSAa!SZ(b|DdWG*S&%r(S7nYpMkrEdUnpDNCxM&D z65&90HtsgVgZZJTErK+>mDN~+W&i(9 zUJPDP1%}KG94?tUmgsy8kD`r{%bP@yp-d)_VYxX#10@GE+wLpPWND+3Z=+bv_p=t0e2I z($fw>`jMg$GTx(KeFBg=H6OrRbd;B`-nm}SWO>^LJkN3u_fnUkrAe64Y=dFT#op+@ zk^vvMVqP)6Xch^=M2~*}gXY;6Ew{9Oi=Ts}*e#r&-D$dgOB1zLO5cms4P~ zsfq^V?5wHEp8HJNmrc~$W0xIMtQ(9UD_%V-^UD6>)7&aOa*hv2iBw%atEuOD>IVFNW6c`ULp4VqC&s@=NBG$b4MC_aMS<6b z)(W|6mk+>)>uFJC5Gy$v}vK)rFc}REv=8^0JAq>M!U+=c(uD8e+h# zv*K%T%g*CuP*`8dp(BhbDuL2cB#dIzoS-ntnpK<NWtdHEE7cp7&T?OX8 zGHRMfBj6xGyzaXJ0U*+Wg_D)sSvbZ$F_i2YX6aRkzi-a0hL?P*K8z2Fw z82zeTb1O{i#YhvErvIx%qhh7ESO<;g_rB ze*YHM!9w{#f9=7-9`jLco5oqJd5N+oCN8>j+DB7kuLfeeVtbad8@d=e``as{%{8r=$cKwzp9ri-}c;(AYE=Dd}f) z#CDfdx7_LQ3dg4Ge6hb3+|6R4&EJAj4xHHBHvjbajg`!sqk#QVX^ZBJVr`L7()KGg zQAUV1w`(EQD~n~HNj#(4lK)0F$e^==C5r-Q+D%_YYQgbVnVC9_)%90^Y3_WF z&_zjUMu|1VStis%B|A}B8IJs|xo+#wjWiI>_PG4WS$7Ti5ii<7f8NLEdUd5ep5)>F z%e-3^8~5-IP`V9}8WR^n(W^7&*%IEGCKDCqQZL)Tq5+$5N*Y#cLCFpP++pu?{oZ&# zj-ta2vOJB}P4ZbKG80}Wn!fWGOTPU#@=NKQ)H+)q^qAihXvV(qBXtUw8dFI-jz}9e znjJr6zEgAQ%1a3yBoekA+*Fk9JEk*ZVx@wJ3(z-K>;D0At6q+eb=9AfVW&*3(I+); zwP6)Y^(V-J2S=8+?gHE^OKF<=_;_oA56`4_%)&Rp!$}q5NgJ?gDxz?DX`UL@|DCM< z-~B3bjBr-G$s_<}SK;T$n&9Q& z5oZ}0&A1Vx5iS<%om;TI&hgbO&wi>@M?aA_t!$4R+#|-4+QQ74#-Nr%k`&(-|IvHS zmPhz%@XG(l7DilJ7x*a~`ca!;ZDQ6z@_CqTRpQm=26h3_%fw6mu*11C!2(+z=NO4S zL-ZbSQ^$8{5f3xcSw>U2MOt8UgHap=(C=R)3v;xUmagc6szUFxp5wU}aIl@AlWA02 zae}YR5mB!64+@W;;UaHqM1}}6C+9}`1jeSE&ukWAP$Nv@?$~0u2g^sceDbcN{@#6+ zEh3WN#1*E!EG;7pGP-D5bOPDp^-H>pKKK7|1_!VRC|%-Z1{9IsWpS@6;iO=W5oO;V zVW>jL6X!qQMeKd;3!4@bcCY?&oF_laq#bDt5#YNO)?7Rr6wXPRU2QnR@osEyk=kxd z3W=%P{tmedkgjPSo5V{{M$wPGX2;|lXx1a_Fb7Am7em+sK)8e2qRxbm7o40d%nZ$0 z%0b&6)@TCh@Ba)3s6Q;ox(drHYnk2iLnr?Mq=A_qko3v$JOY!_h7|llaWQDA24VN(w{1f7zP6m91aB*djcJB?``0s?_{gc7I~)cEYqzsZ zJPCrrAzaw+q(8a)CJodQV)R_?PS0qmZBoA4dr%p_c(N5}$SGO$H)}{)tYkq+rC1L|<3NGQtyQ`T+6r!n)c6aH96FeW z$lI+^pV*Y52m|ZSgi~_d3*;T0jmWzty%~$18YV~q0B>*2(Xs3TC6453}5ejm2WL^Z`_CH6b(!oB7)Jqrq|>7=8~!W6-&V^Rj8<0U@o$nJB6`4Fz` z9~)is-EM^Oi;60rAHRkSv~egp8vZGjwzU;WHLIYwlHrvE!*Ma8>;O}a|yLbb*TF1>L~ZCPhzx1r{Eovo1YMe|?p zc60L-meQ58xSiPNHW0btn9qJ>U8!#CUnkQGhDyl2Vw>$Sm;TsbZOT^@^NJ*nbq7#?YqxzAHU;8oA6g0`eW@Ob=58}jXFKl ze%1USO<~yiWf;R;*n8HQ;~VQGIqYXEYq2#2Ws*v2x2{Gef)8FU3*~g<94R(g{)Kk@ zwT_`x;<8=R8}eI|tn(a4c&%y<_h}wBo^02kU<#8F{t&>0Aur@T|)AVJ%WB^GH>I1tyr;F3Ojf z4GivifaEB)#>k+;iVSI|{D)xcib*KSv~AjglE1|iBi@ZCIq$QB73uhEkLu-`m;U(r z=zi4USOKtr`ssQHNk7rIqd}0H*i(xKHB=oc>@kW*`QZR<+kEai^t%(%(!g8(E0El- zJyh^m>X_opIr}yKJ|Tv@o^+Y1oL$7vqr#i>EIW{5(?%<(n~-Gn zCwET0uFfDs=n(FQ;{@m6JCZAOY;saPr{FJbZ_--qlQWa%kzFPAyR+$q@0RYSKmi<|ECL0^!;fZyq$hX~LzXs9hX-4S5^egpp=Vj;f{+$Wx9x&01fjwR2JgKSb1%HwdJN)-OZ z=u4fM9ICcT_wNpIgc{STDz*0CXcl&+eE=SIBrEoCyk5tVVocHl?eD&SlDIkj#$Zvl zYO{Q1J{kDM+M`Xv4N-qyK@cZoMMjemQIz^~tX zRc(&rdJq7!<$GQz?o2$$r@JyB%gdA0VUZk|HBa&{K}{(Pm%9CvN@1mChI8EHi923; z5;@#S^Rk~Qc|U)*U&t{;`}&Pc$f@>KGVT5uw#n_G+KuJ;kD)JPFrk4I&JqMG%I zI^6?gpLw31k~eYwtv~5M}79!aixzpHzu$+*9R_7Uw*e>$G46D-uoSC{J*<| z&=AR!*J?7wsw9AjGM-YBd@Bo#XKxg#R+JYH*hS^*UX&Zx#wBWp0MlwR<2H1S;9~)w zv+p?^Jch!(o&ma%rj-FWZkq?xitokG^W9c{|;?zFAK*fbl4!vii0LY9QcC zwK2N`(_R)^5(j$-1uG`wql}CT;-w4JY;D&H-S!Z~FIe3Yq+D<%NGxik%O&;aZTWGF z1a0sYIJ(4F8wGSa8*X~94PIv}RB;gF67$uk75-6PgZ{X$yz2nw@`m zLEUR!&WXOp6qkfaS$h*C{zyU^CO#a*-=BY>J>PmLATiZsQ*w5|P9^y?95|1{z3Zlx zN+{5KJuJaY(|ix+lSTW05OSv~zs{o>du%5d;)I8r{Ce=x5<2!(0zHZ+Ez=*KvZT_M zTDnztf9~z(o=Tf(Kl3{PcZ(!P+520Y=Z{-+ho~(r(X$PhjwfeMWrSo`k zfPZ9oDT5$5*b;)eIbGoWBRO-JdOk4D%PpJH)|&UdI87{|*~w3IB34ji*aN;^`$hXQ z@hOfCZL}?tZ~J-LPHhzh@PJl-n$MZ_AvqoHLD$cDrwTt5mr-H1uf|+g%5p=TlwD>( zxG#i%^7Z~&?<&_P>iAF5)y971ke>h4?7V-kMLQ31w#CfuK8d8gm({PPTt=COH<-vD zmMa}Sy@6rSm>ouAy#h`BmQP;VPS%ih8km^y0TM>GDj*}kv1ZdQhhZ|gaN?W5L3d+* zVPD{)oy#LRO1HD6JHhWwMVM`6WDZw<x|+J9?c14oudo<7$w}W1oSvI#^W(RXCjc9V7FnD9U3ssvhqFrKw0%s$g*a*o z!&6P|0%~8UK+V;|eAb=PBZ~g{R+2}?(O$z84u>?CnM7x7>%$E`^fSZ4ppEkeE3q!N zUl?WNiEWOmXUhJGk=sa8@*lAHY@4GxbR6f)gAIWIcSQNkR_kt< zGWb?dutB8fNEa`XoVQ^h@<(h{mSGr!%jA^(Seiq%`qfRp$V3ZrUtf`GwJ+ZSg{MbHs#aEwx^{B_EWe%s6=}hzu-bR_gw)s(OZkECp zl^m*wgx%Udp&?kl?L?SB-~c$-eF7H$?ADEGs>|4id{`Fy9P-WyM`n|6&zjlY96T%6 zeBuO?C>WqiJRn-=s;hIz1w++(U~t>m8t$STQhjmcG`$?bbl(Tf@g4}F%2X@=bDqoG zibN$2j~qJQJl|pDb_Lg{bk7U50!X&jlVwL)m8pae6?+O=`~xb;R?oXD)?eOwRO9&- z{XB_|awwtxurz&=NHTbWqNC7l|IE3sYms?iHtdv zcq!Ty$mM$nEbLq^E^xn#g_;2trF`0goU)w@^MLv7PV=K40>sE{lH`@w#YXABudgWe zbTjJpt&3TNT)IcfyYcN`DrVf86KpjVtrYgQSg*epV=9qTI}mVOl{}Hz)V2m6{|~^6 zPL;(OD3Y7+>Z*RW-n*hJPol=C@s_yavzumoPeFj^YNL75G+!X!k=pXmowwN0koqDWqIqCTvU zoRR22MJB-_|9`rb{@XJ41<|X-fW;vBsTUyC9T|SRQ{WX5>!l{nUlkq`hbi0V?~(Qo z&|V(xrkv8Xqg2hGvlYvuFS~~*=&HVp|9iHHt&?;=PyOTDp!g4k4dU3F^iF#IF}`C% z5!UDR2kMFLe+VApw%Q=bCf*W_jXWiDUn{OTX?VC~nQ)X2H3C0pa)QjPcU9 zsYHzX&5LiM&~s@cH&nvMi5C+B1IK_P?IQ86i|o-kbxxo)DQMn#ejNJ?_L8aJ=nCbO zE5fffe*4y^SWKk4jlEh_<{HcoXnWtZo5b zCj5uGJh7E7Bi>B}k`d~OLya`i>i+;}JDFnVrb+sX&-q?>Bo0o_;3v(ooVC#(o*TVm z$wVM_^VYBQC}fw%dJjCMn5p3>*S1=6)7O@!QZZWgsU$FfKmgzV!yI=8G}=5oh9`Th zrq$84az(~%nd*cd@CQ*g>K<~va0E?G zvs{HUjUc;?ZB=qk1DyPz6b-k!tFf#n->1MUmZHhW`C>kKeIC8e)B2S=9$Cg&ge=9* zxrZREC-#lP=K7ZAG#xnmr2%nB5`}v5p*7Y~#;;Win4tEK{>{_R zZGEiR<^l@JbLP>MRQ%D3gm9K!zZxJ;v~z-*o|Wz~?5B{ht4JJ~CIQD$ZL6|q5|*s0 zlI}7Qkfxx6y6Mi`-_(cY{M#}O$Dl7C;~0M2m^aA*RmbF}^y=0kK^m~t-N&|TS`(v~QF;|S3f~L2H%VK#%MgEV* znDr+WDYjx*%-bk-VG9FY?|6HXKkq}C4@XKXTQ zJ@yB^2aZ3__>mP=?dnUK-Keo|^(D7RvlaaEDrY4xVy>XOz6 zAp+FKjrU&BCKK}`hIDnQV&|AAsX$Xv2DC|-l34M6WDPobewL=~-%BV*Ejxdo=S{`T ziNjD!dGibxP(DUpzpf$W;H+?D5XZLXJ14+5Y8a`0&eFRdv0BB5?nMR0T(7tu+3yW>ssFd452gkQvZMsd%s1 zHYEBc!?)l~`*|e3{)C8_0~5r$BPpa-6R6lz z@9ENx9J{I4IHP$v#OidqjT%U_6BBkN#@u-7B?)aPk0M$89iOcRUeR>_wZv|5OQZfz?a^BO_ z#$aXLYLC6+!z5v3rmb3LVfI4IF>ODlQ*Ect!qyn%uRB z5r3TE@P%ZCYtN>Y)4Ej8iqKI-1EY&6S&Poh<;mlr>+Z&fo4;yO&?!_-qjPp zczNcW^dT3p)14QhR11zWFuG51(3g#Bk4Ve<2SG%9ao}oy>#n&vdM!y+p~{QbZB>5d zMYCx!LB<+o10{-U^^J#}g11K4fu8o3%z|x#&P6{`*hfF;&d-~qGrDIaAwKkpPu)J^|M|GlbN&X8?ecNV9w5ha|wJ)k>mY=l-* zlmqAOY>{~xJ;#1Q>PBd;7M_SNPxzNE>^>vO>=bxMdE3e>#ECYw#GEXL)C?(8bW-YT zqQtUxK#O1z^~KeV4S`1R6UA+EL5&ur$Q9zmEh(n!FH<9bX=z?)`CdSds!!4hggpcS z7?dBk9xk;f`^rZ91oC(5z~sH#T+~A9WTbl!w@>3=8X9~_m1=TywT}%dCbiR@1Wkya z&f%@*Jr9N1eI4FhYQVnfCN(hXxGro1Q(Sr0mO4tgy}2>=(*;U={O`uY7N8~m?~4JH zAkiIYmgmABah_4Df8lCo>Z= zO?G4aM0D$4u^aM(ZwDd+-kqN6@_w@?(Yh%U3z{y$@N@fiYEtseFIf}4TpshS+x!)G zlds%NHuuyfh01bFuju%+Nb-n+ulJ`!OM5%=t!Rmb_@@CrxNqaW9;4-nBoj7AYs_G) z6ogDOwa@w)_@;fJ_SfIL^}v2gxt%Io7UmRU+VH-l;&j3hEG;1yAzxrolpNs${t2{D zxGg(5JccGnHdszORu5dBFJu+>KR+=G;o>glmAsHQhK4pv zm2(whUElNKejqzfI-IK-g1GY08H>FYH}&c7Xy>nZcAMooT&RG!B6q+PipOG56?%~R zyi5v_lrM>;aTrKVH>nR8>Q_pxJ|uCGNI7nm!UNWas)pme7)pvGPEMqfi#jVWYI;mB zNdJP9$wvm6N--O#+&-E9q0mO@_%EBtwE`8X9&}RT=?6>=KUi!^DAyI|nJ0S*f35;3a13Xb?G_eDo)Eg*1%-Fap zXdS^)?s;kw5(}=)Ue2#RtP&n#ms#_=r@s2A>Eu(rpb`rr+5Y`gDpRweX@2DZ$jbiZ zy@}h>g_2i9ue#fwt5Hp2j8uNj_+91B#t!yzX7+bz&%E@}jh(wdnF%Z1Jk0PqW<&50 zVF=eJ*W5YY?oj5%MZn|&_E3s|rFRMIv*(IUj=pPcv}wZ|z03LG`w*nbkoqj9S) z3s3wg2UQ*W&mJJp3u=%nCKm?7pM7mLMt-0O@d3rsXK?uUA%#tMBxlwE7Hnv`~X77}X4 z!heh;z8V~$?37uX|GY8mZ5e+UKF6bKpkJ-P^TufVBN?yI_gN{a`0^?y7zda2O`St* zZqEoh1w!fki=zNp8Z1@7`v)-7k;$4S-Onv%( zgo66|0Sx?c1A+>dLt@%8JsD#@d<^JuY-&2NuwqyO8sqb&5NSit!&amfn8}9ik*ivM z_$oO^!d&FxpZogtcmz#+3*KqFp|ojFGIM#O|FXvub%T2+@_y;?**w|0x_)A`71{k7 z#CkOP^?(;l8M>AD$3qLNJHWpg@l-afb5vNmOa@t=DjXOK84HcFk)C%R{cg#5<*~F+ z;?R&d$TC4eIYPfWqlI0u|FDu&u()D%!_k=<2w$c=Jw^dgj-NxuUwJ6$&t4@bl6p+o zQQsz(z3A|BkLsn{xNp16SPd%G*l!X>=gqbPeWD6On9^1*>yEcv$ls_xC#wQGA{s7- zT_R{SV<$_$T^HN2i3HP~m966Ye5$adAZYVWTX(=qARF591MD>A1dGte{SjfTohxqe zt`DP?ilIb4(RbypBuwvJx?)FTl>hZujg?(K_zz&!?nl&zjVNU9xFW0&eU!2IyW5?b z|FX#J=uh;*`qyK#t7dPmSz$CEGBjc+`ym?8P3CLuO6ZK#FTH|)=>HbWwba*d%-cKi zB774|aTf`RdZmqXoqLPNYvVchSci%MoW`93T$|`}lB*{@@kUx489)jc8SVVyke_Ax zl5DlWBd5{-5#u`rBeSro_Fp|b+=}$?2Yz;KoClue?0v+U-#-OzB@4Z{ZgMV4d#(q8 zYM31tfW`v7XQlrD5#*JEEFd*KoNN3OwK>#F^K*`+ABFg6>ca}@QU41d_1{v2jHM;y z3l3CbWtMtIX)c4f-$$c~V1-*(B%G6`%Ce>Y0dkhCgb-WG**q`WwpqI@0P*&ZkKlBZUcEYK zx(Z~gcA1|4J`4N8x<$fGQjqNGb~<@v4%UNxDrF_pm*+}m#&og(3I2gA;;@)dG{ZJF}+tk!`;NvyEcn=2kq&En@;Y!4*01CvhgB>Doqf}n z{my=6GbuJVdcJp(6g1lBj zv3aj4x&3_poMG$w#@Zm^;Ln#qj@Qy3y}ag=rAt=2?7s4HO4NIOkz^r#^;=vu=A18AJvPfWlLxT>= ziP=*u{R7Ig2}Ann^J3oUB|LY*o(bzw-eKrQg2Vg7r$X;3axargTGqGs5Y! z0tFLQ1387ShD%ZVob8UEE=j3Tm}j;erRneSPY?_}s9;M@a650ZakV-uTRv#ln|$fJ z_tgiF4t2!NB9wd0J52tSJ@;_#s|cg%Rt! z>u34#eUfnI3Tb}x z*eEQ;zzmh&qixJUFWpSpmKz)V^j6s`O!!aX0L7@`E)>3Wfb%k@;HSI7H?OfIYUi~{ zDOYpud6HXmmb~0EH$?+?)Q0oW#J|qU8EYl+b=z50co|H*FspUHRH$~Yb9kX@<5X&y zV}Z)kr&#ZwP(#JZ&`xB+5q3;89~dW?T#lLiYf@N8=CXfR(gsGro(OpyC1_#GF;0nX zP9%^QP8;WJ_xu<6<#kIO?wz#i{1a#1ipBXeBsWs#bBw+j+jf}0fXZYR7jCFL=jc@g zb?gyw0gxCOL_Ba+K4q{#3$E5%&)`SOGusu6`(~pdC`_r<($-X8SDk1HsoN>TIPTY< zcE#jXo!$y#7MqjTF$0wG{rPUSlS)eX6J?xp5D0MW^?~q)*l-<|3zmCVkIl2!GwKt-W%#dX91|iR%Ee*z zp&NQWJ~eC3U|?+d?QEP^KVUKM8z^nab~k6LxkHFxIwZricW{DZl% z39OoJ7YOAua!m#ul$57zRqWJe2se2+W%8E(wWJQ^YVSG&vKu%SMZ-43_swVga?vNRQH2j zRoNb=HLl@eLs5J_X{{KW583mE3{nGME>oPxo!;g$Xa589>~&1xcIa%(;=;GTD<5?r z=1Q&6uc$jz3`N%|??J2xz`%3$|I#1gDWwo8yPp58H5hqxbg!Pp^F;eC+}IzDYLbgT8~y|E4rB1R z5{;d-UhQjp!Gf-bSHt5yAU;Yq9~;n88aOS%7BEGj(U_e8^85Rnt=A}`0{mrL z+o;_nNEjG)P7CAdfIwy(>e+wWXqb%aZ_UOL>MgI_4IL`gDa_+7H?w%L=#}0;1an>; zP>?rV7WEwT5Ywpo-8LQ{>$Bh#pE8fx@HX^Tne(3HLneio=G$K-o=SE^8!Sm~FL{<= zO<^;PFONe!5fM3vnrvv5E?wdXC}qk$22uaF34=lG@4S0cS12>okOMc2B8==CyH}sA zIwE|`d@ItAnN@-Eg1;miV?>M}WvQ9`i*w701tIby4p$c47kv9H$T)-v4S|Qsmh%&E zE|RTUomIue+M*nx&%an!$ZkM#6VZRAOBWT~?Qa(KJdwt2NbS?wI=Pc7Upbc=9yI$x zyDhtXn5qcyIVY9A`?i&rB}!vq{nFdc zIgc17FbDeZ`c-=_R(|Kkr=3&dYYHLv%T1Razp)&E_&R1TUv{Vti@bprPo_p=tUl(>9p zV)2%=FteOnpEQ{=YYF2&4sn06Fx2B`Y&`C)>`*)d=h+tcTNffEM^g}edeit6tW|uS zSy1-uadm;1z=FMPp_WqUd*mLM{Rc~H^P&UUQZ8Nso(*D~Z1)JjNp>K27H!#q6ie33 zvBuTMGp$HE*yKrlQKX8Hat*X8=;M@Rtc=L)@WBfGjJbEyb_$pGFLc2IBl%Y+AYB8* zzA1hMlI3qI-=wq6xRgxy$uFqUKwdS`LR%dD-p1|z=SlQ=l9JYMzUgdvNeF|=^`Q6ic`Hv6Y`Tlk zy8D`Z#v_Dbg8UuvzM+}5lToBhYso_9D}!c;&7QzYK0}nu43so<(r5XCMV^Lut+~p- zkU^?!ftQ(cFP4ULZ@p^$^pxr8{K|Bz1`k+HJvm zsxg?4sqc7enURuE=|Ovpx!gVhwzB7CUQsB{CFeiXkBh3x4-u~zhA6(!w>ELsGTL5< zfeUWwqD_kERhyiW?32k^L(R@cbD};+ZP37jR_WfN82o7@Rgd@h2e{VMih2T-=&!CE zJ}4%GZa79mH$6fqB9*+>S>d*J`;~O*TQjD>mN!>3@=h$JbH0>nw%-i0> zuGnu{Bwm-X4RGKLd;cgb%AVx@gxWRrI7T%zN}wU4$~|C6trDu_tYsqw9%mwnnSjQ> zHDM6j{pj!ktEfWOR7*0)prNTjqk(!1R}KkxIp~_3IRtZaxG3=-Kz-UJ?TG2-4@nQ+ zn9=wqc|Qj@NHGJ%mr2-0(Zcd0+NG z!SYcvA5UDjG2=xLXx#+X*3>U+8e0KQ+B|W)7FohQr-dY~jJzE-vuZghq%b}4pW*0_ z%ls!)EqXd%tzEI)WTahbTTb8JnaIxzF_@ROH*Z{+2}1IN9rbYqcB}BCVly(=eWCnZ z$$w8HGql2)uPi^e%t9*bcT9uKEO zx?Bs*E1XkyRa=r^%*%@ua(6uqg-ySU-O9>xG!#oDE&Z8N(OV55rpbWEBrFoDso{M>VNnfu}zk4k?iy9^^rO$Qs#Rq>oFvsN%gRH z%3_o54fw4x2v66U+D=XUOY@n!GwV%CPAT8=>}N86#lD8h!#+hB;2(H;)Ce!gHtT9i5>{us zSMcQMDZWveeNHYuCtkU;8~MA(!_GpBA?_w z!9tg1#k@8H{QH`$-=NjR@R3>&xL>BY*bL5OH;J^)OK+AEZ^n@SrAtE#;v03Y4Y6cO zc_3^_`|8tV9MjO}`A6uCz1L^vN13setf!sVuZ>x*@K5^p9SfYKypMKvm_0TU*}QKC z$)&~ks@U(&KpTY(a0kDU)(4-yHeOYkr-GO1&}&J;NF#zXT%#>pI+U6N>k$zVTM;VQ z^7K(YNMY14g>qBqX!s7qrTGl@LC?Dz)|-FZ$U-BpVE|SS<p?I!<ju~EXWs>*4@cI}goF!E{!7i#P}PHyc{$mm=N zjM$$+S!zfd%WKV9`&M`9w%oON_;c|u*@$-Ofi3J}h^@Y^fn>S7KSoWx6IF5I;lysG zRpIW6O~(_uX7axLXQ_6sxqMj+6ZS^dGP{3g=I zuTO;)y;ZHnem*BI6YP#yg4`S`uy`rDKMY5qZq~)Fn=hI65t0i#`J>gZxFjfXC3N-F zvAqfZctwMCt-GdhBk!Rz89-B3dLQPw6W_<2CGW{7^KCd9*DWwf4O8R|3;EV~dJP-k zI4i>$I&>gu9)RNTK5q?2A#8uS*ge9Onst!ca=*XIiu+aV-?DvwtJnHj-w|YrK_N{? zj!t93`!+fA`R_~*q#YT(M_+0Ci+Guo+Xm2+^Ct-DQ< z-axmHYCz}95%}f$NQ7g$%GO5{4hnrD|GmP@TcY!^NTS$qmJVj!wwg;5Lfu z*jjw1TXJ0i*Wj~I70-4>KJwd-yu?R}0-E;6*uVF6_4aIy%-T-7_3rfL9vEMJn;Psh>eCKe+ufukjK-s;jKVSr~RW* zmD-IC2~Bo?+(xq@YAvko++Z%Od9(@+F~-gN-m8ssIsgh)J6oT~4_6PI!j{Kgx+@0U5RL&BbSLL|Yrlr^nZCYR&#vJJIiSP6ib3X3pJN%~XVK{a3j>@|{Xsb7;vF{)hf(g(#$mTw%Wd zo>V2=AM*5HhgtpqkHc);CS~lyP4LDyl5X2I3s-XNKU19<$$w`F7$S!PgK2C!*olSA zVQrfKYZN|5*`u^@t9#BH`Q*J(0GnP_ZJuRfy}}1OO^n6&H)xwHwLrtkMfU`M1h$PU z&9pNb==ms~`G-=UwIYJdU^!tcXkq>_BslDP_M}Md^koT|NQlx|1Iy5C z92Q?VI&|+Y{15Pg2YTl}F<9uors^M~Gc6>YYvnFBuQ+F8xBIa=mSHv%skD$GO z=mcFc>-w)M{R8xBh6S(1q3!7g%y7jJpv&hLN`z}?QRF8X;AtCb4Q>0U-ls`!u%3Rl zPoJ2S#2<1&w89~npZ|F&TfAbZKM?WtLvHy;b3PZ(n04JG@=npGG0~LR4g)!|vi# z5d&=y>%allrjG7vkFDK&*I}*|;p+`JUYa~TlW*NTeoRwx9Ozy6FZ&NrphvbHD|CU` zr5E%AH-|;!BDV7dAHxZ0d$Bx?Gdanjn}NLP)G+i6sapUPd0aSuP5O5`cIF?TK(xhf z?A~7T>be3H(QN;d3A$_hZim*>DSk*4{XJRIlIbqR%qBXsbVSHbTcLDm4q1xVz|<4l zrR9S#q&|)Yo|Uj8%lRbuQ}{tc&sG)L+m=w%I;sO3zQh${Z}aEtc=X$GLysqh6gRhr zuqxZ4rRC2tt+Ay$)%VI2kkd#P&4#{xpO3@E*>ZxLTUMox;oGWwQo_RQ`&0F?J=4y= z)Km5fH-u3%&hDV?t#$G;(D&22i|M?4Q9`&Vil~SBj}bJOK-1_8$6mMjTf*iK56Q8` zmbRW{L>W^&S%d`Ld`!VbN>=5i9wOe@*tDgz(C;df6nseA%j4>cGw%hGBJ0N*wN!Pl z7(7fw6Rln6Pmew~PuSWK09R?hkiWV)5P>$F!zG2A58jpMAVrnFNuK;N`U*iG+;SN> zReK?V0i3JgH?AuplP(ZCF~WvG=G};(P|O@Q zce2}4mAk_)(_Et*q%B!CvhsaQuM2dyESd%(|+eFfR^WuS5f z)lyQ}%+TC5)raoa^@$vbUx?v?NjewS0@2MJcXnfD-aPSwze0dR-9?+x$>f0$P`DTG zjbv}uoSJ7tssrXc>XV>9ZoGiWRMW8}vn>lX0bvZiDQOznd+*4j5LIa@JghtlliI0c zs_N-PR5*|4hjcQGIEbTH%i?HBBlI6YlP!0Xm|G;hl<0W7#^HG9H6BWQe`L(MyvtWw zQzVYJLI~!9>7#lV3x~VlVlxfFdvpR%Q?#{s9nU`x8ot77o~lz9?m;sRJ1z<%e4A$w zpoPxH<;gc2lbvX`Z<&Yvy2CR#^1ekxy7ZJMiO%Exg%;UYrgCU3w^5-Y#M`l7s7mUvhW1;?W{DO&vQ&f4hMTKtnlufH_BA4yf{zF|{=0 zC_?vM-kcj2t}k;a@QfexYCa8OwkGrY}q)+9=adSsPxx%)gXgs`?8 zl_vZ51#97|9ObyN+i`x`K*M?U#yC|%*o)bn`&)bVv0K39s6k_$ue9O}_k0@+SM+f) zpDU1fAx@H_=~$b6oNV$`b0cHc%3;1-RY4Yl{t%nZd<6qDOxLp${xoWl(Egn2Xy-+7 zH?Hq83i{SmJ>wd8Oe(K!D{<5W3os%?LP3JbRTV3_lR2R#gO|s2h1S@wD|gpde{vlP z-a)H~x^I|yBrR>9W~unKT~wu&^xaEs+TC?0V?%BX-?ulPC0RM8L8cU$A*&Izlq)Lw zel@PHCF4bRq}l`|lF<6?o%Re+4oZI4<6&5mp?l%gRJ9kAlz&$PW|Z z*!MA?#(0xvwh-v|VD85@o-Uqgg^rX8MYMa38Uv--FDAd>$icjMYKTnI)o$r6&?;jci^K4I%$&tO%( zjtDR%kSNvW@h_PRAknjOy#EDhmTMgQfES%c>m7bwOZcrru9GJ@aP$S+3df?IZQ`1= zNhr#LMP=L1Ph*_!qO5CWY}XH%l&^K& z@YYA#&dEPid-JMNdSkbz>doOdY;IdRg*nxYiydy6eC;K>PczrlNQEvmC(e{?bSsp) z*Ezum2^$}83nxu4$jMvlS1(?>Huf(vynVyMvzpDib5Jr<$%0h0Kvs}%jCs@@2JNwK z=B04#QnT&j*C-pIBdQ?Mk;B00$oiurQzN3E7;H;Evr6*rt90=jhq&wA?QUM1vH4R~ z!!e2X3cG_@wS+`!f&DQ2^}1!@w|{HuVN9a5pFxE>arg;&OA78rwhj|%(y1_OEDwnK zq2V@)2d$^Grdi{o`XQpqZ?67_Nz8vQu>V}_Y%tO=<3P8IkL5YORnXl4F2_I`KctP!#hPt=$Y+K*83Z=5SuFNx|AmQ1`8m5kJe)O|G zg8nXWyDG<`L0|%Xm}=7>Au?A8%^hr$>#?0{24&7!yE*3SRx5FkCi>n#5O*z-Aos-fi8f-fCsmdzZTaJ`W-xoNS4G)yBAZr; zFs5g5pwVTkLFwn@t`>P)-d||AW}7D^*HlneyC)rU&N^!%t-3Dl|ep5f(*Ffad|o!34P?YWtepv3=1X;ZhzoIH$e5ni!B z{|>V=Vf6rCx ztrD-QUM{&audi? znxekbgqK@Qum8sMetfxH0^^!8TVqbnj1Df(u#vx<@gO;VBauW`on^Y#2A?y3^N%1jKg;hER|Qns(*y|pUaUe!=&^A z(O+}8ZC#U}2?L|fvuMYaI~pD_QeN}+FMt(xgxtpcA^ad0=F+M&L@$p>%kMjq&2Ape z8z}58|8lh zLfv`ZO6o6}vT%1(^9g@Z{iFJN0VNEN{vHalbLRdC8#7t%cT_wl7yXU+LRV{!4YiQ# zW5dh-2#8)MOgDM9TYejL_#KVU)*G_hV|9w_RhJM#I%6}M3yr??dN zqD70llv2F76RZSxC%9{IiWGM#5K3`(FYYeEHNlH@^X|;f?z_7m?uVOC$;{3C?{l7W zerLWUg{1vs#dKKMJjGw$wE-MZldhiVmobVH-q}&QZREs@+upr{u6k%wv8y-#CuJZ7 znKv3dj}_Yaeb-TTlWX!&Wi@U_*^m$^cQr}#?&oJdHr(2|7a-gMEeD1}sNlopTjhDP z*3QmGT$E`eh3c-g1ROnBbHv&Z1@ci9_+<5+Es0vAZR%ewobEo#>3eXrimK2T3VZk3 z|22@-68}Sp3`~8Ey&MR7rVP5ROnIHUdXodUnXEq~xRh+4bide{^L2lf`peXI`4ZP0 z^oZ?w{Ma0GdrEBxU?`FeW9D?{`f}Rwo`DKEor+2?J+N2fuM~!7rg!}rjCl3}n6j)8 zsn+R-7vr(^)J`sS`FN6FbbJI)cbiwt)ie+)82F)7f1_euQMC0&5RADNysjf}%Rx0z}4 zk=a%Hsr*>FdWzts{Yg;6N<*&CD7|-Wi>TxbtSLWiO4z7S)1gwyVK&OXp}-*f%Fyzf zcCf_i?(WXGY29- z;3bQsxkW8)LlwaYXmeLeygU^DUQ4BBw87E$sh{GTa`B0#k5h3VFY&Mu?~QP+(t~lS zG3e01Izca4kI;DP-KeP%{O6hoh!;+!6DWN^ZHE(1{vnXry`sUyeZh&`=G-(wqX{}U zUpE`RJH_S-Pn-mB!t{EMF~32A%#*Op#*7@2&Jj?1sMUafm-6A)7=$#Nv&Qzt*?;OAuG>$6@~s!>+sE4Kwt2*90y8hz zHoF}K3n7Zt;>s1P$C?1?7#1ER+=WT*mT`*FC@s#C^#|VoibX)xj~QS~HXBp{7jeqj zx4z^;P*{=ADFBBcP8BGY`1kuNDmN+JH0V#S`21e~wBn)T}I6@IrOG|7U` zZ%dBzNM{pr(sykbHCe3pbMpQnyx76EVv~=CP4JIq$&KoO@11w0oK*%tyX7k-_jg#>jhfJ_Bw{MCVFbhuC z)?3YSmFJv**ruQL&0<(=?X#XA&k=uG^s8{1n21Zn3YJsfhLB0+`eL#Tv18x@Bqns) zTjg5Ts7vzvtpo|v982Py$d3XBt?cgDZ9RZZ?SOfOg}rpfciPEw`7(o!#zEGnBuSDP z5|?uBfN9del)Ko!g-hF1EQ)$IhTB?H&}xC$SGIJrg_}w&Tiy}a1gj?{-ZMlX9oSRqdWV+1B$YD*^ zCjHbCf_)3l7Ao@U&8-w)+&T2L)(~5)%MdRI9rUxsV$A9m1+X&t?Sr^i#^~3duZX-j zK$VSgC)!Ql+$j_`R@r|z%62%Q*dXCo%bWOR9XoO`ev@lis9s?VT_6SvwHSr^V&C7L zYBvZwZo{<#EOXZ^r;yr_-zrw7IuEV89yah4J0pT zw&P0kgkT8J;;%@T>FcjD`kjlPk-NqPi>0nZSy$&)PW6(F4UV~wU4}7-W_s_9YRO9% zoH%M5bz5dQRMU_P@Zg90J7jWNjipnKxP$|8HNX>QnqQ`*GKQyp@5U6A@mXNMkj#w=oN#h@r9}=jTK;;tz1I?Q*)rh8DM`I(KlQp!H5ghrCBK?^q(RS5D@&y$kV-Xa25A z5+53#$Eb2$#gt0@V=eXF>q)|G8g7K^PCkJN$XX4?(PkaBo^IvP;VSYp6NyYN(etQF zVDyxiYd9AB+ZUc0#=|}K9Zc(;R%u?j@!%8>kBE=@6zvD0jj5i(?%j42l4(d#(LClV zK`qyb-X&#et{8M(1(#ARbNv7^^#fwQr0_>kF1x&;$-J#qhR+wrkNJ^~oO^Q(6{`pu zr6~foREL%5Sz8LlLvb5+=W}PSK?GX;%@n8)zhOGl?}Hhcg(gNfMkLZ)jpUl2n&lFvUvhGPsJrG|n>53QCTFD(OrkC3fNu&TM}YDpblbo@pW1vAcaPH#S+gnM z(w$ldJ!b4yDX%7!H~G^y2cxknM1a#+?yvlnD9Rm-bA+I&B+!eUy&q~g(|~t06~d>q z6E%n);E#^ilM(j7Hh8ilz#foco{*3BeU@$AuKsgGk7Lu`wzq_R`Mfj7&ABhAhCbQc zxQrJ~QnMiJ;TDYla)mf0u=*gc;!OAuxw%o!*~D$>br?Z~Hy)dnAl?NVjww4FC-Te+ zO7zrc(22B_Lq|7bfhWW^C6~ydOham*jQrX5PEj!&(ZQP4O$6;-euXx?K)gf4rg%HJ z=x6U`gq%ocfq7+Vn17Ya{$T*{Pzw_cip*H@4utc#PCjS+(Ud_R5jjp+m>*tjRYMe| z=6>TSUg1BqIv$M2+}3bNyb+m_#y`vN;xhAlKH#Tk1;Ytj-{PMTn}V7DLnqewi0=EJ z^4#~wa0IzkmhFGmr#bU@TBAB^wT=9IQ(g2m_p6%nEz%gSjie%9yalrDIU>cJ;jM1A zaK4R_Q(L>qO1&Tpy`nON`w{Pxb1>iirz1`I}mu+$Kh5 z+o5RL17hh*PaR~kOPdgBy~lrb zi~AlrpPDKLe`M|y5pp9L9?2)`qO^O(oh_Jm7N8VsvCjOAqA)i0k)_*-eQZd^!o_{D ztLO!s8?=Kt`~70H3o>^>@z+b$jcxnpt(a7ZpqFoLVy-}YJz+qWqW?k=N8|`X*btt; zg>OcfIJHMy?JkZ&p%FJl5&qVX+RRrP5obcP{J&mKQUoU76;?t~-E^xV?D`(8a_a3& z(5c`KrrPH(OE*|m+1+*e@>J;utdR#OxO{V!=f-RPcn=oIPN`4EnfFg0Avd*z&>ezj z`9%d_R0MA^!)&_&rVl!{`|B->;RX&+g<2>ykobW@%yzG%v)wlR3RKMQQ*T$5MumJN zx+J}ukMUry3Y{$G$_=PcpQaAG=cc%d4ig8HAFWBsNx9Y1Ghws%b{3e7|2}Y^AMz*$MKe z=DKUJwAW56a$<*@*Uc?hYlm@6+_}7=6yGN^Nl3bPkK%3veSm_(Uv|-xl`etZhJ%=1 z-mJ*^12tbl3%I?0vRtF$bp47J&xRJOG=FgBgy+O1;QRPXjtmbrqB#a6Ay(wJ8dmBr zZW=zUjeinmDg^jKIf9BYBm_hc$vS^#_bv(Q6N8M8ZjOn?e;mEjFQv>sKzS~w@;udU z^q8C5HpwVoTQ`ubaP}S{me2Vq1Z1HWevu`$e6j?h8^g!CJ~*G2i8^^f{97sQYTOFi zhxXJs_xS_~w*CA_$+z8uxiTtQ#-D8z(ae6hNhekO^|g-6AA`dN$P|lJAW}%3os$`R za9oi+Xxag1$Qw4HQY;DI|6r95h(@j6*xX#!pNIX4^I=R1+b25_IbWXf(reTcpZYi* zNTsrU`F3NA`V;4;`TVnoQAUNZeUx)L=KRovxlC=vN4R!gglUXUwqNo{F2(gxzY8 zt_pb|9cX&WyCj;+aUWp7sIiw0To8s_6`k4r?*6mO@yx#M6NqnmnrK~tqvJHzqnvY& zUF`gY28*H;Ce_!4{R>(i$<<&FILN!$60)eHoFrw*|D*t!Z9j)wMP-{F2;hA}`|Wmk z6@NpiS6o_eZpxlDpO7D<`jVwNJ8JCBKZlSJV$fY5`Dx9Md9Io=3_r`*+U}fI`N_(~ zST@DV;*9P~S919WRIVReE9PCDD05bbC)18!VRNw;z;vq|Ye2D++B0A-8exXW_nH!a zcC?&a5GUL-^s>tHIW+nMsH#S_f=3ftDM3HoQ)Vb;DyNF2cebJ_GNR10IN9toN!+kr zw}t2klJj!j>IfKrx;9M*tcAGp->Z5n9p7iqmNhP3YFgUoi+Z4cx0RFn$X?U1h+>%5 zRW2Ffjv}`Ki6Pgos((k>>(uE!NOk4Qdom$yOe%?Tyh$yEd)u`5%(0sL%&V0ArQ*>1 zuMf5G_TJnpqW+4kXT_Gi32~}vOtR+t#kX{E>CZkjTM?41R}CTCPsY4+0jFarDjijX z5ONnhVQTggsLr2F?BA`kB>n6Q2w^MoSCvl1Gw8#%Cd#fwwggidiDMgkI0dvBG@YHN^^Hv} z(4iW4NR^Ev7izyELF8vTIqLeWB;UJd`wU0j4}{P1^_>eG9;f=+wwJ4 zgca2)^cm&n<)7IUmYXZb}I^{M`6o4@blDhW$Rm=V? zFQv}JiD`FD?K+>EUF$D_k+n4*?Fx=5?xv#rd0q#i<NCogjc~v^ zWR7@oB?$b?=7KrDRV4tvHHkQ}_=j-92q!DSVRZ1U>n&kWTTq%884dCi#y^Bn&ZVF; z%XWC=7x;5i7-k*qc^Mrj(iB8`8b}5`3?p3RBZ;x9-!Pisb-E@aHrMbq!v?~t{@Xo% z(cupD>)(72=0j9sOL3wgPfC2GEekFq76@^ExHo(`pTsJ&e~L2fg)| zr~-gOuK`e{hrq=E?Fk2ZD@9A2tV=UmR=s6~8|DIN)VB_Ah>G6MMQonLLc`u-t>=!b zw;co5vD087EpKu#DXomjMu9&WR5hNR($8{0b)(PGnW+3NK6hDMk+dHpZ z_AVNlgncbNm3%)t8R`@tKap2Zr`l~Jl0=G;@5Iw9VT(HBe&-9*i*8&~BeF4h-xZ}b zFD_7{Tm}c!%abLZD-;l0Y4kW7>BWM6-|yl?o#*fRbWz!?deu}>lMts<9*5h!TO-jY z9G~>Mzcf|x7l-PGY)c?Nk@hcM$s>Q6o8XuwXUfxknpHSV2Zl_5n*Ylf9?TKcr}(7z z4`D_e8uf}^`wxKzT(#p>58J&Tv_&7FeFG{iJL+KfhR~9)+su4PD&)Fsu|P!^rZwDt<$q?Lf5zbMh_Ps$E&cl2OcE00 z*z*Q|3w?*QY-?FTB`@V0Za$>#oH`@;%d|Dj zf>Z7m@cy*^Ekb|5xTR^Fw0kT8#U2c4;hd&+yb~`sTRE{e5WEwGm|uj|4)2)0UEVr| z#q2k_SR`1EFuKV4mB)8+{X=lFX=+-P4Nt!s5g(49{PNk;A#wNShWNUsv%M#Brzc-M zx$Co&(%a|HzDq5E6EtM=0)X+g@o`$gfnr&XC9IsBtGdJko8)-qQ9~BiN)i|6@OXw1 z?-BQFlG~{oThOP;rGZtQrKp;l{6^#T-|Z(_JQGD9x_3dB#-u}2n6`nm3q(iWbR%OX zesszX)CmDEY_y?W;&b9c{EsHZ4gi%7v)QrAj7Zh&mXofcddJlfNTJGHrEOm2wxj4& zUlWhM{--(Q*Ut6*L#MOh#g3)j4qrP^p5+X*Oh?N0JqTw~k9OW~v3tOj%CP)d$)C^t zps~-%uVR!^{Z4D1)_tkbXI+TH!n{t!<8Qz4AB&q9A=(<>UT{=j1x zdbI#)s(JxKF4e`O-i4-XuJn*?=rX=}#}cUCbSS)Er$cC#l2Spern-k&k}#gC5H6fY zFBGsV8oL0!eYq`zbEYwv^UK%9sR$7zx@OH0TY9NO4(c18Gy*)a9nMiJlXdMw!>fHH z;3gJu+e*4AUUn`T`o@iux|9XR8k9)z!OMtfJ)uf0|ZjW$NY$!{l)zQy4OX^MgQj|G| zzSr(DD=t{JuYr<=oq5J9eQr0%dsAdfjuMqR9a#1gbSOQ?4|3EU1P{y_iMvH1kE`>< zm$(H{s8^IUh`0(Y-^+AP@bzi zHlEn(4-locKhs^((;8^1Vj#8D)Q1QQAMu$uSJ()S!kxRP3`A}vq$kpYLI$!cSv$WU z!A4`l@)d9RahOm0dB7O~RPBs@r-6o_xaBCPX?Tp9Mj$PObQ&k}D0;>_TWD+RAi*c~ zcncg1c5>eQ?+Cc?QrcN@^s=pomj;YRK(u2-a2sK6dow<+s7LkXq>6)pjkfAR*$+sr zhQGj_gp^v!dR3QR+|PD~rg$4w@BpQ@-;=KQ*^_x&?SKji zMvd$rdd*n|I?L69EBR7isex~oZWP3lt5cRJzFB7W=!z8&%62CAx;J}uLvY>Zj7&!-iWItKbi#LEOI?dQsx#oh{hMxFt?v( z@-bEQtS(*HB!K-_u*FhML?S1n&u#9(xGOH3XGIZ}c5$PV8R!3J!Tvv8ZSRhi7n7z@ zcoS}saT_g%?8s61`My0wl@HyqdK6UojF5%R<#jED>vYXurM^WiY9JSK^fM z<7g>dA5fT7bCn%@cKs)~ud6t5=jPOe1!k@O4FDrbHKU5pc<1u&o#94J&1+-jXc5!+)Y}rLa`V-C+750h%tJ2Er+kj@w*E_W z_2h^&xjgE-D`1+MZC{q~XXBkUwFF5fn8f5x$@*&1;JXHv!@p8d%`ztO9|pIOQ@oU2 z{IK=6tvWv8Q5y6YZ?4ovad@wjt%?XW9Vvk^&|4*ZAOhL6u{k3Le(y^a-&_;t>_oZF zRYLOZjH(y3C4-wDen215fgWBXf8a}@f}qEfs5nV_5XpUiR$6Hcj`-bV^2P79Q7ZQY z_Wt602x;St6F@zitVXlE&45^+cO8moVuq5}3@2c(sG&Brqmf z-NaV=Jh^^!Lh($Y_u&F-KRoZ#B&S_!_b=2XrtEI=qq~HmW!>J+3?*xm?%|U#@-0{JUdsxJrqj`MIP1 z80Jqr&84PIeG-Nrpo^=2(qSr8z7Yeis_x<%DTJRyExgO6qCaFIVlVN&{E$1^W!XpZ zMlKM)tEskZUJ=D3kgt)cJgx~U&_Ax(yNLOk>Nb(M>zsN0S2t}b_V%>jD??N_i{yYi zyvQsLh~j>3q-{9Mf6VyDt#!E3P?9c~w%cHm>xo=D*JkvJ6Q>4qfeES?U zijGv1W7jE>gtOgx03r!~puBTWPJMmsGz5hmZ?CK$Clg$4eBw=(;9IhKY+g3a4izS(YU-}g%SI?@W4 zaX1HPyORSI@JG{4`ZjHt2>cCOd zKasNGW@pG@(|AfZ$L%@lR+d-vKOH`!q_`DFzSysdBf{`cj>~Ny{~?6#r^7QNNLZQb zEeK}h-2GIj;$bFI6^wF3?<~exjh^BHbC$ffrQNymE%zQa*Hmb&lfUrFmB%%YxcS{Y z+oVocI#`viF9!0>-`3F2dTn^juAwEqFU;8%6!j$DnA!=7sTaw_!>w2F7CdwfwU2|w z?{(YF|H&{X+to=b(0J-$JS1lj-`>`%U3-?B;eu{iFCa1|I9MEBp|$e(vZn>7+Mc*x zJva}~+IkC$-cWeo*~onli2X9!fgN%UL|DpOTeUJ8C;0S})ZVkU(D7Lj81yyZ&RLK% zee%x|z9=?u9oK&FR=~A(lU;>sj0_dD%=@e7$W}dVr*J$AtosAdF9`Q?9mfJSvRk0O zzysnt)W&sgCF4D7vTfFFz0ve*O8ZQ(c~3eEZq#%_+L5NeMyQc^oAIL{!dL2Tok=mI z?}v&L>wCRG231aldu6~Az{)KTNGc+hU20%#NB?nc^u88R>g^fe^=n(!%Llj+Nbp51 zy3+OM@i_99!6&Yyb^hzB+KfCoq}b@0@5IULup5A6`0H}GC)u;@Xw_Oy=t&E?h*Mhc z1jH0gZ1{b59VKzvjOC=N9Nu;5yuba9_Akr1+J@o0&+oG2CkG|NT5Ggv-K}-=-DP^) z!bKySt40ZjMw!M%m2ef26UW*CA8w0}O};|iJOwqSsqA!Og19}0OMN{P-{O^WV08-B z>KbkFsAZ(6icz))|{sL^PXbe(Qq}x(tg>zJ4?8H6)AYEz``s@^k6_MO`(+ZMgI|w-{Eed zxXxo1QekZ9Ui}uJdn|B$rUSbiBrk?7#4lvTu1ZKLM~)Z)q~FPQUq@QHbdFWAk{=th zM0%*m1;tUMNBVp3uH2kUj;%i8uO>^{dX4HqP}Ry%HGaXS&LEaM6M7%RhD6ot77ViX zza#*X(1^~r5zr)f^cwJD{vjaTs2JY@KG5n@Hvf(s%u_6J7{u=j!ZQuL{cfY}){zhR zhhQ3VtTzeeDLCFK*q!&$vJ-vg6e>%ETz0*XHtCH~l*a=4Oq;r6J(*@asm;0~h)iTt zoLrMJ*5%7-Yy7ZYr)kk<$P*K9iV!Bzu7Wrl4^8U>2Db1eB|)dV`;9r%M{fb(w6ak) zN4|5`blPyUe3gfD;}n%D7T_NAx?Hs=P5)Zfsn-5^W+kW(n*AEn{H+^G(cFO!6I3I5 z2;onMI(3&s#+N+F?uh{@{jBGg3TF)DazEeZZ~N-ht@t0e%=KM0*B_=f=Z19>X#n!L z|GUN(E%@zfn6iQmV!TGGrhuI9^c%Lwrcsi7{@+F%m{s?g7FM;3&e8F8bC$|KIvOpd z`g2~1i5-eA^a`&@s)9U8+9lQr8?;LP=*)#J7IedyJ9)nlo)`J3@qSj~el$x8dY{Vw~{!(eU-INz*;v$J~) z?`gno_*7r5xbFK1=W^XELvWB5z`df1TDy*z)21rzrW?V&>EqUFDY&rTClZ6iA*^@o zwpM@kIWUVfL`!+*W{3KjAo?#ECb}M*&&G)q#TCS{LSKm~;;=c1hKz?y3ZB9_Q3Ss^ zZ(JlW8IYQ(U66F#2+w@#->N9OU;e3|fnn%NPO{~^blX!gXS8KBy1t*A=x+QW@s(l& zB-Eb|OnO#PZ92dRO-~qg?|2xQYkZzQ31X%c+mKzWoNM+^ltNjhaL})w?iJ>C#KTJ1GwD z)kLpxuuYPRyxm|QfLCHn-KqR3(+4L1is0*ouprZ2iGa&@YnCFOHHtSM+NJt8x)H_4Y_eH{JC#$xzX@*koOF zxf+c19UUzCDj*|0?TO>3p3<)r?_`z0{Jp}ZG)+#GZ~jRSSZ15y25Rb?Gii=|ze72@ zrk+$HE7gVvhe?Xo?pn-FMKdJ!X94x!1bN}*w(hyPr+}!TJgZs06akj+Pa2t8TIcw4 zSy~YsNod|RFVLxv(_7Zi#^v@GP+j2( zP@k7J<#SGkd6vmAcL!lsGj0T@@4*iT1c;Xi!u%J3EtC@>FAAO8n(ZtCqEtHUVmcCJ zh6;&O!N(t)o6i%*sv{4+U%&czVh?_#+zr^ycb@7rH||U)M}EK#*6j0 zfx}!S%S%;)0+(>}98!N*T&{I`l}(!0Z*hz z>^5nppB3UIn{CPLQ9c@6VbCsi$mlLPa5Yk9r?n0u>Qa zoD310EmWyVF80~$%d76gtv@4(+I}qL{1jWh&-LEL3on9V9SgRlH1>!Q3dV;ZbosuZ z`ZUlm&q30xNtJ_p4$)dE>m4P24o&BaJ@rnhmD(K95W*zOEx7mt7&Ka&M%AH125zEp zi=N!;llKDUF_owm$u|y?oY2F#uYQhvMhO_Nz3d9iMvi0H!GO3!frE4thPH1?b#oo@ z;x#{*MR3}F@Tw|uZnhe9AVW4p(k!8`MOg36G=vT%PxAt*^0NF+?V}=rjG9A(29$J_ z)N*VP#j_v%Cd77P<-kSHZX)B8N6XE8A7``abrfWLcLI~6peTvcw>nVH_}kreLr}S@ zeimcjp}P|!et>=Z977#+GuzqQ55+rc02Za} zO|I)N_d{J|$P{T~AQEqM*jDkw}5;Q9}NlSVTPIzlY{N)eGX0H*|C zhc)N8{)G@%Rw!56lga^F_w!_Ce1j#TKC-pUMME%EUF0)cr{I5Rdk4)Sos52hOK93o z9bgEin=-@mE{cXc4O7Jg!p0#QA2-sEU4H6SI!QMi#@IKj`kbk*>*G z!eY5M-h%>8?qAE$`=;2y&dk%A&9YVF()Fp*@;xFl8T<6obRgpIW*WjdnO*`QVV4(f z`Qs8pA^9;eunIM%MpuSYZ)ye7|37Y?VwRzB0aC*JcXedL!qigL4K}zU!>^rW)w)I1mkQF; z)J2F0$xc4vGiHFN_{hurIe)(0TvU5One%(AwQZfI26saK>4yB~`qvIbuHh zdYe%c*DiziN8@hmapeEBS9?d~ov zX^}EgQcpNsE3opRKM1kaQunYfQ7524t}uiqkdX56Jftt0$Dd@UOTBIQ(RVtnA{#uHZMRy~(EdxrRIv=r8Pu5!6y5oa3VZ;mVq1yze<}Ce*NbK7n zy)R_cI}^%jQNW!i9}05+0c7D}`f3%#eEC=%^Ax60OMeexliS;M7oP_1)!!kI z2Z6L-5p<)1FF0=E*@74bmUPPiDYO0Lt~noYdL-0{zm@jKYYu7GKLOvaL=GB zK2ZtI{MlP1CYwis`M|l7RPMDLq_CDA7OK4^4ZrddF6>_!akwnfQ^VM3%yzQ6 zL`o?7{d%9y(>0GTb&?%#AayE;@;KvBHN$B}@!CgMnJ75Xb%HBq`~1`HypjH~=J;C| zst+jhlO>vzL++CLLkC7Ai^&hW7n6%pqBC4%gY;KO=)xj5yIoGC!brA8;|GDyBg?HcvX}xF8Zx~M^1}-+k6_^{}$Dd(csl%W-!RZlg4d3HOYU9QIvf9{{Y?;* z8wbvU9}>27n>u*y1@C!`MhXroR|{ zrL{@^9pEEXtCZcp=y@(Mk2oqo(;Y`SRBFLrV}fBt-5(`w0L!9haEmnc5zSlmSS z-8Qk6fp#n(XmL-<%|Y-|m?dlaq0n3n;lw|G9YUy6xjO9QAQDI^AX<^ zqbCV>%TTje*l%?GA`dq%Ev25PS~V}ju%@~Tw-L_S7SVVv=lv}xOh+i)dzz3ojD+b=&sB6>C-TM5Wxt#F^B#!z?(i?oF@We#x-7*M#q2^vhj9RoH`s zHIXFxugv#bbXJ@X`B3oV_>1+9MpP-AhHd+aFEqsi;xy7KOQhS+?XS1Jh4D~lPQwJf z_)+C!dNQ%lHZpp{XrU(s&&mUKq+Z}U10EcwPO%gqi{K&!E$xDTFe3%`XT2iU(Ej(W z_q;l(F!2Kqv{0Y>A!z3;EcrFEam}_#(9kkYSJbnF#OoJ$xiFMFI&Av=wodGOg#Wt> z_doXqGtvbJ6rWga_!0Uwvc4xQP{txyiA-oORGW74r$crM%J(~;&mq!eU*BG&3sq2tVd(e}q zGZCohTARYNF=Lexn<2!t0Wla?G{;jwR&P9xZqH-To*1~GtfCNSj0DD$4qI%KU>SM( z_zNbHa}5}F77Xs`VD&da2 zR5TK(04Pf&$7Q;5_V_gt`Sb6BpGmGc`-!UTB`~}&f5~*W=-WN`VP&0+8H#uU0{a+` zJXSL)1egeI4u?Mkx;<{DoIm5?!^;e+ZU`ANGr-*J*PDyv4o+uQ*&*Rc1V?b}NR6qj zp}!^+qanOG=1eCRyVCrP@a3Q{;R=;a9V~srNs{cer%a*I>ToUp*p$em>=;If4SbMe zKm8D$|5hBsXM4j`tqF$OIdHuu?pk6uP*2cv9Q!v^e^$8?r#_`ie-v5xAjg9|vht}9 z^lX1I6TN6i{b>Z)Ps*57E?f(441$9vG>3KtU__ejpLed_-q1Z4f?av73K z6&c@tmFtRqz=L7+8RGSy27J(4WN2thu*G61ropnH3brf%Nhq z9JN^P;tB&*TZ0TAJ-gWbrO1Y3&P9Vf;oGDxTbScZ0z6Q6pRQ^V3sFx|Fv6`ct(96V zw9)Y1p)N7CTs-LZO2E=DZn1#eM*!IPZikiX%X}WYN@nvc@;><-@x7pGK%d%dURl)7 z_WB7^0)!U3gQ?`bTXM`9V%fx&uV4gsX%G^-lQ!=(hon766IC?c0JR#e*@k6{799&s z$+XILH|(8_^veDOpBA<|iB!#CRylRtRMo}lR^O7dC9ujO z#U}r6^veHEhNIGz76l{!r|3*ZLxD}}i)$7_6XCE zSLUom1-;O*kIdPfxltB3)}d3|HFEq}_Zmx1LJj7EPofQRSQ4 znKW4ZC>2evq#QWDsT4-J2V3vvOt-DdmG~4hMfrQZHaa%yJ2=M83LzJur`Sbi6skcm z_S+maS2uSu=|3Lr2jBkH4ra3xU>b|#(II6B-N=#Hc7D)%oWW@(^%|}@T$=p{Mq`H0R7kqflk4`40XV9zhWl7?{Gr39YZpV2Ci;sp*?z$qz7aq9smxF8Jgwz48_OzRXf*bV^#! zla|(BroBzebr;7-a^^~&kAB;a3)O1&!sAKdo~Mf*Eba`(_Lyfmh7)9Q%%lehAWub& zH8&G7I^FbNN>WCzM@4$p)!c$|b+f;;H?{jQhw7$t%c(o*4!ii8H>&8H%nGgFPOW^D z`-gzn|0PWP_maq4V=ONDPi;=+8|f*#6OLzlNIM`6Q3K=3T6j7AEz%KMcbjuC-th9{5J z(9ztZYZ2YUJib`V-1w>GbA}WWnY$E-w~?8qBBT4RHL1!<-Z$tlEi_{}1s!uBP_DDd z6KPe`ZAB(|8D>Jd!+zx97&!ZStQaXCHOy#X-pTMvf(~z}BT4_gt|&ghXf_Nsd(bPL?JK{6(cTom&-OO=HmpU2PyNoeG~(*AYA9MFYdilV8rJ! z(ek!Pzn`_U-NlNRZJ>-^z1Y;TG;T${w}AeKx*_JT1g-j{(3Znv+V)k1W*#Rw2VFKr0kv&O2;)N}o7SvpfK5XnVF-10NrFT|JrkYB1;X}8G3xsldj&P=ZyBZxeu z`h6>$WK;KtfBdhOHZ}uQ>ES|ft!5;v=fmPI!c6D)pR@clW-%btL6n3jOGqgCtJ+=9nXAU&M6ssa?udi!sp8rSIwxd7+ z<>m5tvo$}OL}-(~wYj4`L4tXcQs~E@YyJ{EA@qXRYZr98Vn{8qJ}9)067c>2Hzmr- z!3?iw;lrChwcainy1(tDIio}8|0a-K8pVB(;0<6eN6WFT7^O0Z0BM6Eq@IPe;e(9{ z#h%VMd)`Vq228SMcvQCti}7dM0=lrhMG}g%oX5blm&}!96P~vP$;kdSQMbezEPx&g zL3D9Q3%I4-#xbmtnASQ#WB$SO8vYC?7UrkfPdp1_5^su7zp&RdEduofb=`*U0m>p% z<4AZNX8{O54-~0W?AXrs1K-ij_)GbwU%lCzzZIAJ3A);4Op)4FdDKn@fVwUf(I7^# z3(o3jade-aq9}iLDqO<7C-4?dab*aWy`Ko)T|hf$18R3P$={qrO+|rI3M0(baIfF0 z2Gk+TA5lM^NmNY^=)!%U$oc4UWRfdcxQ;Z#eIz6)=hyVQy2A`&@g6YD@~@b^ zNT0$mJJH-@jooA%cV0g2I}K*b+OvNPjTm5L6}wAlIZb!~Z(5U?7~}r*m`f?AmC+Ub zt~c4#c2Qe)WH`~V`dTS)_DbH;*4!Fq_~h`G7s3CA5rcCrj}*bpFZQ_M#Y^hVLuj9V z2mEPTb?Bmn$9AS1%p5!4b~owV>7?KKG=1)oZNQvPX|Y_(ci}$iBQYh=i8=ZE zKv`}6>iSEKWxYqa*toKR&F-v+i|_u2ZQjI|TMlZ1%+<^T^qv zoBXNV9B0=E37t3WHg0UEmE>4}G*|=~&NA`3Qc+zj|1mbK?YI`T$O7w_hiR(j1B2nL9y)Q!{w|+Fo>Za3Dp(D}QAySDqCZ(r zBd|<;g6iv}(hoDR4~@YtmAmuN=L#upgjKLh{avKN-CDEJ(#B@}UMEiv5vd9I+;n_j ze~A*|*4?`G7lDY3-d8qWl_>jtz3HGLz zIWNKrcTUAZ+CFbAWJ1E82xeeqnAvOUMUe zQm!%bxSuV%dPu-~RY(NB6-VvbKgw@|5OnWAGqtiP-g1>byHuzg?iJ3Xdd!G<;5eRm z9wG+g*ONd+9`&=+O&xqVw$C`$JJXF2cnJ^k&(9Xtd&wb$3qA6ToX9fbWthsc)e!cM z9IZTHmcCO1i@*lqk~kaHW}&kbZJ|}4o9ME=lSF3}>SsgNA8(b*`qxH4YRBBN1P^4$ zL}7lkYsq{0FomlPF0&-_)a2CIJL@yknNq7?OgKmanc~%PcW97~$696YLT!#C7Ki-8 z>=QOmb=g)-(@Yg~!bWlsDhCzQ?fVw?C1O%}1CIv>3$wjcPLBGJLuu(WDS|gd<<&Yo zeLOvQ0Y9}{g9W_2II-s}=a`$bOZNudqRnEg?P%Z8PF+^#$*qZMPo3v5*kcLRg#D-x z>B*{t?Zn1b=_ix#7YrxTWZgR%I&31l;Sk}zAUBuNrjY!)R{r3~+WjV|LpJY`5S@5% z{*`uMLI|TIwku9@D(w*ml zj~@M`?}KK4c1Ieq6Ske`3q1z~-V6%KMQ?xetn_Z^kv9}XCgs&J?HXJZI_G9y@D=DM z7Bllt++R4qX4m6#p?nxAAGJN?zJf9RsF^`I;NUV!lCm~F!^wTn!^i)^LLlYsB1c%a zu*0^e8A#;(wt9c+#I(fRKbXz7M8&AYLQ|oA@m^apmiC0>%LqsHNF3LNZHgTF^S>E= z|A=6A%?5+2e$%IXR3DYR+f!e7nz2>Noy&_QdG!AJL_+lhH?Tkv`(Bmy(PmtftbDGUk27lJ`>U6`qRqn^j1u08@TV><<}7Frnrz-MekKao?@4hN_EuDdtTb z$}$`y9?+FTf+pF4`as<*2Zxn zQZi=2USY|6W*dnYm2YcKim$xGo%-E2!@_%OH@f)HB$K^i#VZMrolP{{J+wc~%%lH? z?X|y4*n6HfalMUO7n&RPPZU~{wzCuFmd_+g+>Wy)5j8~F&uk7I1uFULe{Csri6Bd4 zYmO@A@_o#(TkGb@hi0W%Q@88rC)+O&DZ{U>xrlG_!rYM^(;^&WbBnoCv-Nt)(xj>u zD4oF4>Q)|Owve)*XlY-99lV0l`L&ua*15Yi-0&_no`;)Z^ztk*M@m*VbYsQ#O^m}X z_O!#qKjUGGDVoO9&ZW0ygA=C@{e!0#DJpmpRyA*Yz7Ns+>;g^?YAIBOVMcSbUMvP>kKEEIE3$X(IrspFf4@!tQ}Q%uTl+BH z^iqZp$8qJ@G9{~ zgcU%Bc<)-hZhUI)X9>Mf|B-QeG#;(%phtHHyRO}Pr`KPwx<>l&C63k_Ewt^DlXg6W zi@}gJCS_6i(Q34g8^?=NE5Va;Dn^L#jgpLJ?%QDRmY=CED%4{Z4)2) zgo%T%i!T+17NGRkh7<@!J#0#O&imaQljJDPnW`vf;?liV`R*T;8S{Anxa z^VrCy+*sz|M);nSI8(*f1g+6~?DKgss$Xz3lNO`ZLG6!EQk(8fJBhKcF8awLE!_7j z>JAmvY8q5X#Nb7|dJF7>W=wAhdHkAQG8DwS`|VLh1MJMKrFMav z6B*1b)_V>{eC%{7LkW))o%Lt?wCC;)RLbHhQv{&5nn@*vQHSyPTZ#_5Iy7TmqQQ@`|NPS!BAje4m0hSi0_4Qqpn51*Rz^dY1n(%md$;nK<<$%I9|UM(zM z-@ZA106U-c`($}_0UntWkvC7+Z@B>xh+ zy+5diE-8dtIRS2;*@N+)7W1fJtjCU5TCIUXmsfA&T^4?R)Q4(S&FJ6oVASGV96LcUj(m4)0Cr zC0|d{}2n4i_xAm*Vktfi|nS+5m*2eL^|LQP_VR+eRfk< zOXI(N-Cb8a{PB>yv`C=pR1s?kcN#<@HN#&L~YpR_W@T`ek_i zMYHAEX`7ziy*RkhSW)ZF`=3RY38{3`;W#1jPx_)pBGVD)K0yuf&Bc3B*_lwV!)XO^ znNjDPqh9 z0&czR(q}^HNME)&)^==6fZ&uzy>QxN+WE=&w_ECJ#Ga@<$?jb__rRy|qO23jdHD`_ z1Ed+*)u^<<5}Uk=8q`z-P6<$Z&a^yK-GDZtkAgU$XV7L;?O=6gdH-{;&l2qYvQo=h z)1{!slKzs4xjpA#>2c-2iT{ZmrERf?K_rRDB!ac5JRrvM$t59cIs%&tz!5P>;qd>) z=KN!WzJP;RJHe`$)Ru5>7BEKWS~KCi(b${vI<=lYL8yiiUZFdSYUK&$tikhdoU>=3 zb^8+Ur!wjj=lw8Q9=Zi0Mm)xIrjuf;ai=NK&zc?TMa+;Aq(CKt#tWyy@0m=OJ6$tD zsmdBfP_fxa?!?+@wEK`g7G5c|%&C6-;ccH6jqZ8~WncPyTPc$kiaw`srX=O?fL^`a zwtT}5R++eV)wJ6yP%v+#RwS!do3XV_FvdK3Onx}I;Yw5oR2dn3cgs>6BtdNSh{KJi z`1#T6Gb|*0PmBR|W0AmE6~&KH8xVd4+LdW#Pf{4#8#!)%aG?ckekRSJj10AZ zky6YqCyKR5-#J-oPJT~Q*$b?15-AVSFwk`Z1NtO$?0|@?WaAC`)%TJz`0VVkXAOL_ zoCy2c;fdO)e)%vH-Cp;1d-7fT?BSdx-O%Hk^6Pl2=XL|6d5$`G?dSbVE846jKRnoM0Wp(1imhB^%4)ug8%xPaqW5e$z9z z<2OV72;K{UoVZ+Y=gX0Mr@ zc>1E;jPYsbp5yG0Y}cuSmuhA}sOWwA-28!$-)?*e%FKQjk1>yY3q1@H(B5CAHDR&X z!uD%hvewLQ;EUHK9CI5vLk7q53?yx8y}rA`ppke)Nk$pTis+G*Vvz`ZFLr3zJY@ z(3U`O$QI-%(W1IPhJK-o)RRAliRvi+N)3IKAEd(0&`h=CsCU(wfqYrNNb47FZu+2S zJ~C~4D))X>jGUHkfmE&s((h2F^V>HqeXeBsV9aLD&dx$<>cSQ+XI~BanXNj8;FG8V zMl2Q|Nyk+7aqR(`xR~C!tkx42swhhg=lcBYe# z4V7Z{FDrEg_QM3AnxE3evs~FHipSQ*7G&%(pOZU$Ob%Z!=n}8qc6h$OHlNZTbT-el z?BYc+~+5b&YO* zQ}>&dM^vy~d-fdgOYRe~vx~h@q-DsnTbLNxs96aw#G}=rfF7J?^#~k_UbA;nc)k?@+nCuJLQVVv^qDAWf)_5q@dzkB*WxhLB3QIPOktv=cWb^2P3<`2Bej zW|yHO!kDpURw35hsM9!>%1HLffeQg~lZbUoi8#Jz<*4bK z$J%ML>dT5>2zVA{)9>P8Ff}kIVqUTly_j#wE!Ka}R#TNKngrEi2@QDYB#*bN z15xTMB8k6OY$dBa$45``*v;PtP46nj#IRghFm$-UY=*TE)4qpNtWn50`l9 z1oOL&7x%q9X&Q3cq_7Zh8dj*Rsf%`B8>kT6a)n9+I(HB-TfWkm|2{;X!DeeQ^lG&> zAQN?zwl$D(oWsy$e}t4W%ywMSM0`F}?(`(n)`n)_k0Uu;$>hL$thuUxQWfMp|7)m+ z=*XYKz+=$}(Gkp1fEG)g-qIo+m%LCY(J*4-M8(mXrvJX39MdQ3XP*#C29`4&C$a$~ zwb{L$AAG!y?LpN+z|CH5#LU` z(MK$?6XA2@@#XVAcw*1(FAr0(8b>P-2j4LHm6;RqQ^@MJCzy*$?nb3`H7o<$`a?hF z%&6Pbw0(7-Z;j3KepXqx*wu~h$Tz(Q!{Yp=v>GLYS&(13ZH-IbDevctWw@;au~~;m zm%2AqD=d3y1=UJ94@2C0>ajk#Gk8g)SRjKFgP=8Pe}S;zcpZz97-4Vz0<|RESd+d9 zy_Fuhth)Y^*f=8|T42;vKs-@{qzWIPNf)LZl3+Wt4>A!r$@Z~e{nQH%>5RlByqYd$ z2%bhW&O!bH`EjAg@;dsfcP+w>2mvNld&T{`AG3Pvx7RGX0H1*R%-5l-GeVSU=l_W@y+t)^Yj)9mz z>W*0xpHJtoYVl1`*RjP9-w(t>dfvI#59veqlc%91fh-~7jC(xKpLJIoypY{&Q|0BE zYUD@ZprF@mKFf-?0%K^=4h>5;J_Rwhq)%+8q&`;jM9QsIYRUXw$up%2o%|Qd+d_Hn zcl6UE2KeqOEJ#BnqKZOSo~`&B)2Deh6WF%xs&nX;Y3p)`)rJlfDY=O4exNUho*C2E zX#?W9qrIP!9v}W{`Q11-uBz))Y08--BHTJa#;TBC%E(jyBIfe8T3_5<0H*;bKvPOg)Av zs(!BELj2Hb3|2f?}UiG`o@|)LLOV)6>zU39bSTf+uQNSkqSgy+{T2`LqBfSn&wMd}> zGRFBmQP#z0G1;#zjfba=*KtE{B|@BQg3TSoU|V|$KeoEM#+GiOuj?MVQ9W?r*$l*E zv6G_ggwqR5a^{TLx*!P$^9+~wI+WO^U$29@#t2LuT19T^+6kT0Ui4R>^)3|!6Y0Eq zrh*E7aI`^#YSdVJ7R0}0RQ_<}N4zt(YEaKIDsVuH(%1b&wk2k!*>(Nj+Z$$Ovwc^sy)S^>~(2t~D({yi*?@Zm6 zIZwmLMu}T4C&apXKJ-V_4b$<3(;f}?c~TFL*k0%xipdo3IampKh4M*TU%JfC*eoo< zoZ;lN*kjXIr{ebaS)r`$oaCit#~%BoEny?9nTX2IzzM{f8SoDt!+!^0-~t9Wp2uT2_=k7&z4!W8x2J+g(i6>{jN6nQgO;LXboJGN&vvS1Q# zN27ckhCoeJR8pf&*T*=VerykYwEp)4g{5Gaz6JL9I%*if^iW;vjnwIp6hH+OL0NDW zE?VhRqEfypBtdxLAwb0yu%`EzR}e%SlepuvN_%2_%M6q7187wWz#qspzR7ykyp}2? zuHzhL7pFbZ!47o2r3cL`2X~~hg;BCO&znr=qby~4^nN{~n;pHVzZ51Vbl1L||9)yM zv|?}ZzR6ZCpzYAG?IX#gILy+~FgD|0y)ROqy`~|G%_Ox*1v7!%m=QApT$cj|1(a}t zRsVcw|GVsh-+fDw=jzlTI0O$42jKwzQvi@tlwpB`;H-doWD*13BL?w-KWg->#Z-g% zF%*1mss0!9>oA@hx8K+FATg!5x7r)o=(ck^k;xw^%UY@LAgLEe5FyYfP{kt zN2}D=VhOQ@z!)l>j9{Fu!z}QLoG0cwkF0P3&%P`xQFuELygK2lhQM+B4F0^9-HGj* z?nV_L3pqFOyVJ=^f4-_Hc_aijNs**Xe@6HZqYeiQ`p;ZQ+7S$Sk+FXgZ{pDHWcm`m z1pLCkV@)wce&di>U~0fH`~>t7$N-~uK@H&ooYhUzWLYulvcLyw2#_6wMLZo_ zoA}ZoolX!~zfkm>Yx;o*_$%KT)s$H|6)gD8PY<1puYuv4kK9-|w7G3t$#P zAQtJ^yNZD9piaQ?jSJzC^E8?Oea@m)n*&lq9AzPkJL%Y+;3`6p9jrhSh=;mwi>>~9 zhARH~H`a7)EIj_^-~x$= zzu%Jl>G=aMW6dETlzx&#GK?GQ|L+G){QWxqeQf<+>i+W({k Date: Sun, 9 Jan 2022 17:47:43 +0300 Subject: [PATCH 0058/3101] Tools: BeastH7v2 board added --- Tools/AP_Bootloader/board_types.txt | 1 + Tools/bootloaders/BeastH7v2_bl.bin | Bin 0 -> 17436 bytes Tools/bootloaders/BeastH7v2_bl.hex | 1092 +++++++++++++++++++++++++++ Tools/scripts/generate_manifest.py | 1 + 4 files changed, 1094 insertions(+) create mode 100644 Tools/bootloaders/BeastH7v2_bl.bin create mode 100644 Tools/bootloaders/BeastH7v2_bl.hex diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index d2a172b0213..e38361f30aa 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -154,6 +154,7 @@ AP_HW_SierraF405 1052 AP_HW_CarbonixL496 1053 AP_HW_MatekF405_TE 1054 AP_HW_SierraF412 1055 +AP_HW_BEASTH7v2 1056 AP_HW_BEASTF7v2 1057 AP_HW_KakuteH7Mini 1058 AP_HW_JHEMCUGSF405A 1059 diff --git a/Tools/bootloaders/BeastH7v2_bl.bin b/Tools/bootloaders/BeastH7v2_bl.bin new file mode 100644 index 0000000000000000000000000000000000000000..74ff9557445dd2b324f88b06f5b2c4d9ca40a635 GIT binary patch literal 17436 zcmd6P4R}<=x&NGx&2By*OE&0c6X5J_&_GZZ0yPNAW;dLCEE>LQ)1dWiqUCHriPW}u zYcCsu%7^zF4b_-vrAAut0|++OsBw{KtF7(b1=Byo+lr#D60K)p!=5B0=l`3t38L-& z-+Q0?+~@g+XU@Fy&dfVA@4WNQJMYXwQi$*imYD7~6Vp!-BIEHM#2vt29A?e_divtL zZ^8?%BPKt>zaVrYe2nnTGXGh6%98(n9ZJ(b<*Ryk{}*Yze$7{P{Pe;(Nn6(~3WxA&g0KpCRNOgd5JyzcYym zovnH>baumoH0SUc1v%Ts%TDA}DTI9RH*?wz=N6CGNfCl)v$xS&tn#}NPFXQhrF`2p zqawHRA?51USyTC!v-k9$haY@44IGKeIfnMnjz=^CEYNt7CuD2GOgBAgdQqF9EAvO>kk^S_Al z;y{#+gCWL)rY@rV%gFASz*T#yOH4fmpRp@<8*69wKCkjFq70012X9vSk)WHH%w2*$ znym6(IlYrn9@U7`Tv-+Wr#mN?GsM^Z9wsI?3EaLaWh_=Xqjka1ucV{B7Tj72svbDwV&Ana4!H!^WMSL}bOXl|F~u z=5e2C4KDY!%Uq1qaf76&%@b4}3}T*?Kaa42&u48Q$`j`+eaF;KF^|3Y9#hRzdR4QX z#>dpp&Asa9z-0rM4O}*G3UCT=R^Y6_S%C|hOBCYoMNDD)MeumuofqXzz^;ui&hcGQ zkuFV>^W_OypY7{aw+b{>6L7?X^J@GQ1{J}3?&LooIs10Xk+V0<`;BoW!=9dODi-I8 z%);{1B!k4cYF;d`GMjDy+?ik55g=UDmd{i#e zA(wL?mroA=B?`&IY+pIdP3Kxpzej7ej%^W1!AD`Py?Q!dK+gQ7@%>MU!cCr%eJQc- zH8^>@Tb+L`QG&y(Md%>YjVF2grt))9-u~0_v-($w=H&|_Kc6TEhk1K?bNXJQ1cv9# ziAO$;k}G}_CUd&Nw57T+thWN99B5>O^~;Fz=rHJ|vVwpf-8^#3@b=;5SapvN8^!9* zHD5Ul+ei3PBO*50E)a;`Ikufo-SqKW$_l zxck)5!5B$9eCB(NHia{|m72zsfiw>72rV9RW>w?MgTc+@OjYBzmrm+Wa&r3+-^{m@ z!oz1u8~OIdmCvqb7JM0f3#0vfG&$Ph#u-;mIsV(2z?+}FOyNjT#pIx#yv{8f`miI#Ahty*4__Z81xw1(m%q9 zA4ivqTnlu-5Z9jG{AJX-m(KJu%=FHoTXkKx9{S~*x@HS>+A^$xNTF!YrLx0 zD4q1NKJKM@&cpkO@>?D6o^(RTFVyk(?N8sAjrn_RXp5K}$KvOSa{15-F@5hNQ983- zDxF#F+@1R|vzeRYx9?9IFpbabl5^kJTes-6r2XJq+L6-m(k1Z^_Pa$>j5iU*JH*@5 zn$sZJ=hc|LinPSYc{&5-=+kK$J}gv|@~NFW^*q%dPY&_z_C;BROU@mC zo_T7e>QqQ*s*#wkMR*=zDCMhAVj`xwX-Fe}D3zFc5n>3mOe)1AZbEz&@+tf~z`O5b z$Q7TREyL>gA<1X=Ppw+&eP68(rl`#Ij}8&nEyG)fYVw$HhbJpit>zQPF<0c|W2&&b zPz;wb1;$94?+MjNnpD!vQq6WhGoPLFL8K}^+yGku7LMjR6D1*PTf7SSY=sO$|LX0f zU^ll^U61yb2fyByS#OK~T3dX9A(HAN0&Qucom1oOlltQJ2TrFnB`kVU+7fi)3JmdOStaCnr9O_F#OQ5U}?UP2&ioWV}XVL?VO4 zMdXGu3O`I}icucyA;jRb$p+XA?Y1Tld%|4GDEU1^XegsJD6Qfy zEA2^$n8{5}pVH-AmMdSrt@mtt%58FP#;zfg@S3L}{HyJ+c~T;bBV(pvge|>IrudRG z4Bz3&3zwYX5Zl5YJ#9ry)27gkj?(GOwdrC;X@}?1qf>{N4b^$y>*PeDGV2GU#Bn@Y ziX&0lCmwQQ_Mt6i&9@?@1tP}f*K2}O=8)S_w;38OMD;Zy#Umv%L zt=4hzZ{qj}Te&LG6ep`z(UK@foaU|;4;OytrZ-kIMMF{8a>UhDIz4-c$xO$d*D^$w zGjs0+r)=GTg`myV!(8)al*~><%%ZD@6*;lhW*K5~$*kc>Lm6Aji7g&B%&AQ7aI~R} zt^?;XgSbt+RNRG`YI@~L@qP84=HI3hlb6+} z3^b)2^!z>y-3Hv75y%={wfZ+HBdpK1388`Vi>?v6ri!~RS$4_w)XJW7ewIk%)#tB} zE#-Y`vv5@9i@gfV9YM?}|2#CgqJZX)(!7jvnwwFv+v6NBQ5FB7C|%;hRFa8O_eyck zr?YX^C?O`dCzHHI<9s4Kt;>J9&mh`1r6pG|&H@=>XaKv_VHLGRwPuhQ^dw1V3Fu6$ zXG)LOABz$~y4nwZc{a6CcF3r6iT34LLlfs~Th~^f{MTNOQq(T|S~*rP$6r*gtzIt~ z`unnt1CWJ zt4eMdy!|Tq^MKtrr#2^?t(Y=1LXb$|Lt}Z!tq7#USfvOYh26wYaq(aSwPCfyyW?k? z4-Kh(Nt?GSP8zhtTjO}tOMPG6yBn)0JzEh>^CF8$z8w`KMC&zC(9d0_mO01Ff*+ z)hb3weU(yu_JQT%BkHZ@97xJd!#jcbJn(8Xc&OmsZ8{u^Uaq)}X0&*xc9ryhqDg*i z0#7W^=Tu5BALe7(gzO?tjA~;>1yMd#`B*V9G@a^81;#&=>dUat6i(e{@;SdxcQG^C z``$~Gx6dIggsJdvaw80E$X(wje&DBYNkXIPI z3d40sp7Feh4+qs-U9O(Z&f(}1=s&Dg=sVhH{k#*C zyhNG7rZVnSZqV<2`%sYX(H5M?W0JeYO$Pn$!)kCY2ia1)rixo+UQ8FC@sOkU%D)I) z7Wk2^%VRuh6=!4B{A9EvP*~dGpLBA{i37RwM7sL!9KA;TKz2dvtRMYQzNYjke}`Nk zuF36?ROoVAOBy{Te-v$!TRhNnCA^p^P8BJ23Dm!(Q=cs^M7d?7b)a77pM27CV$VQ| zX`YTP8TE+WIVWW;u^99{rL`C6Lt8Z_YhHyGKExU~W1W%wUO(GTB)7CxdR=Ptm-tyf zS8MJub!`cF11*rZE%KJ`EiyB2i&T^6@#i#n0$c!cc&u0U`iY$=*J(sB^-z1o1U=sX zKL9UI7ODL1k@s|V$WH~BMIF+rybk}=O%71Z51o=b{HGwbg4l{3h?Xo68+zvI+R>xRPD z0xm|}J&vRB3xH`}Y7#e>!rXYdx+HEMEq60uYE?j2_V7LqTFompNxc%|NnzbY0(m2j>xLDajuS{{%42`)RTqc)=(?9xr!& zvfMmct_*Nk-IZV5WswbH4*J7{8KJm`L7#>rS$8wSP1~E@ygME-V1$g4sj)a6eG%?c zcdf=Ke^4L??k36uLxNb`<6fsFPM{6STRLZ(yRv-DO!hKfq&zmk;+;vZ8V2q&3e!yXW%ISfQ)46Ynm{f* zq5|lg9b@*<_=7Q4EbnC6=jys`x0+lT+(D&hM6L;B$z=9mIClrL$l{JcYJ%+PC?S@) z#c0Xy{`c`#(4%~dyz*{tU<>!2+#a}DUfRjqC99T@$C9gwQBI8t(4$yWx~?nkq4UTn ze|?GWc`la3mNQ%fUAyq3FWTWb<{yr5jo!wFvSR4So=+Rxj1uW4gxjw7 z!OkfRhgP@K*xN{AW)AJ-(smf-+b9XmQk5yH+kit`hZ|?bQzS*hNY*uneH=e(Gwn`Om19roV|i5{~la& zoFKkG$l&{LL1L#gI?gBKmh-fimd+GlTgy_MwD%1?`VL(1r|>TIeqon-pUZoibw0JB z#Zw&WSi5T|=-RV^6jJ*)cxE(6X@4F}b)cp|_iSSC*igHHO3tcri?^YtYp`)MQ96TN zgYL~0;VyU)UqPR@xR}DhG#K;Fasva&pJor=!kIe4#m#lzKY$17p;VsFOMSR&}S8E88*Gl&iEI zQutgLSQn4p(%zH8r^Bq?|J~86eaRjX=M!jut631ID8WqVeP(i)QP?4-ASKLF@7RGW zeY?^7wb3hldl1(}8RcX3OMM)qaC1E40PVE}^_IkcbikG(h-@FD93I(#a-AdVFO*aF ze^svbK<;?CT_bBy?%t8=X!eD9Erbp?7jnOvb#aV~reDB+RcqPU zDcIC=a30M^C_pIgq7u%FQwwJufB#cXxvqfz;hR2dp$^j2bZ7)K(iy{Jfj?%a`ZsGKlvd zOj~9SI|Iz#)JmeP9ile)M;ebi9R|MN_917WI$*@U2OC9GqqJU*KxH5UKDiQqVW3fN z!>G99NxN-(h@Oh>(-`L*KRk_hC3o_%#BJRqMCV2lZ40Bh`GMtw*f^`*iTv|cTxA&`zvsATa zaCc?_V`JEDxaE>=FO_;mwqpMl;FnSOXf;*#S!)3j45&nER`ve+0p-F!O^5T zFh`W*wJ)ZYCr`bw2}AFqUoObI?(5)&4GQ&-n90d+|LM6mi-D*9hUv>-ikpb=DTjuXt{e>ilo289@en-ET(d z3BDk3yle;J+XBzZKlglm^u1_p;AMDu9+I84|C;-Vv{z;e=7g-)*1V)w;Hg&-KS=q8 zyF)kU`>tFgue3FLMvhZEoc3{x#5&l!h{FTRyh-ke-NSm-j5%z;t1h17!MAvhvC^yN z%{hWREAgu4Ijn+po#yJgTA8=pY0c(lBY6O;?mf**C270EUa-JKg_GiND@Xl+r zFG(qej}5l0sS7iw^OX8Qem`!Dn0hKYQ+yssfK{rIERt8^k^9PP^t73j0}gVL61K^# zv|O@^--XmmuO;nJdU6ottsW97?%`yobd$6cxhuxvQG<_*WwSN@Hb3ah>-S^L;KY}d zH?@oNv$Jfz6!C~u9AfP^)mwcwafY~3&A#Plp0r4{ztx{tYmk;ojglz`T!o!->Sb&F zvvR&IBZocqk?jhZKl!+aJxaaLpA0`MKkkViUneh+ejrWPSLPkkucdu*YVJO1#E+99 z=0lg~U)z>B#Z_wdj7E<+T!QtNhBdoFYV$x|nAhR=Ympn}^wJgyH}1ZMvNV|pZJt{H zD(NkWi%=ZSCoi|i+hnd(sBe*sl`HGBY%QMXiN|bu?yS~OOP&>ddi>=HIiR*KT_2QR zNseNUW~GEV1ZGxBIc^eB!cg8&#?5XE@#SRTh<@hC_Yl)@$a6mI#M!_(+4;Q6EGEtzvHuV|zuChax8baD`X;6b_&i=^ z=TO~rL9dl-i6z6y6$YO|i25bX(8z{amtg;3luM>^ms~O}DJ_#L=?UQIupmyVq$iC( z4rieDCqv2F=AL9JBXEa?;?ZP^L+b6JFQU~L!#xO;KGjbd%Vvui6@1J@v_y+$i2a`B zV`h?Cp(P&B41>Sl$V2i|Dw2AuDMUgCuOb5F!CuffgwTUP?Kl%8Z!Gcn`QILb1va1C zl-_NHe`6kY)v?4+aL;u%0xgF1K=pw^%mDVj^Mw_^Jw!F>``T=HfvN5`b(+6*?oi#8 zla9ZyHU|-!^Q+!hTX!QIymFJ+eOUut({!g!u5qDf=9O1QjE)l6&tr+Q^9|kasl;Vm zV4Pp78W+EU7aRF1x42zRanZT>OVs43&`Zuc-`f3D-ejLeWoEGRQd3tgefwrs<@0hj zQEP<4T%HoK&NnUSjb7HkEMVq`u)mi=2IzQKBi5p^8R@t~Sq7NCGJwtwtT3LB;8lfv zrBQ`f7CJ^CD@|RKaBK5yH|~tJbYH(5G4g$Bl{FF@+cvL8I;LjWj~Y|@QYw2@-cE5e zrt?2}9%`e$lUcje)HMY(&f8-Cl_c)ELjd zi!&Hbu3Dl;L#uO%l2En8gZf$4?A8*8Lx)xKTrF{<_H@j|TWy*b;#VLz=uii_!F)|-_BZr$2c58|2v_-i^r85e7^tVQ%cNgk| zg2N6uU=(G96kHuP#5Q^Dm5%Zj4}7zsR*@Ur@`Hy%$Hce9cCiEf*rxB-85$iWAIoF* z4Dgfx06F6@GM3{c=Zxbj5wa9R=5U0*(|OEugUShU{cVbtP{wGzWO;fP;G~JxJ4b|0 zEn(K^tqS_IK$yQTs%e=d17kANU?(ea^Sx5v_cs-Eu7VW(pD_lOwYdjpFRTU1GvzgK zL6;Y{lwX+QJ9^06)r=j$+*6uI4y{rJg4bf1ClAG=%iW|p)932S+mO#2-p$*Pw>ED> zX`GbitxaD>>pJD5hrIBOIPp4(QgDF+-jdxy4tZ5!Ii&!Zrtf3Q*=kD5*u^*)+=gq3 zBO241d>3VbWN3*q+HUMn>^=sX0BD~DUV%)T|UB~I$B;n?5I*P!gyDkb=XQgzwlbl-b&scKsMbWG-GZTJ93tk`)=)#Z!L zc=t6NrPdDREwybZ%^3FkID*nO(er7zVZuA3tci3@z@xuDnn>fNlbOQfubX~^*G)Wq z-Lyx4-IOx9K(vcDoF+zQA${G{t4bTI^3ILFZTg!oKa}Fc8!7r4P#NzJ-NQ_k^iEuN-r_MFYbaZZxrUb@{cM!_3gS^~DKXLac@qIX9v5-o9s=IwX7>{^-2?dnkA<1{ zGf%Hda_}+?!ITO(7chb;{clIB6vpy~npf=Vq23Jm=&PV}PFCeroi4HdRn2xU;oeor z;oD@;6mDCZc8tM2-~3`S%*33h8Rm#O-%5Sb&kY#3*Hp9g0&XOWM0UWG@@Lf=c=6B+ z1NUWGj{g!j1CNQn#2c0xl@E2t_{<|JImn*a7vob0qti!%xz){8({I@9I5b84z+r|8!7Wmw%>;LaK~|9Luz{wA5#hRDuoH&$I%cR&kG_i zC^+;y9d?Z+sz!=^r=()EOp1vUH=Poo>kd1eQdOL{bkr3ENa6W@#+e}$L32JcToiZ@ znv#y`X}~m(Tvj0dQqmHFwoZOSHOueen~ji%^Yt~jQR<*pT~QtGJ-k>G9a2Njblft% z70E%*ub=4o#*2Cm_-j2!`a;hyz;wh8C3s=PNm#Epw6XT!!O0!tRV z4pV-b5UX93gLo-rhduXws`c|V_;$^>UsIT@2X)zd19F0K6z~JyET-s559U08uL_nJ0^YdW=~c{JhdBM1( znKSr-$sATx;f?^A?Fm=zFvP04YrJci50^8}cfkQ}IOD7Zz052AWhrr12L7a)7aC$q zI5K-tIBf@y*9+TsK5SxUZ3|U;!)mMfMr+Aw)-y{ythP#mSOe+!;TU@3%H82TeebFn zUPG*$Tf;;zP&W)trPQnGotU-iVANW*Kgth?7RYa*BTne*FZ`+}j?*0X67rk4%u1BH zWENVWGw2=rGdPE{XPkK1VZ-2WkwWi_ou>_kf1~>Yct?Uff(aqROz^-GI!DNo(ND}+ z2zx*^_VZ>}1+%6an%T{V@tu(!VgwfUNBRFF54wl>gPc>JE?XoY&gyh7s6KLa{Y8Ez zV3ChWIoCZMhfL#-c)* z1BNM_9673*ifL*1JvN6SC)pv`9ILzvS#=mGr{7(1IK>3dE$uEv5 zXUIra?T&7%+%GqKCLPVPHGP^*l9oHQRJ6oSZ7X&oI(P6+4D)B!ZFj=rS=aancwuP? zUPxN#;&ZoP?STqbe51iLtx~XMcfU%m%hX#Xq9Xt>`H0Zu6Ekai&GZ>0p}4*zOw9bi_6e=&9yU`~hI$3A7bK3<1^HntD2L5Ck4s|TE-!`sIS z0ULF=Y3xge>oe)_PsbWKckFk;&u}hDwrJ9x0i0}+)z$${ws?N*D!|DW&yD>pWuir8 zY%kzsi>Jo|fRinLG&UP>vcM(o;u9vH0nTu zuKtOcXRc0*TJS=ZMABqqAs1t~pNH6ntTe_oL)Wx)KC%7-EY_{*uv>m=fJ`P1v-qgT z6ruN6ZLpBqq#F6s(p#j|XqHaNBtNp0jWO(&&;u$~ep*J1$@qjy`TekNy6CBY+gQ)1 zCbyRO(aUtNPU8M~9Jl^ux>r*z!3~s5iBRnpCzjU{Q{8$Ie&INN;WzM$oc}@jalBsM z)x{Tak24$L9~BlS?>IfLPS5Md`J$FvmwYvleDgt12=~=rhjZBZpCH{#1RN zC)xTV8ugqmU3&Z|ZSC!-ORNz`6MIHF>c3ZdbLpDWj#}Ijz-P=p&KEHod;Q!9ZzoP7 zHP!aka+oKsj&FZMpQ}cFuDo?P7ZJ9e&d(9aQ%`4+IFd8fGRm8OUA zHV0m76WxW#WiN=os;|OZ96HOpF=v#+E`82+*Y6uh;iC0RY)|6EGedM=m3zVvb93KT z`NAhWZhXh*>%we()#}eQI`=m$J#mz_^2g{b@fyt7JHs9I*Op#ax(G9dxASmXY;d; zemXxP>YF7F#;2zMGrsD&3A|7C;@w#j-kkMH=>f0b7vO4l2E4WDxT~Rh!PGU!=RTv| zSy+Bd-6o{LLfD40$2P$c*J>S#k)22%Q%~x-Cy{Hi({d*Jj7q?B>5ROiO)F>NWubGE z2dEg(5zPi@HJ~0X2T%i`m$XX(Jpkwh?GiwH0qxha0lfiepJoAc642Ay6hP+zJ*kC5|?KOAQOf^OjO+pVpDx1wD=25zhQ#D)?@D=CRiMX={vq0sZIJ0+Q%)5zbfwbwE5ct z&HVX3n+$KwMw_(4XWNh!cW>syRq)=})}q{6=TeB$(4$(T+$dvI0 z(`Y(yW=svk5O{G`nkr3QE%FpO-A!N0v`EfQ171Vpg)mNC$`f2B;zzW5KznkVSiqrdeN{oQ?`?4>?) z*WWeZYaq&_t8eW{g}^~hV_ zTGoM-p|S<3FjC3e;@ja%oZyS?qIT}_v`Mune@Ug~cGSMTwakeWT16_1)JdN2%dE8A zfxJ6f%YK0rT16_1R4r1bO3Mc1Z9qMw&?-`4q>`=9yD+vCyQ#~BT`e?}VmAt-iB}Mt z5dQ{oD&iLqry)Lo_)^5rA})aU&*X;f(oOdpEph+oXyS+JXrgRnG%*8kY(z^e8l`1$ zPJETxG|WP_6t`~?dRt(|+j_jOuj=p&hON2OhT%k{3-K$9LNc4arOz^FS+b@;w9GYx zGk=#bLN(!p8wtHHqwl3xt-ii!UisBamMkIT;QXSiu3k_?=-(sy_aA;9x&;590YW#z z9)wPWClIzH{1D+y1X}S4yg}TK@FqehLJz_|gr6ZaBG~axk(Ptx>wmZV19wnhc?tZ| zxL>CIqpGs14o?p*W_qehR+i4Z!h=VOtCp^;C|NcW+0f#B_40-QW4el zzt5o9-=2 Qx`Fuxyqnr#|H0q?1#Nhkvj6}9 literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/BeastH7v2_bl.hex b/Tools/bootloaders/BeastH7v2_bl.hex new file mode 100644 index 00000000000..eef22214e3e --- /dev/null +++ b/Tools/bootloaders/BeastH7v2_bl.hex @@ -0,0 +1,1092 @@ +:020000040800F2 +:1000000000060020A1020008791000087D100008F9 +:10001000D11000087D100008A5100008A3020008F8 +:10002000A3020008A3020008A3020008292A00086E +:10003000A3020008A3020008A3020008A30200080C +:10004000A3020008A3020008A3020008A3020008FC +:10005000A3020008A3020008353E0008613E000824 +:100060008D3E0008B93E0008E53E0008A3020008E6 +:10007000A3020008A3020008A3020008A3020008CC +:10008000A3020008A3020008A3020008A3020008BC +:10009000A3020008A3020008A3020008113F000801 +:1000A000A3020008A3020008A3020008A30200089C +:1000B000A3020008A3020008A3020008A30200088C +:1000C000A3020008A3020008A3020008A30200087C +:1000D000A302000861110008A3020008A30200089F +:1000E000793F0008A3020008A3020008A302000849 +:1000F000A3020008A3020008A3020008A30200084C +:10010000A3020008A3020008C13A0008A3020008E5 +:10011000A3020008A3020008A3020008A30200082B +:10012000A3020008A3020008A3020008A30200081B +:10013000A3020008A3020008A3020008A30200080B +:10014000A3020008A3020008A3020008A3020008FB +:10015000A3020008A3020008A3020008A3020008EB +:10016000A3020008A3020008A3020008A3020008DB +:10017000A302000861350008A3020008A3020008DA +:10018000A3020008A3020008A3020008A3020008BB +:10019000A3020008A3020008A3020008A3020008AB +:1001A000A3020008A3020008A3020008A30200089B +:1001B000A3020008A3020008A3020008A30200088B +:1001C000A3020008A3020008A3020008A30200087B +:1001D000A30200084D350008A3020008A30200088E +:1001E000A3020008A3020008A3020008A30200085B +:1001F000A3020008A3020008A3020008A30200084B +:10020000A3020008A3020008A3020008A30200083A +:10021000A3020008A3020008A3020008A30200082A +:10022000A3020008A3020008A3020008A30200081A +:10023000A3020008A3020008A3020008A30200080A +:10024000A3020008A3020008A3020008A3020008FA +:10025000A3020008A3020008A3020008A3020008EA +:10026000A3020008A3020008A3020008A3020008DA +:10027000A3020008A3020008A3020008A3020008CA +:10028000A3020008A3020008A3020008A3020008BA +:10029000A3020008A3020008A3020008A3020008AA +:1002A00002E000F000F8FEE772B6394880F30888F3 +:1002B000384880F3098838484EF60851CEF20001DC +:1002C000086040F20000CCF200004EF63471CEF22D +:1002D00000010860BFF34F8FBFF36F8F40F2000043 +:1002E000C0F2F0004EF68851CEF200010860BFF374 +:1002F0004F8FBFF36F8F4FF00000E1EE100A4EF604 +:100300003C71CEF200010860062080F31488BFF330 +:100310006F8F02F0C7FB03F051FB4FF055301F49C0 +:100320001B4A91423CBF41F8040BFAE71C49194AA9 +:1003300091423CBF41F8040BFAE71A491A4A1B4B99 +:100340009A423EBF51F8040B42F8040BF8E7002034 +:100350001749184A91423CBF41F8040BFAE702F0F2 +:10036000E5FB03F09FFB144C144DAC4203DA54F848 +:10037000041B8847F9E700F03FF8114C114DAC42DF +:1003800003DA54F8041B8847F9E702F0CDBB0000FC +:1003900000060020002200200000000800000020CD +:1003A00000060020D843000800220020442200203C +:1003B0004822002060330020A0020008A0020008AC +:1003C000A0020008A00200082DE9F04F2DED108AD0 +:1003D000C1F80CD0D0F80CD0BDEC108ABDE8F08F7D +:1003E000002383F311882846A047002001F078FFFE +:1003F00001F094FE00DFFEE738B51F4C0123002515 +:1004000023700423A57063701A23E570257165714C +:10041000A571E57125726572A372E57200F008FDA1 +:1004200020B10E2325726572A372E57202F0BEFA46 +:10043000044602F0F3FA0546D0B9104B9C4219D09D +:1004400001339C4241F2883412BF05460024012545 +:10045000002002F0B5FA0DB100F05AF800F0B2FD3C +:1004600000F020FC204600F013F900F051F8F9E705 +:100470000024EDE70446EBE748220020010007B026 +:1004800008B500F08DFBA0F120035842584108BD8B +:10049000054B07B51B88022101A8ADF8043000F018 +:1004A000EBFB03B05DF804FB8040000810B520238F +:1004B00083F311881248C3680BB101F097FF002342 +:1004C000104A4FF47A710E4801F054FF002383F371 +:1004D00011880D4C236813B12368013B23606368C6 +:1004E00013B16368013B6360084B1B7833B96368E1 +:1004F00023B9022000F0D6FC3223636010BD00BF98 +:1005000054220020AD04000870230020682200203F +:10051000554B56492DE9F04153F8042F013201D1D2 +:10052000BDE8F0818B42F7D1514C524B22689A4280 +:1005300040F29880504B9B6803F1006303F5003351 +:100540009A4280F08F80002000F0D6FB02204B4BB7 +:10055000187000F09DFC4A4BD3F8E8200022C3F845 +:10056000E820D3F81011C3F81021D3F81011D3F8F4 +:10057000EC10C3F8EC20D3F81411C3F81421D3F80D +:100580001411D3F8F010C3F8F020D3F81811C3F801 +:100590001821D3F81811D3F8801041F00061C3F886 +:1005A0008010D3F8801021F00061C3F88010D3F8D8 +:1005B0008010D3F8801041F00071C3F88010D3F898 +:1005C000801021F00071C3F88010D3F8803072B62B +:1005D0002C4B2D490B601D682468BFF34F8FBFF370 +:1005E0006F8F2A4BC3F88420BFF34F8F5A6922F4D0 +:1005F00080325A61BFF34F8FD3F8802043F6E07EFC +:10060000C2F3C906C2F34E32B707520102EA0E081E +:1006100038463146013948EA000C00F14040B1F15A +:10062000FF3FC3F874C2F5D1203A12F1200FEDD18B +:10063000BFF34F8FBFF36F8FBFF34F8FBFF36F8F3A +:100640005A6922F400325A610022C3F85022BFF3E3 +:100650004F8FBFF36F8F202383F31188AD4685F34F +:1006600008882047BDE8F081FCFF01081C00020853 +:1006700004000208FFFF0108482200206822002031 +:10068000004402580000020808ED00E000ED00E020 +:100690002DE9F04F99B0B34C2022FF21019010A812 +:1006A000A66800F031FCB04AA3461378A3B9012133 +:1006B000AE481170C360202383F31188C3680BB167 +:1006C00001F094FE0023AA4A4FF47A71A74801F082 +:1006D00051FE002383F31188019B13B1A54B019AAE +:1006E0001A600023A44AA349019F99461C461D464F +:1006F000984613704B600292012000F0C9FB002F56 +:1007000000F012829B4B1B68002B40F00D8219B049 +:10071000BDE8F08F0220FFF7B3FE002840F0FB8118 +:10072000019BB9F1000F08BF1F46944B1B880221A3 +:100730000BA8ADF82C3000F09FFADDE74FF47A708B +:1007400000F02EFA031E0393EADB0220FFF798FE67 +:1007500082460028E4D0039B581E042800F2DD8165 +:10076000DFE800F0030E1114170018A80523042178 +:1007700040F8343D00F080FA54464FF0000856E04F +:1007800004217848F6E704217D48F3E704217D48F9 +:10079000F0E71C242046043400F0B0FA04210B904A +:1007A0000BA800F069FA2C2CF4D1E5E7002DB7D0A6 +:1007B000002CB5D00220FFF763FE0546002800F0AC +:1007C000AF8101206C4C00F097FA4FF00009022035 +:1007D000207000F05DFB5FFA89FA504600F09CFA49 +:1007E000074658B1504609F1010900F0A5FA002862 +:1007F000F1D12C46A9460027634B97E701233E46DB +:100800000220237000F03AFBDBF808309E4206D24B +:10081000304600F073FA0130EBD10436F4E70026DD +:10082000029BA9462C461E703746524B5E6000F074 +:10083000BFFB15B1002C18BF0027FFF729FE5BE7AF +:10084000002D3FF46DAF002C3FF46AAF0220029BF5 +:10085000187000F01DFB322000F0A2F9B0F1000A80 +:10086000C0F25E811AF0030540F05A8106EB0A03DC +:10087000DBF80820934200F25381BAF5807F00F242 +:100880004F8155450DDA4FF47A7000F089F90490E4 +:10089000049B002BC0F24481049B3C4AAB540135BD +:1008A000EFE7C820FFF7ECFD0546002800F038818F +:1008B0001F2E11D8C6F1200410AB26F003003349D7 +:1008C0005445184428BF5446224600F0F5FA224603 +:1008D000FF212E4800F018FB4FEAAA0A2B493046A8 +:1008E0005FFA8AF200F018FB0446002800F01A8133 +:1008F00006EB8A0605469AE70220FFF7C1FD0028AD +:100900003FF40EAF00F032FA00283FF409AF4FF089 +:10091000000A5346DBF8082092451CD2BAF11F0F9B +:1009200012D8109A01320FD02AF0030218A90A44F3 +:1009300052F8202C0B92184604220BA90AF1040A43 +:1009400000F0DAFB0346E5E75046039300F0D6F9E2 +:10095000039B0B90EFE718A8042140F84C3D00F0F2 +:100960008BF964E7482200206C23002054220020E9 +:10097000AD04000870230020682200208240000897 +:100980004C22002050220020844000086C220020CD +:1009900018A80023642140F8343D00F039F90028FC +:1009A0007FF4BEAE0220FFF76BFD00283FF4B8AE27 +:1009B0000B9800F0D3F918AB43F8480D0421184602 +:1009C000CDE718A80023642140F8343D00F020F959 +:1009D00000287FF4A5AE0220FFF752FD00283FF467 +:1009E0009FAE0B9800F0BCF918AB43F8440DE5E757 +:1009F0000220FFF745FD00283FF492AE00F0C6F953 +:100A000018AB43F8400DD9E70220FFF739FD002865 +:100A10003FF486AE0BA9142000F0BEF9824618A858 +:100A2000042140F83CAD00F027F951460BA896E7A9 +:100A3000322000F0B5F8B0F1000AFFF671AE1AF0FE +:100A4000030F7FF46DAE0AEB0803DBF80820934236 +:100A50003FF666AE0220FFF713FD00283FF460AEBC +:100A60002AF0030AC244D0453FF4E1AE404608F103 +:100A7000040800F043F904210A900AA800F0FCF8E9 +:100A8000F1E74FF47A70FFF7FBFC00283FF448AE23 +:100A900000F06CF900283FF4AFAE109B01330CD08E +:100AA000082210A9002000F037FA00283FF4A4AE75 +:100AB0002022FF2110A800F027FAFFF7E9FC3748B1 +:100AC00001F014FC23E6002D3FF42AAE002C3FF485 +:100AD00027AE18A80023642140F8343D00F098F8B0 +:100AE000824600287FF41CAE0220FFF7C9FC0028D4 +:100AF0003FF416AE0390FFF7CBFC41F28830574627 +:100B000001F0F4FB0B9800F097FA00F051FA039B08 +:100B10001C461D46F0E5054689E64FF00008FFE556 +:100B20002546FDE52C4667E6002000F039F80490E4 +:100B3000049B002BFFF6E3AD012000F09FF9049B1E +:100B4000213B122B3FF6D8AD01A252F823F000BF93 +:100B5000150700083D070008AD070008F906000862 +:100B6000F9060008F906000841080008310A0008E3 +:100B7000F908000891090008C3090008F1090008F4 +:100B8000F9060008090A0008F9060008830A0008A7 +:100B90002F080008F9060008C70A0008A08601000F +:100BA0002DE9F3474FF47A75002402AE154F454303 +:100BB000DFF8588006F8014D97F900305FFA84F9A4 +:100BC0005A1C01D0A34212D158F824000122314608 +:100BD0000368D3F820A02B46D047012807D10A4B41 +:100BE0009DF8070083F8009002B0BDE8F08701345B +:100BF000022CE1D14FF4FA7001F078FB4FF0FF3096 +:100C0000F2E700BF00220020BC230020984000082B +:100C10002DE9F0474FF47A7506460024134F4D43F3 +:100C2000DFF8508097F900305FFA84F95A1C01D040 +:100C3000A34210D158F82400042231460368D3F8A7 +:100C400020A02B46D047042805D1094B002083F86B +:100C50000090BDE8F0870134022CE3D14FF4FA7024 +:100C600001F044FB4FF0FF30BDE8F0870022002088 +:100C7000BC23002098400008074B30B41A78074B7B +:100C800053F822400A46014623682046DD69044B9A +:100C9000AC4630BC604700BFBC2300209840000831 +:100CA000A0860100F8B5104C0025104E01F066FD3D +:100CB0000F4F20703068237883420CD800250D4EEA +:100CC0002078013801F056FD23780544013BB542F8 +:100CD0002370F5D9F8BD01F04DFD336805440133AB +:100CE000BD423360E6D9E9E7BD2300207823002028 +:100CF000FFFF0100FFFF030001F00CBE00F10060E8 +:100D000000F500300068704700F10060920000F5C7 +:100D1000003001F08FBD0000054B1A68054B1B78B1 +:100D20009B1A834202D9104401F024BD0020704771 +:100D300078230020BD23002038B5074D04462868DD +:100D4000204401F01DFD28B928682044BDE8384042 +:100D500001F028BD38BD00BF782300200020704777 +:100D6000014BC058704700BF00E8F11F064991F8D9 +:100D7000243033B100230822086A81F82430FFF7B9 +:100D8000C3BF0120704700BF7C230020014B1868BF +:100D9000704700BF0010005C234BF0B5234C1B686C +:100DA0002788C3F30B0665681B0C94F90820BE4224 +:100DB00028D0A789BE4206D101220C2505FB02449A +:100DC000656894F9082041F20104A3421CD041F265 +:100DD0000304A3421AD042F20104A34218D042F203 +:100DE0000304A34208BF5622441E013D0B460C4497 +:100DF000A34217D215F9016F581C5EB1034600F8E3 +:100E0000016CF5E70022D8E75A22EDE75922EBE71B +:100E10005822E9E72C2584421D7001D9981C5A708C +:100E2000401AF0BD1846FBE70010005C04220020C9 +:100E3000104B41F201015B888B429AB211D041F212 +:100E400003018B420FD042F201039A420DD042F2CD +:100E500003039A420BD10323074A02EB8303D8789A +:100E600070470023F8E70123F6E70223F4E70020A8 +:100E7000704700BF0010005C88400008022803D1C2 +:100E80004FF00052014B9A61704700BF00080258B2 +:100E9000022803D14FF40052014B9A61704700BF02 +:100EA00000080258022804D1024A536983F400530F +:100EB0005361704700080258002310B5934203D0D5 +:100EC000CC5CC4540133F9E710BD000030B5441EBA +:100ED00014F9010F0B4658B193F9005001318542C6 +:100EE00006D11AB993F90020801A30BD013AEFE714 +:100EF000002AF7D1104630BD02460346981A13F96E +:100F0000011B0029FAD1704702440346934202D0E4 +:100F100003F8011BFAE770472DE9F047234C05461B +:100F20008846174694F8243083BB2E46DFF87C9021 +:100F3000C7B394F824302BB92022FF2148462662FB +:100F4000FFF7E2FF94F824004146C0F1080504EBE6 +:100F50008000BD4228BF3D465FFA85FAAD00A7EB91 +:100F60000A072A462E44FFF7A7FF94F82430A84426 +:100F7000FFB29A445FFA8AFABAF1080F84F824A003 +:100F8000D6D1FFF7F3FE0028D2D108E0266A06EB9F +:100F90008306B042CAD0FFF7E9FE0028C5D1002081 +:100FA000BDE8F0870120BDE8F08700BF7C2300206A +:100FB000024B1A78024B1A70704700BFBC23002006 +:100FC0000022002038B5164C164D204600F02CFCAF +:100FD0002946204600F054FC2D681348D5F890208F +:100FE000D2F8043843F00203C2F8043801F07EF965 +:100FF0000E49284600F054FDD5F890200C48D2F850 +:1010000004380C49A04223F00203C2F804384FF41C +:10101000E1330B6003D0BDE8384000F063BB38BD5E +:10102000E82A0020A041000840420F00C841000803 +:1010300040240020A423002038B50B4B05461A7825 +:101040000A4B53F822400A4B9C420CD0094B00211A +:1010500018221846FFF758FF056001462046BDE8F4 +:10106000384000F03FBB38BDBC230020984000084A +:10107000E82A0020A4230020FEE7000000B59BB072 +:10108000EFF3098168226846FFF716FFEFF3058347 +:10109000034B9A6B9A6A9A6A9A6A9A6A9B6AFEE703 +:1010A00000ED00E000B59BB0EFF3098168226846CF +:1010B000FFF702FFEFF30583044B9A6B9A6A9A6A73 +:1010C0009A6A9A6A9A6A9B6AFEE700BF00ED00E09E +:1010D00000B59BB0EFF3098168226846FFF7ECFE8C +:1010E000EFF30583034B5A6B9A6A9A6A9A6A9A6A73 +:1010F0009B6AFEE700ED00E030B50A44074D9142DF +:101100000BD011F8013B09245840013CF7D040F3C3 +:1011100000032B4083EA5000F7E730BD2083B8ED91 +:10112000002304491A465A50C81808334260802BDD +:10113000F9D17047C0230020024A136843F0C0036E +:101140001360704700100140044B5A6810439A68BE +:1011500058600AB1181D1047704700BF4024002096 +:101160002DE9F84F414EF56D2F68EC6921072C628F +:1011700019D014F0080F0CBF00208020E20748BFF0 +:1011800040F02000A3074FF0200348BF40F040008C +:10119000610748BF40F4807083F31188FFF7D4FFE4 +:1011A000002383F31188E20509D5202383F31188F6 +:1011B0004FF40070FFF7C8FF002383F311884FF04E +:1011C0002009DFF8A8A04FF0000B14F0200832D15E +:1011D0003B0615D54FF02009DFF894A020060FD567 +:1011E00089F31188504600F0F1F9002830DA082020 +:1011F000FFF7AAFF27F080032B60002383F31188F9 +:1012000079060DD562060BD5202383F31188F26C85 +:10121000336D9A4201D1336C03BB002383F31188F1 +:10122000E30604D5B26E13690BB150699847BDE867 +:10123000F84F01F009BC89F31188AB8C504696F841 +:101240006410194000F05CFA8BF31188EC69BCE77C +:1012500080B2288588F31188EC69BFE727F0400742 +:101260001020FFF771FF2F60D7E700BF4024002058 +:101270007824002013B5104C204600F027FA04F122 +:101280001400009400234FF400720C4900F0E8F8B9 +:1012900004F1380000944FF40072094B094900F042 +:1012A00061F9094B0C212520E365084B236602B048 +:1012B000BDE8104000F05EB840240020AC240020BF +:1012C00039110008AC2600200010014000E1F505AE +:1012D000254B002908BF1946037C012B816630B5D8 +:1012E00011D1224B98420ED1214BD3F8F02042F07D +:1012F0001002C3F8F020D3F8182142F01002C3F80E +:101300001821D3F818310A68036EC46D03EB520339 +:10131000B3FBF2F34A68150442BF23F0070503F05C +:10132000070343EA4503E3608B6843F040036360CF +:10133000CB68510543F00103A36042F4967343F078 +:10134000010323604FF0FF33236205D512F0102212 +:1013500005D0B2F1805F04D080F8643030BD7F23C7 +:10136000FAE73F23F8E700BFBC4000084024002014 +:101370000044025800F1604300F01F0209014009D7 +:1013800003F56143C9B2800083F80013012300F123 +:101390006040934000F56140C0F8803103607047C1 +:1013A000F8B51546826804460B46AA4200D2856805 +:1013B000A1692669761AB54207D218462A46FFF770 +:1013C0007BFDA3692B44A3610DE011D9AF1B32460D +:1013D0001846FFF771FD3A46E1683044FFF76CFDAF +:1013E000E2683A44A261A36828465B1BA360F8BD8B +:1013F00018462A46FFF760FDE368E4E783689342F6 +:101400002DE9F04104460F46154600D28568606913 +:101410002669361AB54207D22A463946FFF74CFDEF +:1014200063692B4463610EE013D9A5EB06083246CD +:101430003946FFF741FD4246B919E068FFF73CFD28 +:10144000E26842446261A36828465B1BA360BDE872 +:10145000F0812A463946FFF72FFDE368E2E70000F6 +:1014600010B50A440024C361029B006040608460A0 +:10147000C160816141610261036210BD08B582698A +:101480004369934201D1826882B9826801328260E5 +:101490005A1C42611970426903699A4201D3C368B8 +:1014A0004361002100F03EFF002008BD4FF0FF30F7 +:1014B00008BD000070B5202304460E4683F3118852 +:1014C000A568A5B1A368A269013BA360531CA361F1 +:1014D00015782269934224BFE368A361E3690BB1E5 +:1014E00020469847002383F31188284670BD314673 +:1014F000204600F007FF0028E2DA85F3118870BD6E +:101500002DE9F74F05460F4690469A46D0F81C90B5 +:10151000202686F311884FF0000B144664B1224652 +:1015200039462846FFF73CFF034668B9514628462E +:1015300000F0E8FE0028F1D0002383F31188A8EB27 +:10154000040003B0BDE8F08FB9F1000F03D00190A3 +:101550002846C847019B8BF31188E41A1F4486F381 +:101560001188DBE7C160816141611144C361009B67 +:10157000006040608260016103627047F8B5044614 +:101580000E461746202383F31188A568A5B1A368EA +:10159000013BA36063695A1C62611E702369626922 +:1015A0009A4224BFE3686361E3690BB12046984720 +:1015B000002080F31188F8BD3946204600F0A2FED5 +:1015C0000028E2DA85F31188F8BD000083694269DA +:1015D0009A4210B501D182687AB98268013282607C +:1015E0005A1C82611C7803699A4201D3C3688361E3 +:1015F000002100F097FE204610BD4FF0FF3010BDD7 +:101600002DE9F74F05460F4690469A46D0F81C90B4 +:10161000202686F311884FF0000B144664B1224651 +:1016200039462846FFF7EAFE034668B95146284680 +:1016300000F068FE0028F1D0002383F31188A8EBA6 +:10164000040003B0BDE8F08FB9F1000F03D00190A2 +:101650002846C847019B8BF31188E41A1F4486F380 +:101660001188DBE7026843681143016003B1184742 +:10167000704700001430FFF743BF00004FF0FF3306 +:101680001430FFF73DBF00003830FFF7B9BF00004E +:101690004FF0FF333830FFF7B3BF00001430FFF7CF +:1016A00009BF00004FF0FF311430FFF703BF000007 +:1016B0003830FFF763BF00004FF0FF323830FFF7DC +:1016C0005DBF000000207047FFF7D4BD044B0360EE +:1016D000002343608360C36001230374704700BF2D +:1016E000D440000810B52023044683F31188FFF787 +:1016F000EFFD02232374002383F3118810BD000043 +:1017000038B5C36904460D461BB904210844FFF7E8 +:10171000A9FF294604F11400FFF7B0FE002806DAFD +:10172000201D4FF48061BDE83840FFF79BBF38BDF6 +:10173000026843681143016003B118477047000015 +:1017400013B5446BD4F894341A681178042915D170 +:10175000217C022912D11979012312898B4013426D +:101760000CD101A904F14C0002F02CF8D4F89444F7 +:101770000246019B2179206800F0EAF902B010BD11 +:10178000143001F0AFBF00004FF0FF33143001F010 +:10179000A9BF00004C3002F081B800004FF0FF33C9 +:1017A0004C3002F07BB80000143001F07DBF000027 +:1017B0004FF0FF31143001F077BF00004C3002F0E1 +:1017C0004DB800004FF0FF324C3002F047B8000037 +:1017D000D0F8942438B5136805461978042901D047 +:1017E000012038BD017C0229FAD1127901205C89DF +:1017F00090400440F4D105F1140001F00FFF0246BF +:101800000028EDD0D5F894544FF480732868697996 +:1018100000F08CF9204638BD406BFFF7D9BF0000BF +:1018200000207047704700007FB5124B012502264B +:10183000044603600023057400F1840243602946D6 +:101840008360C3600C4B0290143001934FF480739B +:10185000009601F0BFFE094B04F52372294601935F +:1018600004F14C004FF480730294009601F086FF5F +:1018700004B070BDFC4000081918000841170008AA +:101880000A68202383F311880B790B3342F8230075 +:101890004B79133342F823008B7913B10B3342F8A1 +:1018A00023000223C0F894140374002383F31188E7 +:1018B0007047000038B5037F044613B190F85430E8 +:1018C000ABB90125201D0221FFF732FF04F11400FE +:1018D00025776FF0010100F031FD84F8545004F1D8 +:1018E0004C006FF00101BDE8384000F027BD38BD65 +:1018F00010B5012104460430FFF71AFF00232377B7 +:1019000084F8543010BD000038B50446002514306A +:1019100001F078FE04F14C00257701F047FF201D0F +:1019200084F854500121FFF703FF2046BDE83840FA +:10193000FFF74EBF90F8803003F06003202B19D1E1 +:1019400090F88120212A0AD0222A4FF000030ED0DD +:10195000202A0FD1084A42670722826704E0064B1B +:101960004367072383670023C367012070474367EA +:101970008367F9E7002070471C220020D0F89434D8 +:1019800037B51A680446117804291AD1017C022956 +:1019900017D11979012312898B40134211D100F11B +:1019A0004C05284601F0C2FF58B101A9284601F0B4 +:1019B00009FFD4F894440246019B2179206800F085 +:1019C000C7F803B030BD000001F10B03F7B550F8C4 +:1019D000234005460E46F4B1202383F3118805EB1E +:1019E0008607201D08214C34FFF7A2FEFB685B69C7 +:1019F0001B6813B1204601F0F3FE01A9204601F057 +:101A0000E1FE024648B1019B3146284600F0A0F8AD +:101A1000002383F3118803B0F0BDFB685A69126894 +:101A2000002AF5D01B8A013B1340F1D105F1800259 +:101A3000EAE70000133138B550F82140DCB120232B +:101A400083F31188D4F894241368527903EB82034A +:101A5000DB689B695D6845B104216018FFF768FE8B +:101A6000294604F1140001F0E3FD2046FFF7B0FE23 +:101A7000002383F3118838BD7047000001F0DEB801 +:101A8000012300F1300200F1500103700023436094 +:101A900042F8043B8A42D361FAD103814381704703 +:101AA00038B50446202383F31188002500F10C0388 +:101AB00000F13002416043F8045B9342FBD12046C1 +:101AC00001F0DEF80223237085F3118838BD000091 +:101AD00070B500EB8103054650690E461446DA6086 +:101AE00018B110220021FFF70FFAA06918B11022D7 +:101AF0000021FFF709FA31462846BDE8704001F0A1 +:101B0000D1B90000038900F13002002103F0010384 +:101B10000381438903F00103438100F1100343F87B +:101B2000041B9342FBD101F055BA0000F0B401252B +:101B300000EB810447898D40E4683D43A4694581F9 +:101B400023600023A2606360F0BC01F071BA000062 +:101B5000F0B4012500EB810407898D40E4683D4322 +:101B60006469058123600023A2606360F0BC01F01A +:101B7000E9BA0000022300F10C0200F1300110B5B7 +:101B8000037004460023A0F8883080F88A3080F87B +:101B90008B300381438142F8043B8A42FBD184F8B5 +:101BA0007030204601F012F963681B6823B12046AB +:101BB0000021BDE81040184710BD000002784368BE +:101BC00080F88C2005221B6802700BB10421184795 +:101BD00070470000436890F88C201B6802700BB1BE +:101BE000052118477047000090F8703070B5044622 +:101BF00013B1002380F8703004F18002204601F018 +:101C00000DFA63689B6863BB94F8805015F060061A +:101C100015D194F8813005F07F0545EA032540F29F +:101C200002339D4200F00E815BD8022D00F0DC8073 +:101C30003FD8002D00F08780012D00F0CF800021DB +:101C4000204601F0A3FC0021204601F093FC6368CC +:101C50001B6813B1062120469847062384F870308C +:101C600070BD204698470028CED094F8872094F87D +:101C7000863043EA0223A26F934238BFA36794F9E8 +:101C80008030A56F002B4FF0200380F2FD80002DE7 +:101C900000F0EC80092284F8702083F31188002181 +:101CA000A36F626F2046FFF753FF002383F3118871 +:101CB00070BDB5F5817F00F0B180B5F5407F49D0AA +:101CC000B5F5807FBBD194F88230012BB7D1B4F841 +:101CD000883023F00203A4F888306667A667E667B9 +:101CE000C3E740F201639D421ED8B5F5C06F3BD2F9 +:101CF000B5F5A06FA3D1B4F88030B3F5A06F0ED1C5 +:101D000094F88230204684F88A3001F0BDF8636888 +:101D10001B6813B101212046984703232370002339 +:101D20006367A367E367A0E7B5F5106F32D040F6AD +:101D300002439D4252D0B5F5006F80D104F18B0370 +:101D40006367012324E004F18803E56763670223E6 +:101D5000A3678AE794F88230012B7FF470AFB4F860 +:101D6000883043F00203B6E794F885202046616886 +:101D700094F884304D6843EA022394F8831094F871 +:101D80008220A84700283FF45AAF4368636703687E +:101D9000A367A4E72378042B10D1202383F31188B1 +:101DA0002046FFF7AFFE86F311886368032184F8AD +:101DB0008B601B6821700BB12046984794F88230E5 +:101DC000002BACD084F88B300423237063681B682D +:101DD000002BA4D0022120469847A0E7374B636729 +:101DE0000223A36700239DE794F88410204611F096 +:101DF000800F01F00F010ED001F002F9012806D08A +:101E000002287FF41CAF2E4BA067636767E72D4B5A +:101E1000A567636763E701F0E5F8EFE794F88230C0 +:101E2000002B7FF40CAF94F8843013F00F013FF4D3 +:101E300076AF1A06204602D501F0C0FB6FE701F02D +:101E4000B1FB6CE794F88230002B7FF4F8AE94F885 +:101E5000843013F00F013FF462AF1B06204602D519 +:101E600001F094FB5BE701F085FB58E7142284F84E +:101E7000702083F311882B462A4629462046FFF717 +:101E800055FE85F3118870BD5DB1152284F8702070 +:101E900083F311880021A36F626F2046FFF746FE8F +:101EA00003E70B2284F8702083F311882B462A461F +:101EB00029462046FFF74CFEE3E700BF2C4100080F +:101EC000244100082841000838B590F870300446D5 +:101ED000152B29D8DFE803F03E28282828283E289B +:101EE000280B293928282828282828283E3E90F819 +:101EF000871090F88620836F42EA01229A4214D913 +:101F0000C268128AB3FBF2F502FB15356DB92023C6 +:101F100083F311882B462A462946FFF719FE85F3DD +:101F200011880A2384F8703038BD142384F8703087 +:101F3000202383F31188002320461A461946FFF711 +:101F4000F5FD002383F3118838BDC36F03B19847B3 +:101F50000023E7E7002101F019FB0021204601F0F2 +:101F600009FB63681B6813B10621204698470623C6 +:101F7000D8E7000090F87020152A38B5044622D81A +:101F80000123934040F6416213421DD113F48015A2 +:101F90000FD19B0217D50B2380F87030202383F3D9 +:101FA00011882B462A462946FFF7D2FD85F3118872 +:101FB00038BDC3689B695B682BB9C36F03B1984791 +:101FC000002384F8703038BD002101F0DFFA0021D1 +:101FD000204601F0CFFA63681B6813B10621204642 +:101FE00098470623EDE70000024B00221B605B6070 +:101FF0009A607047AC280020002382680374054B68 +:102000001B6899689142FBD25A6803604260106075 +:1020100058607047AC28002008B5202383F311884E +:10202000037C032B05D0042B0DD02BB983F311882F +:1020300008BD436900221A604FF0FF334361FFF788 +:10204000DBFF0023F2E790E80C001A60026853609F +:10205000F2E70000002382680374054B1B6899684F +:102060009142FBD85A680360426010605860704724 +:10207000AC280020054B1969087418680268536081 +:102080001A60186101230374FEF79EB9AC28002082 +:102090004B1C30B5054687B00A4C10D0236901A807 +:1020A000094A00F067F92846FFF7E4FF049B13B1E3 +:1020B00001A800F09BF92369586907B030BDFFF70C +:1020C000D9FFF8E7AC2800201920000838B50C4DDE +:1020D000044641612B6981689A68914203D8BDE842 +:1020E0003840FFF789BF1846FFF786FF01232C61B0 +:1020F000014623742046BDE83840FEF765B900BFAD +:10210000AC280020044B1A681B6990689B689842B1 +:1021100094BF002001207047AC28002010B5084C67 +:10212000236820691A6854602260012223611A74AE +:10213000FFF790FF01462069BDE81040FEF744B963 +:10214000AC280020FEE7000010B5194CFFF74CFF4B +:1021500000F002F980221749204600F087F8012399 +:1021600044F8180C0374144B144AD96821F4E06144 +:102170000904090C0A431249DA60CA6842F0807205 +:10218000CA60104A1049C2F8B01F116841F001013D +:1021900011601022DA77202283F82220002383F3B3 +:1021A000118862B60948BDE8104000F081B800BF50 +:1021B000D42800203041000800ED00E00003FA05BB +:1021C000F0ED00E0001000E055CEACC5384100084D +:1021D0002DE9F84F1E4C4FF00008DFF87890656944 +:1021E00004F11407D9F82430266AAA689E1B964287 +:1021F0001CD34FF0200AAA68236AD5F80CB0B61A8F +:10220000134423622B68BB425F606361C5F80C8096 +:1022100001D101F0B7FB88F311882869D8478AF308 +:1022200011886569AB689E42E5D2DBE76269BA4214 +:102230000CD0916823628E1B9660A86802282CBF80 +:102240001818981CBDE8F84F01F0A2BBBDE8F88F44 +:10225000AC280020000C0040034B59685A68521A01 +:102260009042FBD8704700BF001000E0826002225D +:1022700002740022427470478368A3F17C0243F821 +:102280000C2C026943F83C2C426943F8382C074A6D +:1022900043F81C2CC268A3F1180043F8102C02224A +:1022A00003F8082C002203F8072C7047E10300080C +:1022B00010B5202383F31188FFF7DEFF00210446C9 +:1022C000FFF704FF002383F31188204610BD0000B0 +:1022D000024B1B6958610F20FFF7CCBEAC280020D1 +:1022E000202383F31188FFF7F3BF000008B50146F0 +:1022F000202383F311880820FFF7CAFE002383F30D +:10230000118808BD49B1064B42681B6918605A60C4 +:10231000136043600420FFF7BBBE4FF0FF307047EF +:10232000AC2800200368984206D01A68026050600A +:1023300018465961FFF760BE7047000038B5044683 +:102340000D462068844200D138BD036823605C607C +:102350004561FFF751FEF4E7054B03F114025A61A2 +:102360009A614FF0FF32DA6100221A62704700BFB3 +:10237000AC280020F8B5036102291A4B0646C2605A +:1023800038BF02215C6A184B1A461F4652F8145F88 +:1023900095420AD1586198611C6205604560816070 +:1023A0000819BDE8F84001F0E3BA186AAB68241ACE +:1023B0000C1902D3E41A2D6804E09C4202D2204496 +:1023C00001F0E6FAAB689C42F4D86B683560736044 +:1023D0001E604FF0FF336E60B460A968091BA960EE +:1023E000FB61F8BD000C0040AC28002010B41A4C72 +:1023F000234653F8141F814210D0416802680A60D6 +:10240000026851609A424FF00001C16003D09368A6 +:1024100081680B4493605DF8044B70470A680020A4 +:102420009A4262615360C86003D15DF8044B01F0C9 +:10243000A9BA936888680344206A9360074A526A7D +:10244000121A9342E7D9991A5DF8044B012998BFF3 +:10245000931C184401F09CBAAC280020000C0040EA +:1024600000207047FEE70000704700004FF0FF308B +:1024700070470000022906D0032906D0012906482A +:1024800018BF0020704705487047032A9ABF0448C8 +:1024900000EBC2000020704724420008D841000829 +:1024A0002422002070B59AB006460846144601ADB5 +:1024B000294600F095F82846FEF71EFDC0B2431CE1 +:1024C0005B0086E8180023700323023404F8013C03 +:1024D00000231946DAB20234904201D81AB070BD16 +:1024E000EA5C013304F8011C04F8022CF2E7000056 +:1024F00008B5202383F311880348FFF73FFA002330 +:1025000083F3118808BD00BFE82A002010B50446F7 +:10251000052916D8DFE801F016150316161D20232D +:1025200083F311880E4A0121FFF7D2FA20460D4AA3 +:102530000221FFF7CDFA0C48FFF7E6F9002383F3F9 +:10254000118810BD202383F311880748FFF7B2F9E3 +:10255000F4E7202383F311880348FFF7C9F9EDE777 +:10256000584100087C410008E82A002038B50C4D8D +:102570000C4C2A460C4904F10800FFF793FF05F1C3 +:10258000CA0204F110000949FFF78CFF05F5CA7271 +:1025900004F118000649BDE83840FFF783BF00BFCB +:1025A000B02F002024220020A8410008B2410008DA +:1025B000BD41000870B5044608460D46FEF79CFC78 +:1025C000C6B22046013403780BB9184670BD3246B6 +:1025D0002946FEF77BFC0028F3D1012070BD0000E6 +:1025E0002DE9F84F05460C46FEF786FC2B49C6B28E +:1025F0002846FFF7DFFF2A492846FFF7DBFF08B12F +:102600001036F6B2632E0DD8DFF89080DFF8909088 +:10261000244FDFF898A0DFF898B02E7846B92670DE +:10262000BDE8F88F29462046BDE8F84F01F020BDEF +:10263000252E2ED1072241462846FEF747FC70B9C9 +:10264000DBF800300735073444F8073CBBF80430AA +:1026500024F8033C9BF8063004F8013CDDE708222F +:1026600049462846FEF732FC98B9A21C0E4B13F8D7 +:10267000011F023209095345C95D02F8041C19788B +:1026800001F00F01C95D02F8031CF0D118340835C0 +:10269000C3E7267001350134BFE700BF444200089C +:1026A000BD41000854420008FFE7F11F0BE8F11F8D +:1026B0004C420008BFF34F8F044B1A695107FCD1FD +:1026C000D3F810215207F8D1704700BF0020005204 +:1026D00008B50D4B1B78ABB9FFF7ECFF0B4BDA6875 +:1026E000D10704D50A4A5A6002F188325A60D3F8F9 +:1026F0000C21D20706D5064AC3F8042102F188321C +:10270000C3F8042108BD00BF0E3200200020005293 +:102710002301674508B5114B1B78F3B9104B1A69B3 +:10272000510703D5DA6842F04002DA60D3F810218D +:10273000520705D5D3F80C2142F04002C3F80C2112 +:10274000FFF7B8FF064BDA6842F00102DA60D3F80F +:102750000C2142F00102C3F80C2108BD0E3200200A +:10276000002000520F289ABF00F58060400400202E +:10277000704700004FF40030704700001020704791 +:102780000F2808B50BD8FFF7EDFF00F500330268FE +:10279000013204D104308342F9D1012008BD002068 +:1027A00008BD00000F2810B504463FD8FFF782FF90 +:1027B000FFF78EFF1E484FF0FF33072C4361C0F830 +:1027C00014311DD80361FFF775FF230243F0240382 +:1027D000C360C36843F08003C36003695A07FCD435 +:1027E000FFF768FF2046FFF7BDFF4FF4003100F010 +:1027F000F7F8FFF78FFF2046BDE81040FFF7C0BF96 +:10280000C0F81031FFF756FFA4F108031B0243F094 +:102810002403C0F80C31D0F80C3143F08003C0F829 +:102820000C31D0F810315B07FBD4D9E7002010BD84 +:10283000002000522DE9F84F40EA020305460E46FB +:102840001746DB0656D13446DFF8C090DFF8C0A04B +:10285000FFF73EFF3B1B33441F2B04D8FFF75AFF03 +:102860000120BDE8F88F20222146284601F0F0FB28 +:10287000002837D005F17843214A22489342224B61 +:1028800098BF984603F5827392BFD346C8469B46CD +:10289000A3F1080388BF1846FFF70CFF4FF0FF3382 +:1028A00004F11C01A5EB040ECBF80030036843F0E3 +:1028B00002030360231FD8F8002012F0050FFAD19D +:1028C00053F8042F99424EF80320F4D1BFF34F8FF1 +:1028D000FFF7F0FE4FF0FF33CBF80030036823F032 +:1028E0000203036020222146284601F0B1FB20B1FB +:1028F000FFF710FF0020BDE8F88F20352034A9E74E +:10290000FFFF0F000C200052102000521021005237 +:102910001420005210B5084C237828B153B9FFF7A2 +:10292000D7FE0123237010BD23B12070BDE81040F5 +:10293000FFF7F0BE10BD00BF0E3200200244043984 +:10294000064BD2B210B5904200D110BD441C00B26B +:1029500053F8200041F8040FE0B2F4E7504000586B +:102960000F4B30B51C6F240405D41C6F1C671C6F03 +:1029700044F400441C670B4C024404392368D2B26F +:1029800043F480732360084B904200D130BD441C57 +:1029900051F8045F00B243F82050E0B2F4E700BF02 +:1029A00000440258004802585040005807B5012220 +:1029B00001A90020FFF7C2FF019803B05DF804FBF6 +:1029C00013B50446FFF7F2FFA04206D002A9012288 +:1029D000002041F8044DFFF7C3FF02B010BD000016 +:1029E0000144BFF34F8F064B884204D3BFF34F8F90 +:1029F000BFF36F8F7047C3F85C022030F4E700BF6D +:102A000000ED00E0034B1B685B0142BF0122024B5B +:102A10001A707047D04402580F320020014B1878CA +:102A2000704700BF0F320020064A536823F00103AD +:102A30005360EFF30983683383F30988002383F33A +:102A40001188704730EF00E010B5202383F3118820 +:102A5000104B5B6813F4006318D0F1EE103AEFF3FB +:102A600009844FF0807344F84C3C0B4BDB6844F80E +:102A7000083CA4F1680383F30988FFF743FB18B10E +:102A8000064B44F8503C10BD054BFAE783F3118820 +:102A900010BD00BF00ED00E030EF00E0F1030008E2 +:102AA000F4030008F0B5BFF34F8FBFF36F8F1D4BDA +:102AB0000021C3F85012BFF34F8FBFF36F8F5A69D5 +:102AC00042F400325A61BFF34F8FBFF36F8FC3F8E8 +:102AD0008410BFF34F8FD3F8802043F6E076C2F323 +:102AE000C904C2F34E32A507520102EA060E284677 +:102AF00021464EEA0007013900F14040C3F86072F8 +:102B00004F1CF6D1203A12F1200FEED1BFF34F8FB8 +:102B10005A6942F480325A61BFF34F8FBFF36F8F0F +:102B2000F0BD00BF00ED00E0FEE70000084A094BE1 +:102B300009498B4204D3094A0021934205D37047C7 +:102B400052F8040F43F8040BF3E743F8041BF4E7CF +:102B500018440008603300206033002060330020F8 +:102B6000D0F89430002230B51146D0F890409D68DE +:102B70004FF0FF3004EB421301329542C3F80019C5 +:102B8000C3F81019C3F80809C3F8001BC3F8101BD9 +:102B9000C3F8080BEED24FF00113C4F81C3830BD57 +:102BA00000EB81032DE9F04FD3F80CE04F1C4FEA06 +:102BB0004118DEF814403F03D4F800C06568D0F82F +:102BC000902065450AD30120D2F8343800FA01F18B +:102BD00023EA0101C2F83418BDE8F08FACEB05031D +:102BE000BEF81060B34228BF334602EB0806D6F8A1 +:102BF0001869B6B2B3EB860F13D8A6683A44994663 +:102C0000A6F1040A5AF804BFB9F1040FC2F800B0E3 +:102C100002D9A9F10409F5E71E442B44A6606360BC +:102C2000CCE70020BDE8F08F890141F0200101616F +:102C300003699B06FCD41220FFF70EBB10B50A4CAB +:102C40002046FEF71DFF094BC4F89030084BC4F82E +:102C50009430084C2046FEF713FF074BC4F8903021 +:102C6000064BC4F8943010BD10320020000008401C +:102C70008C420008AC32002000000440984200085A +:102C80000378012B70B505465DD1494BD0F89040D3 +:102C9000984259D1474B0E216520D3F8D82042F0F5 +:102CA0000062C3F8D820D3F8002142F00062C3F8D4 +:102CB0000021D3F80021D3F8802042F00062C3F84D +:102CC0008020D3F8802022F00062C3F88020D3F85F +:102CD0008030FEF74FFB384BE360384BC4F80038C8 +:102CE0000023D5F89060C4F8003EC02323604FF461 +:102CF0000413A3633369002BFCDA01230C20336136 +:102D0000FFF7AAFA3369DB07FCD41220FFF7A4FA15 +:102D10003369002BFCDA00262846A660FFF720FF67 +:102D20006B68C4F81068DB68C4F81468C4F81C68E1 +:102D3000002B3AD1224BA3614FF0FF336361A368AC +:102D400043F00103A36070BD1E4B9842C8D1194BDC +:102D50000E214D20D3F8D82042F00072C3F8D820BD +:102D6000D3F8002142F00072C3F80021D3F800210B +:102D7000D3F8802042F00072C3F88020D3F880207E +:102D800022F00072C3F88020D3F88020D3F8D82036 +:102D900022F08062C3F8D820D3F8002122F08062AC +:102DA000C3F80021D3F8003193E7074BC3E700BF16 +:102DB000103200200044025840140040030020025A +:102DC000003C30C0AC320020083C30C0F8B5D0F830 +:102DD0009040054600214FF000662046FFF724FF93 +:102DE000D5F8941000234FF001128F684FF0FF3098 +:102DF000C4F83438C4F81C2804EB431201339F4252 +:102E0000C2F80069C2F8006BC2F80809C2F8080BE2 +:102E1000F2D20B68D5F89020C5F898306362102381 +:102E20001361166916F01006FBD11220FFF714FA91 +:102E3000D4F8003823F4FE63C4F80038A36943F4DF +:102E4000402343F01003A3610923C4F81038C4F8E9 +:102E500014380B4BEB604FF0C043C4F8103B094BE8 +:102E6000C4F8003BC4F81069C4F80039D5F89830AC +:102E700003F1100243F48013C5F89820A362F8BD53 +:102E80006842000840800010D0F8902090F88A1026 +:102E9000D2F8003823F4FE6343EA0113C2F8003885 +:102EA000704700002DE9F0410EB20D4600EB860898 +:102EB000D8F80C100F6807F00303022B53D0032B34 +:102EC00053D03F4A3F4F012B18BF1746D0F89040D0 +:102ED0004FEA451E002205F1100C04EB0E03C3F867 +:102EE000102B8A69002A42D04A8A05F158033A43D6 +:102EF0005B01E2500123D4F81C2803FA0CF31343BE +:102F0000C4F81C38A64400234A69CEF8103905F1EC +:102F10003F03002A3BD00A8A04EB8303898B920883 +:102F2000012988BF4A43D0F89810561841EA024256 +:102F30002946C0F8986020465A60FFF775FED8F819 +:102F40000C301B8A43EA85531F4305F148035B019C +:102F5000E7500123D4F81C2803FA05F51543C4F8FB +:102F60001C58BDE8F081184FB0E7184FAEE704EBEE +:102F70004613D3F8002B22F40042C3F8002B0123A0 +:102F8000D4F81C2803FA0CF322EA0303B8E704EB95 +:102F900083030F4A04EB461629465A602046FFF782 +:102FA00043FED6F80039012223F4004302FA05F566 +:102FB000C6F80039D4F81C3823EA0505CFE700BF6E +:102FC00000800010008004100080081000800C10A9 +:102FD00000040002D0F894201268C0F89820FFF78F +:102FE000BFBD00005831D0F8903049015B5813F450 +:102FF000004004D013F4001F14BF012002207047CA +:103000004831D0F8903049015B5813F4004004D0A7 +:1030100013F4001F14BF01200220704700EB810150 +:10302000CB68196A0B6813604B68536070470000E7 +:1030300000EB810330B5DD68AA691368D36019B964 +:10304000402B84BF402313606B8A1468D0F8902013 +:103050001C4402EB4110013C09B2B4FBF3F463439E +:10306000033323F0030343EAC44343F0C043C0F8EF +:10307000103B2B6803F00303012B0ED1D2F8083864 +:1030800002EB411013F4807FD0F8003B14BF43F0F3 +:10309000805343F00053C0F8003B02EB4112D2F8DA +:1030A000003B43F00443C2F8003B30BD2DE9F04142 +:1030B000D0F8906005460C4606EB4113D3F8087B28 +:1030C0003A07C3F8087B08D5D6F814381B0704D58F +:1030D00000EB8103DB685B689847FA072FD5D6F8C9 +:1030E0001438DB072BD505EB8403D968CCB98B6981 +:1030F000488A5E68B6FBF0F200FB12628AB9186873 +:10310000DA6890420DD2121A83E81400202383F368 +:10311000118821462846FFF78BFF84F31188BDE80C +:10312000F081012303FA04F26B8923EA02036B8125 +:10313000CB6823B121462846BDE8F0411847BDE8D9 +:10314000F081000000EB81034A0170B5DD68D0F822 +:1031500090306C692668E66056BB1A444FF4002034 +:10316000C2F810092A6802F00302012A0AB20ED13D +:10317000D3F8080803EB421410F4807FD4F8000958 +:1031800014BF40F0805040F00050C4F8000903EB39 +:103190004212D2F8000940F00440C2F800090122AE +:1031A000D3F8340802FA01F10143C3F8341870BDB2 +:1031B00019B9402E84BF4020206020681A442E8A0E +:1031C000841940F00050013CB4FBF6F440EAC440DE +:1031D000C6E700002DE9F041D0F8906004460D46A6 +:1031E00006EB4113D3F80879C3F80879FB071CD51F +:1031F000D6F81038DA0718D500EB8103D3F80CE0C5 +:10320000DEF81430D3F800C0DA6894451BD2A2EB84 +:103210000C024FF000081A60C3F80480202383F3E7 +:103220001188FFF78FFF88F311883B0618D501231B +:10323000D6F83428AB40134212D029462046BDE8C8 +:10324000F041FFF7ADBC012303FA01F2038923EA41 +:1032500002030381DEF80830002BE6D09847E4E74C +:10326000BDE8F0812DE9F047D0F8905004466E6932 +:10327000AB691E40F1046E6103D5BDE8F047FEF76F +:1032800079BC002E12DAD5F8003E9A0705D0D5F8A1 +:10329000003E23F00303C5F8003ED5F8043820466D +:1032A00023F00103C5F80438FEF794FC330502D57A +:1032B0002046FEF783FCB7040CD5D5F8083813F088 +:1032C000060FEB6823F470530CBF43F4105343F420 +:1032D000A053EB60300704D56368DB680BB1204670 +:1032E0009847F10200F1A180B2020BD5D4F890808A +:1032F00000274FF00109D4F89430F9B29B688B4253 +:1033000080F0D280F3061AD5D4F890100A6AC2F37E +:103310000A1702F00F0302F4F012B2F5802F00F04A +:10332000EB80B2F5402F0AD104EB830301F5805105 +:10333000DB68186A00231A469F4240F0D1803003B0 +:10334000D5F8185835D5E90303D500212046FFF7F5 +:10335000ADFEAA0303D501212046FFF7A7FE6B03AC +:1033600003D502212046FFF7A1FE2F0303D5032139 +:103370002046FFF79BFEE80203D504212046FFF715 +:1033800095FEA90203D505212046FFF78FFE6A02AC +:1033900003D506212046FFF789FE2B0203D507211E +:1033A0002046FFF783FEEF0103D508212046FFF7F3 +:1033B0007DFE700340F1C780E90703D50021204658 +:1033C000FFF708FFAA0703D501212046FFF702FFF8 +:1033D0006B0703D502212046FFF7FCFE2F0703D51C +:1033E00003212046FFF7F6FEEE0603D50421204612 +:1033F000FFF7F0FEA80603D505212046FFF7EAFEF9 +:10340000690603D506212046FFF7E4FE2A0603D508 +:1034100007212046FFF7DEFEEB0540F194802046B1 +:103420000821BDE8F047FFF7D5BED4F890904FF0E3 +:1034300000084FF0010AD4F894305FFA88F79B68CF +:10344000BB42FFF451AF09EB4713D3F8002902F454 +:103450004022B2F5802F24D1D3F80029002A20DAA7 +:10346000D3F8002942F09042C3F80029D3F800298C +:10347000002AFBDB3946D4F89000FFF7D5FB228900 +:103480000AFA07F322EA0303238104EB8703DB68CC +:103490009B6813B139462046984739462046FFF7C6 +:1034A0007FFB08F10108C6E708EB4112D2F8003BA8 +:1034B00003F44023B3F5802F10D1D2F8003B002B4A +:1034C0000CDA628909FA01F322EA0303638104EB4F +:1034D0008103DB68DB680BB12046984701370AE7B8 +:1034E00013F0030F00D10A68072B03F101039EBFFD +:1034F0000270120A01301FE704EB830301F58051CB +:10350000DA6890690268D0F808C04068A2EB000E43 +:1035100000221046974208D1DB689B699A683A44BA +:103520009A605A6817445F6009E712F0030F00D1F0 +:103530000868964502F1010282BF8CF80000000A7B +:103540000CF1010CE6E7BDE8F087000008B5034880 +:10355000FFF788FEBDE80840FFF776BA103200207A +:1035600008B50348FFF77EFEBDE80840FFF76CBAD8 +:10357000AC320020D0F8903003EB4111D1F8003B81 +:1035800043F40013C1F8003B70470000D0F89030BE +:1035900003EB4111D1F8003943F40013C1F80039AD +:1035A00070470000D0F8903003EB4111D1F8003B98 +:1035B00023F40013C1F8003B70470000D0F89030AE +:1035C00003EB4111D1F8003923F40013C1F800399D +:1035D0007047000030B50433039C0172002104FBE6 +:1035E0000325C361049B00600363059B4060C160C9 +:1035F000426102618561046242628162C162436329 +:1036000030BD00000022416AC260416101616FF07B +:1036100001018262C262FEF791BE00000369426945 +:10362000934203D1C2680AB100207047181D704749 +:10363000036919600021C2680132C260C269134483 +:1036400082699342036124BF436A0361FEF76ABE45 +:1036500038B504460D46E3683BB162690020131D8E +:103660001268A3621344E36238BD237A33B9294652 +:103670002046FEF747FE0028EDDA38BD6FF0010066 +:10368000FBE70000C368C269013BC36043691344A0 +:1036900082699342436124BF436A4361002383628A +:1036A000036B03B11847704770B52023044683F3BA +:1036B0001188866A3EB9FFF7CBFF054618B186F33D +:1036C0001188284670BDA36AE26A13F8015B934231 +:1036D000A36202D32046FFF7D5FF002383F31188AE +:1036E000EFE700002DE9F84F04460E469046994654 +:1036F000202787F311880025AA46D4F828B0BBF10B +:10370000000F09D149462046FFF7A2FF20B18BF3F5 +:1037100011882846BDE8F88FA16AA8EB050BE36A7B +:103720005B1A9B4528BF9B46BBF1400F1BD9334614 +:1037300001F1400251F8040B914243F8040BF9D116 +:10374000A36A403640354033A362A26AE36A9A42D4 +:1037500002D32046FFF796FF8AF311884545D8D259 +:1037600087F31188C9E730465A46FDF7A5FBA36ADF +:103770005E445D445B44A362E7E7000010B5029C31 +:1037800004330172C36103FB0421002300608362E0 +:10379000C362039B40600363049BC4604261026197 +:1037A000816104624262436310BD0000026A6FF0EF +:1037B0000101C260426A4261026100228262C26209 +:1037C000FEF7BCBD436902699A4203D1C2680AB1DF +:1037D00000207047184650F8043B0B60704700000B +:1037E000C3680021C2690133C3604369134482691D +:1037F0009342436124BF436A4361FEF793BD0000D7 +:1038000038B504460D46E3683BB1236900201A1D14 +:10381000A262E2691344E36238BD237A33B92946D0 +:103820002046FEF76FFD0028EDDA38BD6FF001008D +:10383000FBE7000003691960C268013AC260C2690F +:10384000134482699342036124BF436A03610023E6 +:103850008362036B03B118477047000070B52023E3 +:1038600004460E4683F31188856A35B91146FFF781 +:10387000C7FF10B185F3118870BDA36A1E70A36ADB +:10388000E26A01339342A36204D3E169204604391A +:10389000FFF7D0FF002080F3118870BD2DE9F84FAD +:1038A00004460D4691469A464FF0200888F3118849 +:1038B0000026B346A76A4FB951462046FFF7A0FF3E +:1038C00020B187F311883046BDE8F88FA06AA9EBD4 +:1038D0000603E76A3F1A9F4228BF1F46402F1BD9A5 +:1038E00005F1400355F8042B9D4240F8042BF9D113 +:1038F000A36A40364033A362A26AE36A9A4204D3C1 +:10390000E16920460439FFF795FF8BF311884E4596 +:10391000D9D288F31188CDE729463A46FDF7CCFA8B +:10392000A36A3D443E443B44A362E5E70269436920 +:103930009A4203D1C3681BB9184670470023FBE7BE +:10394000836A002BF8D0043B9B1AF5D01360C36840 +:10395000013BC360C3691A4483699A42026124BF70 +:10396000436A0361002383620123E5E700F0BCB8EA +:10397000034B002258631A610222DA60704700BFCD +:10398000000C00400022014BDA607047000C004040 +:10399000014B5863704700BF000C00404B68436008 +:1039A0008B688360CB68C3600B6943614B690362BA +:1039B0008B6943620B6803607047000008B52C4BAD +:1039C00040F2FF712B48D3F888200A43C3F88820BF +:1039D000D3F8882022F4FF6222F00702C3F888207F +:1039E000D3F88820D3F8E0200A43C3F8E020D3F8C6 +:1039F00008210A43C3F808211F4AD3F808311146A9 +:103A0000FFF7CCFF1D4802F11C01FFF7C7FF1C4860 +:103A100002F13801FFF7C2FF1A4802F15401FFF723 +:103A2000BDFF194802F17001FFF7B8FF174802F116 +:103A30008C01FFF7B3FF164802F1A801FFF7AEFFB4 +:103A4000144802F1C401FFF7A9FF134802F1E00195 +:103A5000FFF7A4FF114802F1FC01FFF79FFF104898 +:103A600002F58C71FFF79AFFBDE8084000F016B927 +:103A70000044025800000258A44200080004025802 +:103A800000080258000C0258001002580014025896 +:103A900000180258001C0258002002580024025846 +:103AA0000028025808B500F0C1FAFEF74DFBFEF7FA +:103AB000A9FFBDE80840FEF759BD000070470000AF +:103AC000084B1A69920710B508D500241C61202301 +:103AD00083F31188FEF77CFB84F31188BDE8104066 +:103AE000FEF7B2BF000C0040124B08213220D3F881 +:103AF000E82042F00802C3F8E820D3F8102142F091 +:103B00000802C3F810210C4AD3F81031D36B43F0EC +:103B10000803D363C722094B9A624FF0FF32DA627F +:103B200000229A615A63DA605A6001225A611A606F +:103B3000FDF720BC004402580010005C000C00405F +:103B4000F8B5514B0024D3F880204FF0FF32C3F872 +:103B50008020D3F88010C3F88040D3F88010D3F8C9 +:103B60008410C3F88420D3F88410C3F88440D3F8B9 +:103B70008410D96F41F0FF4141F4FF0141F4DF416E +:103B800041F07F01D967D96F21F0FF4121F4FF0196 +:103B900021F4DF4121F07F01D967D96FD3F8881074 +:103BA0006FEA41516FEA5151C3F88810D3F8881079 +:103BB000C1F30A01C3F88810D3F88810D3F8901025 +:103BC000C3F89020D3F89010C3F89040D3F8901029 +:103BD000D3F89410C3F89420D3F89410C3F8944009 +:103BE000D3F89410D3F89810C3F89820D3F898100D +:103BF000C3F89840D3F89810D3F88C10C3F88C20F1 +:103C0000D3F88C10C3F88C40D3F88C10D3F89C10E8 +:103C1000C3F89C20D3F89C20C3F89C40D3F89C3078 +:103C200000F0E4F9194B07229A60194ADA60194A40 +:103C30001A6105225A60184A536A43F4803353626A +:103C4000C2F88440BFF34F8FD2F8803043F6E0765D +:103C5000C3F3C904C3F34E33A5075B0103EA060EA1 +:103C6000284621464EEA0007013900F14040C2F8DB +:103C700074724F1CF6D1203B13F1200FEED1BFF32D +:103C80004F8FBFF36F8FF8BD0044025890ED00E0F6 +:103C9000000004301B00080300ED00E00122574B38 +:103CA00057491A60574B19609A60574ADA600022E8 +:103CB0001A614FF440429A619A699004FCD51A68DF +:103CC00042F480721A60514B1A6F12F4407F04D094 +:103CD0004FF480321A6700221A671A6842F0010214 +:103CE0001A604A4A134611684907FCD50021116140 +:103CF0001A6912F03802FBD1012119604FF080419E +:103D000059605A67424ADA62424A1A611A6842F4B2 +:103D100080321A603D4B1A689203FCD53E4A3F49F7 +:103D20009A6200225A631963A1F58021DA634B3944 +:103D300099635A643A4A1A643A4ADA621A6842F053 +:103D4000A8521A60314B1A6802F02852B2F1285F6B +:103D5000F9D1482222219A614FF48862DA61402227 +:103D60001A624FF00052DA641A652F4A5A652F4AD8 +:103D70009A652F4A11601A6942F003021A61234BB7 +:103D80001A6902F03802182AFAD1D3F8DC2042F07E +:103D90000052C3F8DC20D3F8042142F00052C3F8EB +:103DA0000421D3F80421D3F8DC2042F08042C3F888 +:103DB000DC20D3F8042142F08042C3F80421D3F878 +:103DC0000421D3F8DC2042F00042C3F8DC20D3F811 +:103DD000042142F00042C3F80421D3F80421D3F8AF +:103DE000F42042F00202C3F8F420D3F81C2142F080 +:103DF0000202C3F81C21D3F81C317047088100511E +:103E000000C000F0004802580200000100440258BF +:103E10000000FF01008890081210200063020701D3 +:103E200047040508FD0BFF010010E0000001010040 +:103E300000200052084A08B5936811680B4003F04F +:103E40000103936023B1054A13680BB1506898478A +:103E5000BDE80840FEF7F8BD80000058C0230020F0 +:103E6000084A08B5936811680B4003F00203936099 +:103E700023B1054A93680BB1D0689847BDE8084064 +:103E8000FEF7E2BD80000058C0230020084A08B5B4 +:103E9000936811680B4003F00403936023B1054A53 +:103EA00013690BB150699847BDE80840FEF7CCBDD7 +:103EB00080000058C0230020084A08B593681168A4 +:103EC0000B4003F00803936023B1054A93690BB1DB +:103ED000D0699847BDE80840FEF7B6BD800000589D +:103EE000C0230020084A08B5936811680B4003F00E +:103EF0001003936023B1054A136A0BB1506A9847C7 +:103F0000BDE80840FEF7A0BD80000058C023002097 +:103F1000174B10B59C681A68144004F478729A60C4 +:103F2000A30604D5134A936A0BB1D06A984760067A +:103F300004D5104A136B0BB1506B9847210604D57A +:103F40000C4A936B0BB1D06B9847E20504D5094A34 +:103F5000136C0BB1506C9847A30504D5054A936CBC +:103F60000BB1D06C9847BDE81040FEF76DBD00BFA7 +:103F700080000058C02300201A4B10B59C681A68B6 +:103F8000144004F47C429A60620504D5164A136D0D +:103F90000BB1506D9847230504D5134A936D0BB1AF +:103FA000D06D9847E00404D50F4A136E0BB1506EE4 +:103FB0009847A10404D50C4A936E0BB1D06E984774 +:103FC000620404D5084A136F0BB1506F984723045D +:103FD00004D5054A936F0BB1D06F9847BDE81040E8 +:103FE000FEF732BD80000058C0230020062108B52E +:103FF0000846FDF7BFF906210720FDF7BBF90621AA +:104000000820FDF7B7F906210920FDF7B3F90621CD +:104010000A20FDF7AFF906211720FDF7ABF90621BD +:104020002820BDE80840FDF7A5B9000008B5FFF756 +:1040300087FDFDF775F8FDF747FBFDF71FFDFDF761 +:10404000F1FBFFF73BFDBDE80840FFF78FBC000028 +:1040500010B501390244904201D1002010BD10F882 +:10406000013B11F8014FA342F5D0181B10BD000011 +:10407000034611F8012B03F8012B002AF9D17047F0 +:10408000121012131211000001105A0003105900EF +:104090000120580003205600E82A00204024002078 +:1040A00053544D333248373F3F3F0053544D333222 +:1040B000483734332F3735330000000000960000B6 +:1040C00000000000000000000000000000000000F0 +:1040D0000000000000000000911600087D16000896 +:1040E000B9160008A5160008B11600089D160008AC +:1040F0008916000875160008C516000800000000A3 +:104100009D17000889170008C5170008B117000897 +:10411000BD170008A91700089517000881170008A7 +:10412000211800080000000001000000000000004D +:104130006D61696E0000000050410008F028002009 +:10414000E82A0020010000004521000800000000CE +:1041500069646C65000000000200000000000000BF +:10416000C9190008351A000840004000802F0020BF +:10417000902F00200200000000000000030000005B +:1041800000000000791A0008000000001000000084 +:10419000A02F00200000000001000000000000002F +:1041A00010320020010102004172647550696C6F89 +:1041B000740025424F415244252D424C0025534561 +:1041C0005249414C250000000D25000875240008C7 +:1041D00035190008F124000843000000E041000800 +:1041E00009024300020100C032090400000102027A +:1041F000010005240010010524010001042402022D +:104200000524060001070582030800FF09040100D8 +:10421000020A0000000705010240000007058102B4 +:1042200040000000120000002C42000812011001A2 +:104230000200004009124157000201020301000080 +:104240000403090425424F4152442500426561732D +:1042500074483700303132333435363738394142DB +:10426000434445460000000000000000E91B000830 +:10427000C91E0008751F0008400040004833002098 +:104280004833002001000000583300208000000067 +:1042900040010000080000000001000000040000D0 +:1042A000080000000001A86A00000000AAAAAAAA4B +:1042B00000011440FFFF00000000000070A70A008A +:1042C0000000000000000000AAAAAAAA0000000046 +:1042D000FFFF0000000000000000000000000004DC +:1042E00000000000AAAAAAAA00000000FFDF000048 +:1042F00000000000000000000000000000000000BE +:10430000AAAAAAAA00000000FFFF00000000000007 +:10431000000000000001000000000000AAAAAAAAF4 +:1043200000010000FFFF000000000000000000008E +:104330000000000000000000AAAAAAAA00000000D5 +:10434000FFFF00000000000000000000000000006F +:1043500000000000AAAAAAAA00000000FFFF0000B7 +:10436000000000000000000000000000000000004D +:10437000AAAAAAAA00000000FFFF00000000000097 +:10438000000000000000000000000000AAAAAAAA85 +:1043900000000000FFFF000000000000000000001F +:1043A0000000000000000000AAAAAAAA0000000065 +:1043B000FFFF0000000000000000000000000000FF +:1043C00000000000AAAAAAAA00000000FFFF000047 +:1043D0000000000000000000FF00000000000000DE +:1043E000A04000083F00000050040000AB4000085F +:1043F0003F000000009600000000080004000000DC +:104400004042000800000000000000000000000022 +:0C441000000000000000000000000000A0 +:00000001FF diff --git a/Tools/scripts/generate_manifest.py b/Tools/scripts/generate_manifest.py index 7c1057003ee..4733e2c367a 100755 --- a/Tools/scripts/generate_manifest.py +++ b/Tools/scripts/generate_manifest.py @@ -76,6 +76,7 @@ 'speedybeef4' : ('SpeedyBee F4', 'SpeedyBee'), 'QioTekZealotF427' : ('ZealotF427', 'QioTek'), 'BeastH7' : ('Beast H7 55A AIO', 'iFlight'), + 'BeastH7v2' : ('Beast H7 v2 55A AIO', 'iFlight'), 'BeastF7' : ('Beast F7 45A AIO', 'iFlight'), 'BeastF7v2' : ('Beast F7 v2 55A AIO', 'iFlight'), 'MambaF405US-I2C' : ('Diatone Mamba Basic F405 MK3/MK3.5', 'Diatone'), From d589c0fc5edff73e38e8cb14e511353671de1e2d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 1 Mar 2022 20:32:04 +0100 Subject: [PATCH 0059/3101] AP_HAL_ChibiOS: normalize BeastH7v2 in line with BeastF7v2 definition --- .../AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat index fa45fc60a7f..51d45469743 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat @@ -7,10 +7,10 @@ undef APJ_BOARD_ID undef HAL_SERIAL3_PROTOCOL undef IMU undef BARO -undef STM32_PWM_USE_ADVANCED -undef PD15 #Buzzer -undef PE9 PE11 # Motors +undef PD15 +undef PE9 PE11 PB8 PB9 PB11 PB10 PA10 PA9 PA3 PA2 undef HAL_DEFAULT_INS_FAST_SAMPLE DMA_PRIORITY DMA_NOSHARE +undef STM32_PWM_USE_ADVANCED undef I2C1 # board ID for firmware load @@ -18,21 +18,33 @@ APJ_BOARD_ID 1056 # V2 has different motor mapping PB4 TIM3_CH1 TIM3 PWM(2) GPIO(51) BIDIR # 2 -PB5 TIM3_CH2 TIM3 PWM(3) GPIO(52) BIDIR # 3 +PB5 TIM3_CH2 TIM3 PWM(3) GPIO(52) # 3 # only one I2C bus I2C_ORDER I2C2 # order of UARTs (and USB) SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 EMPTY EMPTY UART7 -define HAL_SERIAL2_PROTOCOL SerialProtocol_RCIN # Buzzer - DMA timer channel use by LEDs PD2 BUZZER OUTPUT GPIO(80) LOW +# USART1 (DJI) +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 NODMA + +# USART2 (DJI RCIN) +PA3 USART2_RX USART2 +PA2 USART2_TX USART2 NODMA +define HAL_SERIAL2_PROTOCOL SerialProtocol_RCIN + +# USART3 (RCIN) +PB11 USART3_RX USART3 +PB10 USART3_TX USART3 + # I2C2 for compass. These pins can also be used as USART3 -PB10 I2C2_SCL I2C2 ALT(1) -PB11 I2C2_SDA I2C2 ALT(1) +PB10 I2C2_SCL I2C2 ALT(1) +PB11 I2C2_SDA I2C2 ALT(1) # spi devices SPIDEV bmi270 SPI1 DEVID1 MPU6000_CS MODE3 10*MHZ 10*MHZ From 587254e5a50ff773456bd6519ba5ae4fe2d51457 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 1 Mar 2022 20:32:44 +0100 Subject: [PATCH 0060/3101] bootloaders: update BeastH7v2 bootloaders --- Tools/bootloaders/BeastH7v2_bl.bin | Bin 17436 -> 17260 bytes Tools/bootloaders/BeastH7v2_bl.hex | 2097 ++++++++++++++-------------- 2 files changed, 1043 insertions(+), 1054 deletions(-) mode change 100644 => 100755 Tools/bootloaders/BeastH7v2_bl.bin diff --git a/Tools/bootloaders/BeastH7v2_bl.bin b/Tools/bootloaders/BeastH7v2_bl.bin old mode 100644 new mode 100755 index 74ff9557445dd2b324f88b06f5b2c4d9ca40a635..534dd4f85c7344ea42cb507b629773478a4a34dc GIT binary patch literal 17260 zcmd6O4SZD9weLP3lbL(~lT3tR5@0@p27)>yAOWmQX2QwGpozo}1Z__ey_^XuBia^Q z-(~o)MzGf))TW8vqOldV1p>xv&@@QU+FRQ<1EdY1USqKiAzDvJU`}SjocG^nCWyB8 zzTWqH@ArG~+u7^vz4qFBueJ8xYp=bAq!8;1EHPYbB!*v3AqHJCKLxlA zo7X=||EmAT^#9Ru{NKQ12Z&6RczDAbOUeTyhQ`JCn~J};Tr}MJ`}2e`h_3U5e1LSz z#RU%~O1ej?p6(u5|1_;Ra$ZJFV-k0wrb;H{>;||=T@G+~y*g;2}Ro)||w=?qAIB^&&t46=~ z(ByJPcHxWGPFmrROzjSdS?Un$0o4Pl7Y*K=jYXaHlB1Kg68Q(?g^qgB*hRW2&wj)c z!($F&eFU)5ajW;%%3NW(#LOphYk)7>j>-?_33U8~#4U>BA0x4>72juMm z$U)v4W36Rgv!BRSVKjk0W%Wur_+nT#?s z%FHM;qby)`%cQOcIfd<4z{$D|SJX|w&JAx=$pela#c=bBdj>L`Mb>Gqsk*2#*SN;nue`Y&W9!_ zP2`jg0UhEvs#F(Flv*|3KQ7R2k0$4Tg`8L5i^`d_K3B=Ln?&Mh ztsu`e3$4OTp+!jd*N6-xvLlw)zT2B4kpd!5-9ly580DI{R#I^OG_8Lq#7<(c-tN2V`1L5ru-uQt}Y)5SIN1l z2f-kUSQ}Z zd1~aQ_}!pYUjDlYSZW)`&ny_w)S>L3nAIvV+V#w zs(P_<|8$wtZyFW$Pp(WI(s9p7yWGnzF5BOEA+Tv#Wa<7aZ|8Dmm;J*8*B>2&cC?VW z>c#8!b1lpwrj=8eg3rTrkJ^a6;p_VZk8%T22CYYWAG~S3M0vApbkhD=*Jk#;J8Wgs zUYjj*WLrh$a&u{Zccs8QL*$iFrbV@FDkt*8(W-N=aK9OOf_hLc@Nh zRX4O%H$O~yzWx%W`saN_J}^e15BHI4It4$t^_JUwYS#6ceQB*s(d6JYGRI}TYAUT! ztT*a*l?%LG_X@uV@`!B~!r@|Y^^P%J)A{i4lvQiuzM07D(CX7G@FEJuYtis*8s2UV@2;_P+IXjFW!o`cY3yUIWU^MWd7ti; zY)GhnOfT>@zKO>OPAiLr&M=kRn~Hv>dU5@}w;;3o_Z3&_htiTVyHD%svvE52Z|%b@ z^>>yYUcPL!ZC|yJHq56H`J6JL<%g7EZ5^qRlNx>*MsXG9RT`u`N)He22c3-kZ&4k%mL0uCxDh=6VC;>*qzEfMrK?W;%bUXYKV2j|`YF{Js#|-0z>r8s zhG4#ho?|#$W6a32W5imWn;yz8p98I6rD+Xz;mxvbZs=qPC*A;=I~=4qx8^cIX6~d= zI`CL&DzFX!OBZ4shDye2@^c5|uBy?fpOGJo5^E%EM1S%eV*LR6{5}$K=sQ?}u}vD* zlOHO2ry4n|THBvj^dxcKR>`Up=QT-#v8hNLip_tu`bX;CD9ZfeelDqS~a)(9Y1&8) zm|ZtBT_~NIR=UeQ<#_fOvwl_XgYBF^6z2X5upj%vt6@9cHG3$$YW85*Ea(R2878&!RnN$Ip_srU zu(fu%x!?zPAG?hy8Vkc#B22Nd!#Kt)TiU*JjJTe2b0JGRGoM*98n)09YqQ1JWfaE4 zoWKe}OV${ZL*_(77)L25>~ga~?5H`>u)pluLB%uc=4?SPGzvdb@~kW0`bhCNJ$DGO zv!Ii%$#UWPIyG`eMF}Ypjtm=E-B5tM=+5u93x7EH3{fNdX`S<>oN1*qOPfkRIe77) z;kw6_Jn?i$jm%I<{vShzfr18ZXbZR1{Xc_h#DucnhKRN6tg@A{#==yVtk<#;$xnGh z^_8xV;*O~+rrvDDx{b!>2sF2kUn`l)dzB{ZafL7T$SijhIU^raCS%tovh@WpA2tVpxnET|DtjLl3fD6UHxK2x0Uy{tm|)R zx%|Ji0aiV1udm`}T*mzxX7}IXUW`+^oOkuQ3ET;q^uFNiNGmnmywg1?n5i&JSGf!h z*9hU6rSn`SS>?8!t0osiUxb}q)~#=T&PpCB)5C~dySWU!Cv2rj^2kRxlZY=P<1%$j zUO_p(BgzkF6XK{EB^%Nz$ZjI97)$liuha^DxR}tAcgOUVdhfBpX%6-PwdW~Mi*W~e z0RD>NtHB%4BOhPTagRAW@>Y;J=I#uEDaZ`jz0K~)_)9Htw`^o#g~Qr5+Y&If!p-yT z68GRc4{$>(xo!V+D0Y6NPP&SBcROUgj+6DA=iGAcw_+Pc?&7@S8RQJNLv)Wm`!I#l zMeLqM>2 zjrwf*1$EPd*|LE#cL!{L)|Ype$X2FJUnN^P3cDDQmW>7;((Z6G)W~gW!Y=-^F0D6V zJ%dvZ<)fx7S%bEz^c2dLt%l;z(v+#av_v;toMufi_C5)lx*A-s#>sd?zrjXZ&Q-6G z16&boY}(7CDcQYsDOpMCy@MtPcCAps{YCB9$S%E6YmuogN@AZ$G55aupvm`Id$kWI zYMHr=_V92+KGyub#_3oS9b?;3GU|IHe4wsTgARv*`GnGOm9Pc-YP#^Mn;d^!dck+K z?>m+bclvR&Fc%u$eyPn@P})|TFSJQ(a@s_tE92Du9J5e}y0%M8gpVa>sT%p$OJ}5| zr9;>!X`3J8>o3sLt*`OOWVYn+_u`QHrjo=(C&=XR(r;m-fk6|%Mwz`7xV-3wVi7d?hk+Ne;lp- zUDpmqswk~#LbDK5a+@IeG|GMGL;6+1eeH7v+D46(sGJ~nmT2w2h4#b21l5f>oMsnw z;^x8ynI^07ei_rc0~9{D%q*pp{uF*k?5QQKTSPi{U1nw}3lkwMC6Q zafx*}9!bcKp54VAJw7UhoKDzs(BN0pIRf2X)yM(xHl^P>qiQrzOV1eGW4epGzCIUI zhie_V5V-B6@=~!y%G}7$$qq4(v2!v*(;sCVSx@e8ncV3i=9LvrR{n+pta4bEt+q$4 z5|KX|)AnY}laadTOzVD_zym)R`H3XnJqf%*igzbq$`515 zS4TBX8&!7bE{tlWHmb=XN@JGF3gzu|Tvem4hifseuCqq$L7O^_U9NR%I@{*Z_L=$qO<0c!7uZ zm~jF)z6%UzLKmo~j&h|jPFv3YE+gNmT4zuS45%}zRkJqKNOgA;JVl%=a@K;BP?NIR zN*uPiT;Hs@DSedt_PMdpk--^7+yGbUUaPlnS=+NVVJpvrEwzZs=Jwvp8e`F9GwlkE z$qbR<6}(ko1V`tFgIxAQj9tVX1GVl|3kR8&vtSCx7VrMEZv3CExPR4ru<1V!bH8&B%`)CS(Ff zpTxP>r~3-d7r^m%unsbt7xL_!AFD-atj=({6#i~CLeSeb#b3?H*N)XoIg+ED%AxD5 z-v#Thi`y7bo^t9OoZo{wI>vPHl%8d6)IUp_l&7p7iS_5e_vY^lj3*MRY4p9 z3PEO+ThpWUQmb^Rb2oI>B#XL*L1LrjJ>!Y|&^Ybi)SiOU=CTwAM&(6dIiZY^tnaqXG4?}QIs z*u05)n-5)ZZF(uRTiN1d3igCvR~nu4zEL=E=mJ@Pb8s4H?O8uR*mmJaJyX!sSC2b4 zaIN2zb}L(ruPfV}2g*qPwxCClxc_c%{aV)1ww5@0 zFkafrrqKuX(|+i!00IGk@1wJv zylDP>gS~Km^53FGkFP%feSF@XeY{7?+1?QwInTwiZhxRN!Pi7DYNSvbS(>KFlLxZ3 zJb^(RR>2}n$LJ?t-ivzHbM16xg~N&Uuqc+uuZewiUHlRIpD{Vs?1DMayOaW5km<{5 zH+H2iWeZ|`iFVVH?e$-4KYie;g27O_wl*h5lNd{X(s$n;%!Crmlfv|oHtt}?h$x;q@PGT-Ld1VB~EmUMsZcL-T<42u1M}N4rAmCCIt&R z>9?WF<0E#9b_?^=;(W(CJs%DZl0uWXT7*yet91LmPFLrh?lU}MmRPAqw20FcC#^9;A8ftR>EIBMm6OVm2Wk*Gw>)b94qjx^67DMsgYBv5~l55 znMrjZ1AeKJ+5+EZsRc9Q`VuzRj&6F_e_dr9^J?Kudn9oJ$0N4Gq?@+o;qS`vHDP~i zcJJkqo8GPxyyL*jl%Pdo#cI(k+yyOp|5pi{ zi&_{~X`7pbin}<;A>J)6M{Q?(G^~R^O`XpTXR|$nIy==>eHP^bm4&|}_ZFRy0b7xC zF!D>WBB4gfzP`mhDden6n>VkUOiy#p72h3X^4=TN-A+$HM*jX=^BX*tGv;Ev2JMpC z;L9{jXXHTaVPA{1+noWu{DoK`Hi%b=Ez&(=R!)ni(Id6~u9qx1(r21p6?f#UsP{F) z_p(`<&~RcuiWX_53H&p)bRKqr*xX9?vR&@gxTA6*s(FlD`c{jyS(@EIaT&QHR_`mx zX_27D|{6t<`v``*r6%+`Ea(!91X8Z?^w3G zuj(_E_9__xM_q4nwblE+5D($D#>kUn^bVKrJ5rXA*T>;nU%NXsXe!OI;O1YFRxq#o zh&c@>jd~d0n-hGcmL}C*Dw~A#pbv90g4`TF$js4~o3S4JW!zj{pRSw?9R(g=4qTLx z?o}nka3gHd4CMZfmIFG$0NaUqg~&%0W^X+5$|X-OvA=h44KWJu4w3~9Kj&Z<@CtJy z^IGU2yWn5kOrHg|ya~QbrV!;Byu#)VhcE9tDC1QjFpUyX;LHSqf9evn^dVrK95eqh$bgTH1TduN2f=5-`M$Zq9uo~FUE$#zrdOf zBGGZvy(weGTpSCId)A%Z@SD1cmGbu~ z(0CT<1tdRqM+3FdBhm5q4#EOkz->(LGz2RRgAo;YxZ^5h4l6kSiTff$D2D}%$@9=zL70VP5sS1titDNa)(7xvz&f&xpK(7z_73<%q&FPb=~2O&?`FU z?a0a5;aM3-0l@TK9DBC!R{eO0zW%S&EAWJ4ue6ethK@;yMtH4LjeMr2d;6u3p6^Yo zTrIN0jfJbl6H12dxIU#frLqUFTC|Mj2KY9wUXIpi?_kz0H*{oz#t33CXdhf}zvPx( zY(HAlzI45m3N7hDu14NeQ_zoFW;6_1!)P^zt_hk`@>{xbTN7Qpwr&V~hu9@?USJhX z$v1SQLq9)P@%~c8!hgkwc;{C47-WcXS;ryADfz`6I$;^Gwy3jk!u(LBUJreKeeE$N zd;W|*2lmfzs~qc*^SCpbw={s<;ajaAKS`y$R#hWiDtblKMMR+`|Dq`3=9#W-J$ z+@bCnHgGu_g=0#_{F6u?cqbFEasC>8?BprDlHhd*JetreqpRjsoXZ zobttLx52-VFVJ?>@}}(=*);Czv@WzRc-!3t&Z?0~;DGY9Q@|UX6{$M|^UAg%Ave+l zSUdkW=C|iSn@+?T*wHvG3&w+1tO=t_jST%Np*L))-h2EYtU8=&#;(#_auBb?gvKrn z92^DhY8P2G+3W1cU7yG6PUf!9U7NeUbd;3lu1#CPmO7;42R);KhaIBcODQ-(0g+8> z$U%={t=1^e*p6~`6{Vx^VC>joF@t0D2Cl{Kn6M{cpHla@VM%JW%x*REo=QDKOetM0 zDc(jme_|q@#9!8<+>O8jIyU+i!NLBq!16e*1oJGSzbv1w1sA)DzDo|PEMDb~j08kO z7cIwI$W~p2R8+an4n41|--^hv#10LO{(=lxHQ!?08}2m6 z$uvspvxCEbH7>y@wpGc2hou|O)TVk*uXFsohA<`ym;=9J@(c*6=z9-^)k}*AxAf|^*#Pskfj3Aub{B0W3Bd+{v zOb?|q5f4P;+x%U04D_5$^QM=lS2nrRPo#U_nc1J4Wua?lhr5_YY}w+GX@{ZLIk?^D zd=|dnLku+LY$DM;!w-LVo`=|>=?ni=;)mR(TbUCINhSHZeww@FfU5^N;Phl!da^8? zDXcd?>qZpQT}W#Nj#t5RaZJfA#^?z3eqdia0^7s$Cnei@OKwnUsy?oe0o_o_4Z5Kc zbG77Oo_2z<_bLmD*(fvYIL9#WDhtf8CARJxBHY`G(ba}mLp+q4(2HqqZT6K`qnho)oXH&2Wl>*w;w0e@xW_`V$3b{VEKw#$LK ziFOHCYd3#aSnut@8qxTOea4kdTpXTn$&JngW6%oBBcC7gWHCiQbstrT4{6;|#gK}We*Sp6MBj4XRhpgkbw-D&cB*fx zY)pxTD1Ww%odi!Eqw#vm+cvBom%}3)xwhI=2lxca*~RJJA=u|fJosKp^@1z~0)1;+oznFjbifrLh&^b6&A8nNP1fjp&M_v8tC+4O`c_W$o@#oA5D@yM zDbdYi<;+wmMdnf-p2-$K?@gF9v%C8ZzW5eI8|Glf46HV;xjSVza0e6RMt1rDUmyaR zvm|)uc7Avjv&d7ytgr4>a?R-;VtW@Wtia4%gVxhucGdW5XSNYS1f-1Di%D# zxy6oidObW7;1}Tu8&q~%v!o*!mm2vs_J&pS!69b_vt|``U>6_6-&q;mtaeW>|1avm zTjsaY^oDfFBzkezr#z?pc6NLyz3MYbC&cNUu7YjdE{926w>?{;vdS%Ghf0jhjj0D; zkByAhd$=M8BCkB>j(PBkjqw_n7DyaQn1acnNYCJNj!Mv8D zCJRzu>XCU5B)-&%cRINQeTdX1PJv`kD2DGZ(PJSyVqu zD?P2x>D3Kw<4#E*bnft#Z(l7PjL8>CY^r^Gr&;Qkn%t9)XIbD^$xB%OHrTtkx2W52 z`p|n8yc<#e{JJL{u$9(r{w-GeNRZ;tJDItR_E*XH!4b;i`+ZkSpNe<9WhsTVW<9DH zQ|C%7&Ie|0+QG@Nbqxk?$enG(e6e0qv%9ggSG#0Vja86}8aiK`0K5zHO=avSlm!O5 zQ!}0(?9=`(aQixN+iDf9lBM*z?c1g^H@+UbIO;lg%tfiZhx2KINA#AS>8h^OWf5Ody6nq+NF8hSOS8t}aCOA{g3lI8U^UZWk z9Sy8{u$D%CC{^*uhPTOeS^d4(g@3Bo1!+#TokV#ING9)=h{WTfgo>60sXLL`^kXQ&^sK@@Cc>Nt=$?)O8zp+>M zp2Q7_T!UR8Ep@0&(=QHt6|C9oGog=UDJhuLU-x8MR6kmaBhwk>e*g6sO!5Ra7$+u%Z= z`m~hO@ME`2K8RVecTv^%D4W)yU5HtAH5|-Vt91nEHD0?jFA$FsH$r;%8Z| zm)GD&W!-eeu7hO!Rm>z87$!#}DE@z=$am!7xbjX}US?u&( z0_8LIvzQ&yN!O&~fOK|1I<1h-U7B?0Up4Y*j9K*S@J>Aw!mbj~&t-=h_PK6ESna4f zXU_jdSd$3k(JviW%vc2m>z@Wm?h8X18N~K9qBiLJRg1I)KA+Y2yIQ;^oGEit64ahr zj`hfF>t>VGK82PmPs4)gpk9N|;RFWB@JKHER zy@Q-rfw<^R$pe!%g)qE5QM#3R2%jKSyq#a-x$(3p=N=_q16 zdPGl>o=2nSsm1-Jj$4Ft`5z-}&&dv^D&>0bYwWyWDMc z>(pgp?UcS-a>3gYKX>usF4>C2}+|o=oz3qeR41@ z>*XO``bx{L<@w#agtvxWQ*urf3Dg>(zi`><_TcYWsI9?ugKMFysI3ky5FM20|3@l~ z59xCE9jCqhChQbe3zs6lkF_;?v-Flsug}|Iz7`NEy`O+%f}S`EqCLQ zvA5}4`-Hq4ke6k~i*g}nZPmlVq9X(7Dhv~O+L(atYzwM-70#2!TU z^oTt^OX*fb;;_lpKzYG9-~q(z6SQ3Z{hJ)#c~;e1YFT?fl}|UxAT9=F+Ls2es{gWl zRl@}Al|14OX{-2KsYgurd1}2ru5O3VQd$-y20x@uRgS}{DjhIO~XFWh&xH6 z)jq1$*=0SvUWxA$%79iofLeo%;u&mnDgn>Wg!X>`Pgel`xLk(%C=zfn;3G-+Ho%9I zupjX2N%$$i`;zcpzSx#y{cKNPT{P!mN0kU4wbt z82R}I_)pxjfwhP<=aI)Fx5K|=zzY#aA*H)c z9cfm+&PXMR z9C!2+oeAF5dKt{iU=zXbQSh4uk4J%%3w~2L*T%v=&Ygdk7krg=^5ea$DlOn|i+E>c zzlhh(T0|SUTWW9hw)mbwEhIY{*kBP?crEL*MqQiuVAW`#(6SboYpK*3NPRic z{q*|975();sb76>ais;RzmE2_7nlWT$;$aF4-V&lYb;a_{iqsktn06J*)4U~SN7iv zJA9?(zH2L2BemRD=(x7hB${wY;%&OFE6J0~eLM!qG+OI`JGIjE09roKT;{+Rdc;={Ujsbf zJEhX}AnG1$E_(rA=n-E*eAVI0P-$9^y7g#>FZ777Aifek&PmRT+O3x&!dOB&GrMRE zw)PvBONo-JU}L1s!p0Ho?jvR^0%%5fS%QZEhxL&Veu8c^<#NKuH1p z{Ti7&hF7PW+ior@EMK&2*)oy@7ZhE;Xkih0A@to(i$8yse*OJ_wO0|t%}7roeH&>r z(gR3$AuUJx5fW|r5aJx~LHY^O<48M^{s)o|>2{=_AyGW68tvZ$wY~=_;Jf$!`)Gc@ zix@WIZJCaV;`HKxqva(V?)`SP@Ba0VklF64l9i>iuXQ76v1a+oijozxQIGYi{o85A z?=Pprr$$_)8Sj#NktXW6Bu)kDDQHA`6ZI6H7!SSAFbqp}I`F1vCx&kzw;_3elZ`Vs zmGux7KY%Js<tx@fkUW+V$q z1g$K_h@NmRW|iCtqAZ6&@LVdNoFQ@FSNrfI<%_;kNSyNvixw=rVNvno61S&xxc~zA z_q+u>oq_oxri9M-Ug!l)bS{q|4LQ)1dWiqUCHriPW}u zYcCsu%7^zF4b_-vrAAut0|++OsBw{KtF7(b1=Byo+lr#D60K)p!=5B0=l`3t38L-& z-+Q0?+~@g+XU@Fy&dfVA@4WNQJMYXwQi$*imYD7~6Vp!-BIEHM#2vt29A?e_divtL zZ^8?%BPKt>zaVrYe2nnTGXGh6%98(n9ZJ(b<*Ryk{}*Yze$7{P{Pe;(Nn6(~3WxA&g0KpCRNOgd5JyzcYym zovnH>baumoH0SUc1v%Ts%TDA}DTI9RH*?wz=N6CGNfCl)v$xS&tn#}NPFXQhrF`2p zqawHRA?51USyTC!v-k9$haY@44IGKeIfnMnjz=^CEYNt7CuD2GOgBAgdQqF9EAvO>kk^S_Al z;y{#+gCWL)rY@rV%gFASz*T#yOH4fmpRp@<8*69wKCkjFq70012X9vSk)WHH%w2*$ znym6(IlYrn9@U7`Tv-+Wr#mN?GsM^Z9wsI?3EaLaWh_=Xqjka1ucV{B7Tj72svbDwV&Ana4!H!^WMSL}bOXl|F~u z=5e2C4KDY!%Uq1qaf76&%@b4}3}T*?Kaa42&u48Q$`j`+eaF;KF^|3Y9#hRzdR4QX z#>dpp&Asa9z-0rM4O}*G3UCT=R^Y6_S%C|hOBCYoMNDD)MeumuofqXzz^;ui&hcGQ zkuFV>^W_OypY7{aw+b{>6L7?X^J@GQ1{J}3?&LooIs10Xk+V0<`;BoW!=9dODi-I8 z%);{1B!k4cYF;d`GMjDy+?ik55g=UDmd{i#e zA(wL?mroA=B?`&IY+pIdP3Kxpzej7ej%^W1!AD`Py?Q!dK+gQ7@%>MU!cCr%eJQc- zH8^>@Tb+L`QG&y(Md%>YjVF2grt))9-u~0_v-($w=H&|_Kc6TEhk1K?bNXJQ1cv9# ziAO$;k}G}_CUd&Nw57T+thWN99B5>O^~;Fz=rHJ|vVwpf-8^#3@b=;5SapvN8^!9* zHD5Ul+ei3PBO*50E)a;`Ikufo-SqKW$_l zxck)5!5B$9eCB(NHia{|m72zsfiw>72rV9RW>w?MgTc+@OjYBzmrm+Wa&r3+-^{m@ z!oz1u8~OIdmCvqb7JM0f3#0vfG&$Ph#u-;mIsV(2z?+}FOyNjT#pIx#yv{8f`miI#Ahty*4__Z81xw1(m%q9 zA4ivqTnlu-5Z9jG{AJX-m(KJu%=FHoTXkKx9{S~*x@HS>+A^$xNTF!YrLx0 zD4q1NKJKM@&cpkO@>?D6o^(RTFVyk(?N8sAjrn_RXp5K}$KvOSa{15-F@5hNQ983- zDxF#F+@1R|vzeRYx9?9IFpbabl5^kJTes-6r2XJq+L6-m(k1Z^_Pa$>j5iU*JH*@5 zn$sZJ=hc|LinPSYc{&5-=+kK$J}gv|@~NFW^*q%dPY&_z_C;BROU@mC zo_T7e>QqQ*s*#wkMR*=zDCMhAVj`xwX-Fe}D3zFc5n>3mOe)1AZbEz&@+tf~z`O5b z$Q7TREyL>gA<1X=Ppw+&eP68(rl`#Ij}8&nEyG)fYVw$HhbJpit>zQPF<0c|W2&&b zPz;wb1;$94?+MjNnpD!vQq6WhGoPLFL8K}^+yGku7LMjR6D1*PTf7SSY=sO$|LX0f zU^ll^U61yb2fyByS#OK~T3dX9A(HAN0&Qucom1oOlltQJ2TrFnB`kVU+7fi)3JmdOStaCnr9O_F#OQ5U}?UP2&ioWV}XVL?VO4 zMdXGu3O`I}icucyA;jRb$p+XA?Y1Tld%|4GDEU1^XegsJD6Qfy zEA2^$n8{5}pVH-AmMdSrt@mtt%58FP#;zfg@S3L}{HyJ+c~T;bBV(pvge|>IrudRG z4Bz3&3zwYX5Zl5YJ#9ry)27gkj?(GOwdrC;X@}?1qf>{N4b^$y>*PeDGV2GU#Bn@Y ziX&0lCmwQQ_Mt6i&9@?@1tP}f*K2}O=8)S_w;38OMD;Zy#Umv%L zt=4hzZ{qj}Te&LG6ep`z(UK@foaU|;4;OytrZ-kIMMF{8a>UhDIz4-c$xO$d*D^$w zGjs0+r)=GTg`myV!(8)al*~><%%ZD@6*;lhW*K5~$*kc>Lm6Aji7g&B%&AQ7aI~R} zt^?;XgSbt+RNRG`YI@~L@qP84=HI3hlb6+} z3^b)2^!z>y-3Hv75y%={wfZ+HBdpK1388`Vi>?v6ri!~RS$4_w)XJW7ewIk%)#tB} zE#-Y`vv5@9i@gfV9YM?}|2#CgqJZX)(!7jvnwwFv+v6NBQ5FB7C|%;hRFa8O_eyck zr?YX^C?O`dCzHHI<9s4Kt;>J9&mh`1r6pG|&H@=>XaKv_VHLGRwPuhQ^dw1V3Fu6$ zXG)LOABz$~y4nwZc{a6CcF3r6iT34LLlfs~Th~^f{MTNOQq(T|S~*rP$6r*gtzIt~ z`unnt1CWJ zt4eMdy!|Tq^MKtrr#2^?t(Y=1LXb$|Lt}Z!tq7#USfvOYh26wYaq(aSwPCfyyW?k? z4-Kh(Nt?GSP8zhtTjO}tOMPG6yBn)0JzEh>^CF8$z8w`KMC&zC(9d0_mO01Ff*+ z)hb3weU(yu_JQT%BkHZ@97xJd!#jcbJn(8Xc&OmsZ8{u^Uaq)}X0&*xc9ryhqDg*i z0#7W^=Tu5BALe7(gzO?tjA~;>1yMd#`B*V9G@a^81;#&=>dUat6i(e{@;SdxcQG^C z``$~Gx6dIggsJdvaw80E$X(wje&DBYNkXIPI z3d40sp7Feh4+qs-U9O(Z&f(}1=s&Dg=sVhH{k#*C zyhNG7rZVnSZqV<2`%sYX(H5M?W0JeYO$Pn$!)kCY2ia1)rixo+UQ8FC@sOkU%D)I) z7Wk2^%VRuh6=!4B{A9EvP*~dGpLBA{i37RwM7sL!9KA;TKz2dvtRMYQzNYjke}`Nk zuF36?ROoVAOBy{Te-v$!TRhNnCA^p^P8BJ23Dm!(Q=cs^M7d?7b)a77pM27CV$VQ| zX`YTP8TE+WIVWW;u^99{rL`C6Lt8Z_YhHyGKExU~W1W%wUO(GTB)7CxdR=Ptm-tyf zS8MJub!`cF11*rZE%KJ`EiyB2i&T^6@#i#n0$c!cc&u0U`iY$=*J(sB^-z1o1U=sX zKL9UI7ODL1k@s|V$WH~BMIF+rybk}=O%71Z51o=b{HGwbg4l{3h?Xo68+zvI+R>xRPD z0xm|}J&vRB3xH`}Y7#e>!rXYdx+HEMEq60uYE?j2_V7LqTFompNxc%|NnzbY0(m2j>xLDajuS{{%42`)RTqc)=(?9xr!& zvfMmct_*Nk-IZV5WswbH4*J7{8KJm`L7#>rS$8wSP1~E@ygME-V1$g4sj)a6eG%?c zcdf=Ke^4L??k36uLxNb`<6fsFPM{6STRLZ(yRv-DO!hKfq&zmk;+;vZ8V2q&3e!yXW%ISfQ)46Ynm{f* zq5|lg9b@*<_=7Q4EbnC6=jys`x0+lT+(D&hM6L;B$z=9mIClrL$l{JcYJ%+PC?S@) z#c0Xy{`c`#(4%~dyz*{tU<>!2+#a}DUfRjqC99T@$C9gwQBI8t(4$yWx~?nkq4UTn ze|?GWc`la3mNQ%fUAyq3FWTWb<{yr5jo!wFvSR4So=+Rxj1uW4gxjw7 z!OkfRhgP@K*xN{AW)AJ-(smf-+b9XmQk5yH+kit`hZ|?bQzS*hNY*uneH=e(Gwn`Om19roV|i5{~la& zoFKkG$l&{LL1L#gI?gBKmh-fimd+GlTgy_MwD%1?`VL(1r|>TIeqon-pUZoibw0JB z#Zw&WSi5T|=-RV^6jJ*)cxE(6X@4F}b)cp|_iSSC*igHHO3tcri?^YtYp`)MQ96TN zgYL~0;VyU)UqPR@xR}DhG#K;Fasva&pJor=!kIe4#m#lzKY$17p;VsFOMSR&}S8E88*Gl&iEI zQutgLSQn4p(%zH8r^Bq?|J~86eaRjX=M!jut631ID8WqVeP(i)QP?4-ASKLF@7RGW zeY?^7wb3hldl1(}8RcX3OMM)qaC1E40PVE}^_IkcbikG(h-@FD93I(#a-AdVFO*aF ze^svbK<;?CT_bBy?%t8=X!eD9Erbp?7jnOvb#aV~reDB+RcqPU zDcIC=a30M^C_pIgq7u%FQwwJufB#cXxvqfz;hR2dp$^j2bZ7)K(iy{Jfj?%a`ZsGKlvd zOj~9SI|Iz#)JmeP9ile)M;ebi9R|MN_917WI$*@U2OC9GqqJU*KxH5UKDiQqVW3fN z!>G99NxN-(h@Oh>(-`L*KRk_hC3o_%#BJRqMCV2lZ40Bh`GMtw*f^`*iTv|cTxA&`zvsATa zaCc?_V`JEDxaE>=FO_;mwqpMl;FnSOXf;*#S!)3j45&nER`ve+0p-F!O^5T zFh`W*wJ)ZYCr`bw2}AFqUoObI?(5)&4GQ&-n90d+|LM6mi-D*9hUv>-ikpb=DTjuXt{e>ilo289@en-ET(d z3BDk3yle;J+XBzZKlglm^u1_p;AMDu9+I84|C;-Vv{z;e=7g-)*1V)w;Hg&-KS=q8 zyF)kU`>tFgue3FLMvhZEoc3{x#5&l!h{FTRyh-ke-NSm-j5%z;t1h17!MAvhvC^yN z%{hWREAgu4Ijn+po#yJgTA8=pY0c(lBY6O;?mf**C270EUa-JKg_GiND@Xl+r zFG(qej}5l0sS7iw^OX8Qem`!Dn0hKYQ+yssfK{rIERt8^k^9PP^t73j0}gVL61K^# zv|O@^--XmmuO;nJdU6ottsW97?%`yobd$6cxhuxvQG<_*WwSN@Hb3ah>-S^L;KY}d zH?@oNv$Jfz6!C~u9AfP^)mwcwafY~3&A#Plp0r4{ztx{tYmk;ojglz`T!o!->Sb&F zvvR&IBZocqk?jhZKl!+aJxaaLpA0`MKkkViUneh+ejrWPSLPkkucdu*YVJO1#E+99 z=0lg~U)z>B#Z_wdj7E<+T!QtNhBdoFYV$x|nAhR=Ympn}^wJgyH}1ZMvNV|pZJt{H zD(NkWi%=ZSCoi|i+hnd(sBe*sl`HGBY%QMXiN|bu?yS~OOP&>ddi>=HIiR*KT_2QR zNseNUW~GEV1ZGxBIc^eB!cg8&#?5XE@#SRTh<@hC_Yl)@$a6mI#M!_(+4;Q6EGEtzvHuV|zuChax8baD`X;6b_&i=^ z=TO~rL9dl-i6z6y6$YO|i25bX(8z{amtg;3luM>^ms~O}DJ_#L=?UQIupmyVq$iC( z4rieDCqv2F=AL9JBXEa?;?ZP^L+b6JFQU~L!#xO;KGjbd%Vvui6@1J@v_y+$i2a`B zV`h?Cp(P&B41>Sl$V2i|Dw2AuDMUgCuOb5F!CuffgwTUP?Kl%8Z!Gcn`QILb1va1C zl-_NHe`6kY)v?4+aL;u%0xgF1K=pw^%mDVj^Mw_^Jw!F>``T=HfvN5`b(+6*?oi#8 zla9ZyHU|-!^Q+!hTX!QIymFJ+eOUut({!g!u5qDf=9O1QjE)l6&tr+Q^9|kasl;Vm zV4Pp78W+EU7aRF1x42zRanZT>OVs43&`Zuc-`f3D-ejLeWoEGRQd3tgefwrs<@0hj zQEP<4T%HoK&NnUSjb7HkEMVq`u)mi=2IzQKBi5p^8R@t~Sq7NCGJwtwtT3LB;8lfv zrBQ`f7CJ^CD@|RKaBK5yH|~tJbYH(5G4g$Bl{FF@+cvL8I;LjWj~Y|@QYw2@-cE5e zrt?2}9%`e$lUcje)HMY(&f8-Cl_c)ELjd zi!&Hbu3Dl;L#uO%l2En8gZf$4?A8*8Lx)xKTrF{<_H@j|TWy*b;#VLz=uii_!F)|-_BZr$2c58|2v_-i^r85e7^tVQ%cNgk| zg2N6uU=(G96kHuP#5Q^Dm5%Zj4}7zsR*@Ur@`Hy%$Hce9cCiEf*rxB-85$iWAIoF* z4Dgfx06F6@GM3{c=Zxbj5wa9R=5U0*(|OEugUShU{cVbtP{wGzWO;fP;G~JxJ4b|0 zEn(K^tqS_IK$yQTs%e=d17kANU?(ea^Sx5v_cs-Eu7VW(pD_lOwYdjpFRTU1GvzgK zL6;Y{lwX+QJ9^06)r=j$+*6uI4y{rJg4bf1ClAG=%iW|p)932S+mO#2-p$*Pw>ED> zX`GbitxaD>>pJD5hrIBOIPp4(QgDF+-jdxy4tZ5!Ii&!Zrtf3Q*=kD5*u^*)+=gq3 zBO241d>3VbWN3*q+HUMn>^=sX0BD~DUV%)T|UB~I$B;n?5I*P!gyDkb=XQgzwlbl-b&scKsMbWG-GZTJ93tk`)=)#Z!L zc=t6NrPdDREwybZ%^3FkID*nO(er7zVZuA3tci3@z@xuDnn>fNlbOQfubX~^*G)Wq z-Lyx4-IOx9K(vcDoF+zQA${G{t4bTI^3ILFZTg!oKa}Fc8!7r4P#NzJ-NQ_k^iEuN-r_MFYbaZZxrUb@{cM!_3gS^~DKXLac@qIX9v5-o9s=IwX7>{^-2?dnkA<1{ zGf%Hda_}+?!ITO(7chb;{clIB6vpy~npf=Vq23Jm=&PV}PFCeroi4HdRn2xU;oeor z;oD@;6mDCZc8tM2-~3`S%*33h8Rm#O-%5Sb&kY#3*Hp9g0&XOWM0UWG@@Lf=c=6B+ z1NUWGj{g!j1CNQn#2c0xl@E2t_{<|JImn*a7vob0qti!%xz){8({I@9I5b84z+r|8!7Wmw%>;LaK~|9Luz{wA5#hRDuoH&$I%cR&kG_i zC^+;y9d?Z+sz!=^r=()EOp1vUH=Poo>kd1eQdOL{bkr3ENa6W@#+e}$L32JcToiZ@ znv#y`X}~m(Tvj0dQqmHFwoZOSHOueen~ji%^Yt~jQR<*pT~QtGJ-k>G9a2Njblft% z70E%*ub=4o#*2Cm_-j2!`a;hyz;wh8C3s=PNm#Epw6XT!!O0!tRV z4pV-b5UX93gLo-rhduXws`c|V_;$^>UsIT@2X)zd19F0K6z~JyET-s559U08uL_nJ0^YdW=~c{JhdBM1( znKSr-$sATx;f?^A?Fm=zFvP04YrJci50^8}cfkQ}IOD7Zz052AWhrr12L7a)7aC$q zI5K-tIBf@y*9+TsK5SxUZ3|U;!)mMfMr+Aw)-y{ythP#mSOe+!;TU@3%H82TeebFn zUPG*$Tf;;zP&W)trPQnGotU-iVANW*Kgth?7RYa*BTne*FZ`+}j?*0X67rk4%u1BH zWENVWGw2=rGdPE{XPkK1VZ-2WkwWi_ou>_kf1~>Yct?Uff(aqROz^-GI!DNo(ND}+ z2zx*^_VZ>}1+%6an%T{V@tu(!VgwfUNBRFF54wl>gPc>JE?XoY&gyh7s6KLa{Y8Ez zV3ChWIoCZMhfL#-c)* z1BNM_9673*ifL*1JvN6SC)pv`9ILzvS#=mGr{7(1IK>3dE$uEv5 zXUIra?T&7%+%GqKCLPVPHGP^*l9oHQRJ6oSZ7X&oI(P6+4D)B!ZFj=rS=aancwuP? zUPxN#;&ZoP?STqbe51iLtx~XMcfU%m%hX#Xq9Xt>`H0Zu6Ekai&GZ>0p}4*zOw9bi_6e=&9yU`~hI$3A7bK3<1^HntD2L5Ck4s|TE-!`sIS z0ULF=Y3xge>oe)_PsbWKckFk;&u}hDwrJ9x0i0}+)z$${ws?N*D!|DW&yD>pWuir8 zY%kzsi>Jo|fRinLG&UP>vcM(o;u9vH0nTu zuKtOcXRc0*TJS=ZMABqqAs1t~pNH6ntTe_oL)Wx)KC%7-EY_{*uv>m=fJ`P1v-qgT z6ruN6ZLpBqq#F6s(p#j|XqHaNBtNp0jWO(&&;u$~ep*J1$@qjy`TekNy6CBY+gQ)1 zCbyRO(aUtNPU8M~9Jl^ux>r*z!3~s5iBRnpCzjU{Q{8$Ie&INN;WzM$oc}@jalBsM z)x{Tak24$L9~BlS?>IfLPS5Md`J$FvmwYvleDgt12=~=rhjZBZpCH{#1RN zC)xTV8ugqmU3&Z|ZSC!-ORNz`6MIHF>c3ZdbLpDWj#}Ijz-P=p&KEHod;Q!9ZzoP7 zHP!aka+oKsj&FZMpQ}cFuDo?P7ZJ9e&d(9aQ%`4+IFd8fGRm8OUA zHV0m76WxW#WiN=os;|OZ96HOpF=v#+E`82+*Y6uh;iC0RY)|6EGedM=m3zVvb93KT z`NAhWZhXh*>%we()#}eQI`=m$J#mz_^2g{b@fyt7JHs9I*Op#ax(G9dxASmXY;d; zemXxP>YF7F#;2zMGrsD&3A|7C;@w#j-kkMH=>f0b7vO4l2E4WDxT~Rh!PGU!=RTv| zSy+Bd-6o{LLfD40$2P$c*J>S#k)22%Q%~x-Cy{Hi({d*Jj7q?B>5ROiO)F>NWubGE z2dEg(5zPi@HJ~0X2T%i`m$XX(Jpkwh?GiwH0qxha0lfiepJoAc642Ay6hP+zJ*kC5|?KOAQOf^OjO+pVpDx1wD=25zhQ#D)?@D=CRiMX={vq0sZIJ0+Q%)5zbfwbwE5ct z&HVX3n+$KwMw_(4XWNh!cW>syRq)=})}q{6=TeB$(4$(T+$dvI0 z(`Y(yW=svk5O{G`nkr3QE%FpO-A!N0v`EfQ171Vpg)mNC$`f2B;zzW5KznkVSiqrdeN{oQ?`?4>?) z*WWeZYaq&_t8eW{g}^~hV_ zTGoM-p|S<3FjC3e;@ja%oZyS?qIT}_v`Mune@Ug~cGSMTwakeWT16_1)JdN2%dE8A zfxJ6f%YK0rT16_1R4r1bO3Mc1Z9qMw&?-`4q>`=9yD+vCyQ#~BT`e?}VmAt-iB}Mt z5dQ{oD&iLqry)Lo_)^5rA})aU&*X;f(oOdpEph+oXyS+JXrgRnG%*8kY(z^e8l`1$ zPJETxG|WP_6t`~?dRt(|+j_jOuj=p&hON2OhT%k{3-K$9LNc4arOz^FS+b@;w9GYx zGk=#bLN(!p8wtHHqwl3xt-ii!UisBamMkIT;QXSiu3k_?=-(sy_aA;9x&;590YW#z z9)wPWClIzH{1D+y1X}S4yg}TK@FqehLJz_|gr6ZaBG~axk(Ptx>wmZV19wnhc?tZ| zxL>CIqpGs14o?p*W_qehR+i4Z!h=VOtCp^;C|NcW+0f#B_40-QW4el zzt5o9-=2 Qx`Fuxyqnr#|H0q?1#Nhkvj6}9 diff --git a/Tools/bootloaders/BeastH7v2_bl.hex b/Tools/bootloaders/BeastH7v2_bl.hex index eef22214e3e..b4515e95f9a 100644 --- a/Tools/bootloaders/BeastH7v2_bl.hex +++ b/Tools/bootloaders/BeastH7v2_bl.hex @@ -1,34 +1,34 @@ :020000040800F2 -:1000000000060020A1020008791000087D100008F9 -:10001000D11000087D100008A5100008A3020008F8 -:10002000A3020008A3020008A3020008292A00086E +:1000000000060020A10200082D100008AD0F000816 +:1000100005100008AD0F0008D90F0008A302000862 +:10002000A3020008A3020008A3020008BD280008DC :10003000A3020008A3020008A3020008A30200080C :10004000A3020008A3020008A3020008A3020008FC -:10005000A3020008A3020008353E0008613E000824 -:100060008D3E0008B93E0008E53E0008A3020008E6 +:10005000A3020008A30200082D3D0008593D000836 +:10006000853D0008B13D0008DD3D0008A302000801 :10007000A3020008A3020008A3020008A3020008CC :10008000A3020008A3020008A3020008A3020008BC -:10009000A3020008A3020008A3020008113F000801 +:10009000A3020008A3020008A3020008093E00080A :1000A000A3020008A3020008A3020008A30200089C :1000B000A3020008A3020008A3020008A30200088C :1000C000A3020008A3020008A3020008A30200087C -:1000D000A302000861110008A3020008A30200089F -:1000E000793F0008A3020008A3020008A302000849 +:1000D000A3020008E13E0008A3020008A3020008F2 +:1000E0006D3E0008A3020008A3020008A302000856 :1000F000A3020008A3020008A3020008A30200084C -:10010000A3020008A3020008C13A0008A3020008E5 +:10010000A3020008A3020008F53E0008A3020008AD :10011000A3020008A3020008A3020008A30200082B :10012000A3020008A3020008A3020008A30200081B :10013000A3020008A3020008A3020008A30200080B :10014000A3020008A3020008A3020008A3020008FB :10015000A3020008A3020008A3020008A3020008EB :10016000A3020008A3020008A3020008A3020008DB -:10017000A302000861350008A3020008A3020008DA +:10017000A302000829340008A3020008A302000813 :10018000A3020008A3020008A3020008A3020008BB :10019000A3020008A3020008A3020008A3020008AB :1001A000A3020008A3020008A3020008A30200089B :1001B000A3020008A3020008A3020008A30200088B :1001C000A3020008A3020008A3020008A30200087B -:1001D000A30200084D350008A3020008A30200088E +:1001D000A302000815340008A3020008A3020008C7 :1001E000A3020008A3020008A3020008A30200085B :1001F000A3020008A3020008A3020008A30200084B :10020000A3020008A3020008A3020008A30200083A @@ -41,1052 +41,1041 @@ :10027000A3020008A3020008A3020008A3020008CA :10028000A3020008A3020008A3020008A3020008BA :10029000A3020008A3020008A3020008A3020008AA -:1002A00002E000F000F8FEE772B6394880F30888F3 -:1002B000384880F3098838484EF60851CEF20001DC +:1002A00002E000F000F8FEE772B63A4880F30888F2 +:1002B000394880F3098839484EF60851CEF20001DA :1002C000086040F20000CCF200004EF63471CEF22D :1002D00000010860BFF34F8FBFF36F8F40F2000043 :1002E000C0F2F0004EF68851CEF200010860BFF374 :1002F0004F8FBFF36F8F4FF00000E1EE100A4EF604 :100300003C71CEF200010860062080F31488BFF330 -:100310006F8F02F0C7FB03F051FB4FF055301F49C0 -:100320001B4A91423CBF41F8040BFAE71C49194AA9 -:1003300091423CBF41F8040BFAE71A491A4A1B4B99 -:100340009A423EBF51F8040B42F8040BF8E7002034 -:100350001749184A91423CBF41F8040BFAE702F0F2 -:10036000E5FB03F09FFB144C144DAC4203DA54F848 -:10037000041B8847F9E700F03FF8114C114DAC42DF -:1003800003DA54F8041B8847F9E702F0CDBB0000FC -:1003900000060020002200200000000800000020CD -:1003A00000060020D843000800220020442200203C -:1003B0004822002060330020A0020008A0020008AC -:1003C000A0020008A00200082DE9F04F2DED108AD0 -:1003D000C1F80CD0D0F80CD0BDEC108ABDE8F08F7D -:1003E000002383F311882846A047002001F078FFFE -:1003F00001F094FE00DFFEE738B51F4C0123002515 -:1004000023700423A57063701A23E570257165714C -:10041000A571E57125726572A372E57200F008FDA1 -:1004200020B10E2325726572A372E57202F0BEFA46 -:10043000044602F0F3FA0546D0B9104B9C4219D09D -:1004400001339C4241F2883412BF05460024012545 -:10045000002002F0B5FA0DB100F05AF800F0B2FD3C -:1004600000F020FC204600F013F900F051F8F9E705 -:100470000024EDE70446EBE748220020010007B026 -:1004800008B500F08DFBA0F120035842584108BD8B -:10049000054B07B51B88022101A8ADF8043000F018 -:1004A000EBFB03B05DF804FB8040000810B520238F -:1004B00083F311881248C3680BB101F097FF002342 -:1004C000104A4FF47A710E4801F054FF002383F371 -:1004D00011880D4C236813B12368013B23606368C6 -:1004E00013B16368013B6360084B1B7833B96368E1 -:1004F00023B9022000F0D6FC3223636010BD00BF98 -:1005000054220020AD04000870230020682200203F -:10051000554B56492DE9F04153F8042F013201D1D2 -:10052000BDE8F0818B42F7D1514C524B22689A4280 -:1005300040F29880504B9B6803F1006303F5003351 -:100540009A4280F08F80002000F0D6FB02204B4BB7 -:10055000187000F09DFC4A4BD3F8E8200022C3F845 -:10056000E820D3F81011C3F81021D3F81011D3F8F4 -:10057000EC10C3F8EC20D3F81411C3F81421D3F80D -:100580001411D3F8F010C3F8F020D3F81811C3F801 -:100590001821D3F81811D3F8801041F00061C3F886 -:1005A0008010D3F8801021F00061C3F88010D3F8D8 -:1005B0008010D3F8801041F00071C3F88010D3F898 -:1005C000801021F00071C3F88010D3F8803072B62B -:1005D0002C4B2D490B601D682468BFF34F8FBFF370 -:1005E0006F8F2A4BC3F88420BFF34F8F5A6922F4D0 -:1005F00080325A61BFF34F8FD3F8802043F6E07EFC -:10060000C2F3C906C2F34E32B707520102EA0E081E -:1006100038463146013948EA000C00F14040B1F15A -:10062000FF3FC3F874C2F5D1203A12F1200FEDD18B -:10063000BFF34F8FBFF36F8FBFF34F8FBFF36F8F3A -:100640005A6922F400325A610022C3F85022BFF3E3 -:100650004F8FBFF36F8F202383F31188AD4685F34F -:1006600008882047BDE8F081FCFF01081C00020853 -:1006700004000208FFFF0108482200206822002031 -:10068000004402580000020808ED00E000ED00E020 -:100690002DE9F04F99B0B34C2022FF21019010A812 -:1006A000A66800F031FCB04AA3461378A3B9012133 -:1006B000AE481170C360202383F31188C3680BB167 -:1006C00001F094FE0023AA4A4FF47A71A74801F082 -:1006D00051FE002383F31188019B13B1A54B019AAE -:1006E0001A600023A44AA349019F99461C461D464F -:1006F000984613704B600292012000F0C9FB002F56 -:1007000000F012829B4B1B68002B40F00D8219B049 -:10071000BDE8F08F0220FFF7B3FE002840F0FB8118 -:10072000019BB9F1000F08BF1F46944B1B880221A3 -:100730000BA8ADF82C3000F09FFADDE74FF47A708B -:1007400000F02EFA031E0393EADB0220FFF798FE67 -:1007500082460028E4D0039B581E042800F2DD8165 -:10076000DFE800F0030E1114170018A80523042178 -:1007700040F8343D00F080FA54464FF0000856E04F -:1007800004217848F6E704217D48F3E704217D48F9 -:10079000F0E71C242046043400F0B0FA04210B904A -:1007A0000BA800F069FA2C2CF4D1E5E7002DB7D0A6 -:1007B000002CB5D00220FFF763FE0546002800F0AC -:1007C000AF8101206C4C00F097FA4FF00009022035 -:1007D000207000F05DFB5FFA89FA504600F09CFA49 -:1007E000074658B1504609F1010900F0A5FA002862 -:1007F000F1D12C46A9460027634B97E701233E46DB -:100800000220237000F03AFBDBF808309E4206D24B -:10081000304600F073FA0130EBD10436F4E70026DD -:10082000029BA9462C461E703746524B5E6000F074 -:10083000BFFB15B1002C18BF0027FFF729FE5BE7AF -:10084000002D3FF46DAF002C3FF46AAF0220029BF5 -:10085000187000F01DFB322000F0A2F9B0F1000A80 -:10086000C0F25E811AF0030540F05A8106EB0A03DC -:10087000DBF80820934200F25381BAF5807F00F242 -:100880004F8155450DDA4FF47A7000F089F90490E4 -:10089000049B002BC0F24481049B3C4AAB540135BD -:1008A000EFE7C820FFF7ECFD0546002800F038818F -:1008B0001F2E11D8C6F1200410AB26F003003349D7 -:1008C0005445184428BF5446224600F0F5FA224603 -:1008D000FF212E4800F018FB4FEAAA0A2B493046A8 -:1008E0005FFA8AF200F018FB0446002800F01A8133 -:1008F00006EB8A0605469AE70220FFF7C1FD0028AD -:100900003FF40EAF00F032FA00283FF409AF4FF089 -:10091000000A5346DBF8082092451CD2BAF11F0F9B -:1009200012D8109A01320FD02AF0030218A90A44F3 -:1009300052F8202C0B92184604220BA90AF1040A43 -:1009400000F0DAFB0346E5E75046039300F0D6F9E2 -:10095000039B0B90EFE718A8042140F84C3D00F0F2 -:100960008BF964E7482200206C23002054220020E9 -:10097000AD04000870230020682200208240000897 -:100980004C22002050220020844000086C220020CD -:1009900018A80023642140F8343D00F039F90028FC -:1009A0007FF4BEAE0220FFF76BFD00283FF4B8AE27 -:1009B0000B9800F0D3F918AB43F8480D0421184602 -:1009C000CDE718A80023642140F8343D00F020F959 -:1009D00000287FF4A5AE0220FFF752FD00283FF467 -:1009E0009FAE0B9800F0BCF918AB43F8440DE5E757 -:1009F0000220FFF745FD00283FF492AE00F0C6F953 -:100A000018AB43F8400DD9E70220FFF739FD002865 -:100A10003FF486AE0BA9142000F0BEF9824618A858 -:100A2000042140F83CAD00F027F951460BA896E7A9 -:100A3000322000F0B5F8B0F1000AFFF671AE1AF0FE -:100A4000030F7FF46DAE0AEB0803DBF80820934236 -:100A50003FF666AE0220FFF713FD00283FF460AEBC -:100A60002AF0030AC244D0453FF4E1AE404608F103 -:100A7000040800F043F904210A900AA800F0FCF8E9 -:100A8000F1E74FF47A70FFF7FBFC00283FF448AE23 -:100A900000F06CF900283FF4AFAE109B01330CD08E -:100AA000082210A9002000F037FA00283FF4A4AE75 -:100AB0002022FF2110A800F027FAFFF7E9FC3748B1 -:100AC00001F014FC23E6002D3FF42AAE002C3FF485 -:100AD00027AE18A80023642140F8343D00F098F8B0 -:100AE000824600287FF41CAE0220FFF7C9FC0028D4 -:100AF0003FF416AE0390FFF7CBFC41F28830574627 -:100B000001F0F4FB0B9800F097FA00F051FA039B08 -:100B10001C461D46F0E5054689E64FF00008FFE556 -:100B20002546FDE52C4667E6002000F039F80490E4 -:100B3000049B002BFFF6E3AD012000F09FF9049B1E -:100B4000213B122B3FF6D8AD01A252F823F000BF93 -:100B5000150700083D070008AD070008F906000862 -:100B6000F9060008F906000841080008310A0008E3 -:100B7000F908000891090008C3090008F1090008F4 -:100B8000F9060008090A0008F9060008830A0008A7 -:100B90002F080008F9060008C70A0008A08601000F -:100BA0002DE9F3474FF47A75002402AE154F454303 -:100BB000DFF8588006F8014D97F900305FFA84F9A4 -:100BC0005A1C01D0A34212D158F824000122314608 -:100BD0000368D3F820A02B46D047012807D10A4B41 -:100BE0009DF8070083F8009002B0BDE8F08701345B -:100BF000022CE1D14FF4FA7001F078FB4FF0FF3096 -:100C0000F2E700BF00220020BC230020984000082B -:100C10002DE9F0474FF47A7506460024134F4D43F3 -:100C2000DFF8508097F900305FFA84F95A1C01D040 -:100C3000A34210D158F82400042231460368D3F8A7 -:100C400020A02B46D047042805D1094B002083F86B -:100C50000090BDE8F0870134022CE3D14FF4FA7024 -:100C600001F044FB4FF0FF30BDE8F0870022002088 -:100C7000BC23002098400008074B30B41A78074B7B -:100C800053F822400A46014623682046DD69044B9A -:100C9000AC4630BC604700BFBC2300209840000831 -:100CA000A0860100F8B5104C0025104E01F066FD3D -:100CB0000F4F20703068237883420CD800250D4EEA -:100CC0002078013801F056FD23780544013BB542F8 -:100CD0002370F5D9F8BD01F04DFD336805440133AB -:100CE000BD423360E6D9E9E7BD2300207823002028 -:100CF000FFFF0100FFFF030001F00CBE00F10060E8 -:100D000000F500300068704700F10060920000F5C7 -:100D1000003001F08FBD0000054B1A68054B1B78B1 -:100D20009B1A834202D9104401F024BD0020704771 -:100D300078230020BD23002038B5074D04462868DD -:100D4000204401F01DFD28B928682044BDE8384042 -:100D500001F028BD38BD00BF782300200020704777 -:100D6000014BC058704700BF00E8F11F064991F8D9 -:100D7000243033B100230822086A81F82430FFF7B9 -:100D8000C3BF0120704700BF7C230020014B1868BF -:100D9000704700BF0010005C234BF0B5234C1B686C -:100DA0002788C3F30B0665681B0C94F90820BE4224 -:100DB00028D0A789BE4206D101220C2505FB02449A -:100DC000656894F9082041F20104A3421CD041F265 -:100DD0000304A3421AD042F20104A34218D042F203 -:100DE0000304A34208BF5622441E013D0B460C4497 -:100DF000A34217D215F9016F581C5EB1034600F8E3 -:100E0000016CF5E70022D8E75A22EDE75922EBE71B -:100E10005822E9E72C2584421D7001D9981C5A708C -:100E2000401AF0BD1846FBE70010005C04220020C9 -:100E3000104B41F201015B888B429AB211D041F212 -:100E400003018B420FD042F201039A420DD042F2CD -:100E500003039A420BD10323074A02EB8303D8789A -:100E600070470023F8E70123F6E70223F4E70020A8 -:100E7000704700BF0010005C88400008022803D1C2 -:100E80004FF00052014B9A61704700BF00080258B2 -:100E9000022803D14FF40052014B9A61704700BF02 -:100EA00000080258022804D1024A536983F400530F -:100EB0005361704700080258002310B5934203D0D5 -:100EC000CC5CC4540133F9E710BD000030B5441EBA -:100ED00014F9010F0B4658B193F9005001318542C6 -:100EE00006D11AB993F90020801A30BD013AEFE714 -:100EF000002AF7D1104630BD02460346981A13F96E -:100F0000011B0029FAD1704702440346934202D0E4 -:100F100003F8011BFAE770472DE9F047234C05461B -:100F20008846174694F8243083BB2E46DFF87C9021 -:100F3000C7B394F824302BB92022FF2148462662FB -:100F4000FFF7E2FF94F824004146C0F1080504EBE6 -:100F50008000BD4228BF3D465FFA85FAAD00A7EB91 -:100F60000A072A462E44FFF7A7FF94F82430A84426 -:100F7000FFB29A445FFA8AFABAF1080F84F824A003 -:100F8000D6D1FFF7F3FE0028D2D108E0266A06EB9F -:100F90008306B042CAD0FFF7E9FE0028C5D1002081 -:100FA000BDE8F0870120BDE8F08700BF7C2300206A -:100FB000024B1A78024B1A70704700BFBC23002006 -:100FC0000022002038B5164C164D204600F02CFCAF -:100FD0002946204600F054FC2D681348D5F890208F -:100FE000D2F8043843F00203C2F8043801F07EF965 -:100FF0000E49284600F054FDD5F890200C48D2F850 -:1010000004380C49A04223F00203C2F804384FF41C -:10101000E1330B6003D0BDE8384000F063BB38BD5E -:10102000E82A0020A041000840420F00C841000803 -:1010300040240020A423002038B50B4B05461A7825 -:101040000A4B53F822400A4B9C420CD0094B00211A -:1010500018221846FFF758FF056001462046BDE8F4 -:10106000384000F03FBB38BDBC230020984000084A -:10107000E82A0020A4230020FEE7000000B59BB072 -:10108000EFF3098168226846FFF716FFEFF3058347 -:10109000034B9A6B9A6A9A6A9A6A9A6A9B6AFEE703 -:1010A00000ED00E000B59BB0EFF3098168226846CF -:1010B000FFF702FFEFF30583044B9A6B9A6A9A6A73 -:1010C0009A6A9A6A9A6A9B6AFEE700BF00ED00E09E -:1010D00000B59BB0EFF3098168226846FFF7ECFE8C -:1010E000EFF30583034B5A6B9A6A9A6A9A6A9A6A73 -:1010F0009B6AFEE700ED00E030B50A44074D9142DF -:101100000BD011F8013B09245840013CF7D040F3C3 -:1011100000032B4083EA5000F7E730BD2083B8ED91 -:10112000002304491A465A50C81808334260802BDD -:10113000F9D17047C0230020024A136843F0C0036E -:101140001360704700100140044B5A6810439A68BE -:1011500058600AB1181D1047704700BF4024002096 -:101160002DE9F84F414EF56D2F68EC6921072C628F -:1011700019D014F0080F0CBF00208020E20748BFF0 -:1011800040F02000A3074FF0200348BF40F040008C -:10119000610748BF40F4807083F31188FFF7D4FFE4 -:1011A000002383F31188E20509D5202383F31188F6 -:1011B0004FF40070FFF7C8FF002383F311884FF04E -:1011C0002009DFF8A8A04FF0000B14F0200832D15E -:1011D0003B0615D54FF02009DFF894A020060FD567 -:1011E00089F31188504600F0F1F9002830DA082020 -:1011F000FFF7AAFF27F080032B60002383F31188F9 -:1012000079060DD562060BD5202383F31188F26C85 -:10121000336D9A4201D1336C03BB002383F31188F1 -:10122000E30604D5B26E13690BB150699847BDE867 -:10123000F84F01F009BC89F31188AB8C504696F841 -:101240006410194000F05CFA8BF31188EC69BCE77C -:1012500080B2288588F31188EC69BFE727F0400742 -:101260001020FFF771FF2F60D7E700BF4024002058 -:101270007824002013B5104C204600F027FA04F122 -:101280001400009400234FF400720C4900F0E8F8B9 -:1012900004F1380000944FF40072094B094900F042 -:1012A00061F9094B0C212520E365084B236602B048 -:1012B000BDE8104000F05EB840240020AC240020BF -:1012C00039110008AC2600200010014000E1F505AE -:1012D000254B002908BF1946037C012B816630B5D8 -:1012E00011D1224B98420ED1214BD3F8F02042F07D -:1012F0001002C3F8F020D3F8182142F01002C3F80E -:101300001821D3F818310A68036EC46D03EB520339 -:10131000B3FBF2F34A68150442BF23F0070503F05C -:10132000070343EA4503E3608B6843F040036360CF -:10133000CB68510543F00103A36042F4967343F078 -:10134000010323604FF0FF33236205D512F0102212 -:1013500005D0B2F1805F04D080F8643030BD7F23C7 -:10136000FAE73F23F8E700BFBC4000084024002014 -:101370000044025800F1604300F01F0209014009D7 -:1013800003F56143C9B2800083F80013012300F123 -:101390006040934000F56140C0F8803103607047C1 -:1013A000F8B51546826804460B46AA4200D2856805 -:1013B000A1692669761AB54207D218462A46FFF770 -:1013C0007BFDA3692B44A3610DE011D9AF1B32460D -:1013D0001846FFF771FD3A46E1683044FFF76CFDAF -:1013E000E2683A44A261A36828465B1BA360F8BD8B -:1013F00018462A46FFF760FDE368E4E783689342F6 -:101400002DE9F04104460F46154600D28568606913 -:101410002669361AB54207D22A463946FFF74CFDEF -:1014200063692B4463610EE013D9A5EB06083246CD -:101430003946FFF741FD4246B919E068FFF73CFD28 -:10144000E26842446261A36828465B1BA360BDE872 -:10145000F0812A463946FFF72FFDE368E2E70000F6 -:1014600010B50A440024C361029B006040608460A0 -:10147000C160816141610261036210BD08B582698A -:101480004369934201D1826882B9826801328260E5 -:101490005A1C42611970426903699A4201D3C368B8 -:1014A0004361002100F03EFF002008BD4FF0FF30F7 -:1014B00008BD000070B5202304460E4683F3118852 -:1014C000A568A5B1A368A269013BA360531CA361F1 -:1014D00015782269934224BFE368A361E3690BB1E5 -:1014E00020469847002383F31188284670BD314673 -:1014F000204600F007FF0028E2DA85F3118870BD6E -:101500002DE9F74F05460F4690469A46D0F81C90B5 -:10151000202686F311884FF0000B144664B1224652 -:1015200039462846FFF73CFF034668B9514628462E -:1015300000F0E8FE0028F1D0002383F31188A8EB27 -:10154000040003B0BDE8F08FB9F1000F03D00190A3 -:101550002846C847019B8BF31188E41A1F4486F381 -:101560001188DBE7C160816141611144C361009B67 -:10157000006040608260016103627047F8B5044614 -:101580000E461746202383F31188A568A5B1A368EA -:10159000013BA36063695A1C62611E702369626922 -:1015A0009A4224BFE3686361E3690BB12046984720 -:1015B000002080F31188F8BD3946204600F0A2FED5 -:1015C0000028E2DA85F31188F8BD000083694269DA -:1015D0009A4210B501D182687AB98268013282607C -:1015E0005A1C82611C7803699A4201D3C3688361E3 -:1015F000002100F097FE204610BD4FF0FF3010BDD7 -:101600002DE9F74F05460F4690469A46D0F81C90B4 -:10161000202686F311884FF0000B144664B1224651 -:1016200039462846FFF7EAFE034668B95146284680 -:1016300000F068FE0028F1D0002383F31188A8EBA6 -:10164000040003B0BDE8F08FB9F1000F03D00190A2 -:101650002846C847019B8BF31188E41A1F4486F380 -:101660001188DBE7026843681143016003B1184742 -:10167000704700001430FFF743BF00004FF0FF3306 -:101680001430FFF73DBF00003830FFF7B9BF00004E -:101690004FF0FF333830FFF7B3BF00001430FFF7CF -:1016A00009BF00004FF0FF311430FFF703BF000007 -:1016B0003830FFF763BF00004FF0FF323830FFF7DC -:1016C0005DBF000000207047FFF7D4BD044B0360EE -:1016D000002343608360C36001230374704700BF2D -:1016E000D440000810B52023044683F31188FFF787 -:1016F000EFFD02232374002383F3118810BD000043 -:1017000038B5C36904460D461BB904210844FFF7E8 -:10171000A9FF294604F11400FFF7B0FE002806DAFD -:10172000201D4FF48061BDE83840FFF79BBF38BDF6 -:10173000026843681143016003B118477047000015 -:1017400013B5446BD4F894341A681178042915D170 -:10175000217C022912D11979012312898B4013426D -:101760000CD101A904F14C0002F02CF8D4F89444F7 -:101770000246019B2179206800F0EAF902B010BD11 -:10178000143001F0AFBF00004FF0FF33143001F010 -:10179000A9BF00004C3002F081B800004FF0FF33C9 -:1017A0004C3002F07BB80000143001F07DBF000027 -:1017B0004FF0FF31143001F077BF00004C3002F0E1 -:1017C0004DB800004FF0FF324C3002F047B8000037 -:1017D000D0F8942438B5136805461978042901D047 -:1017E000012038BD017C0229FAD1127901205C89DF -:1017F00090400440F4D105F1140001F00FFF0246BF -:101800000028EDD0D5F894544FF480732868697996 -:1018100000F08CF9204638BD406BFFF7D9BF0000BF -:1018200000207047704700007FB5124B012502264B -:10183000044603600023057400F1840243602946D6 -:101840008360C3600C4B0290143001934FF480739B -:10185000009601F0BFFE094B04F52372294601935F -:1018600004F14C004FF480730294009601F086FF5F -:1018700004B070BDFC4000081918000841170008AA -:101880000A68202383F311880B790B3342F8230075 -:101890004B79133342F823008B7913B10B3342F8A1 -:1018A00023000223C0F894140374002383F31188E7 -:1018B0007047000038B5037F044613B190F85430E8 -:1018C000ABB90125201D0221FFF732FF04F11400FE -:1018D00025776FF0010100F031FD84F8545004F1D8 -:1018E0004C006FF00101BDE8384000F027BD38BD65 -:1018F00010B5012104460430FFF71AFF00232377B7 -:1019000084F8543010BD000038B50446002514306A -:1019100001F078FE04F14C00257701F047FF201D0F -:1019200084F854500121FFF703FF2046BDE83840FA -:10193000FFF74EBF90F8803003F06003202B19D1E1 -:1019400090F88120212A0AD0222A4FF000030ED0DD -:10195000202A0FD1084A42670722826704E0064B1B -:101960004367072383670023C367012070474367EA -:101970008367F9E7002070471C220020D0F89434D8 -:1019800037B51A680446117804291AD1017C022956 -:1019900017D11979012312898B40134211D100F11B -:1019A0004C05284601F0C2FF58B101A9284601F0B4 -:1019B00009FFD4F894440246019B2179206800F085 -:1019C000C7F803B030BD000001F10B03F7B550F8C4 -:1019D000234005460E46F4B1202383F3118805EB1E -:1019E0008607201D08214C34FFF7A2FEFB685B69C7 -:1019F0001B6813B1204601F0F3FE01A9204601F057 -:101A0000E1FE024648B1019B3146284600F0A0F8AD -:101A1000002383F3118803B0F0BDFB685A69126894 -:101A2000002AF5D01B8A013B1340F1D105F1800259 -:101A3000EAE70000133138B550F82140DCB120232B -:101A400083F31188D4F894241368527903EB82034A -:101A5000DB689B695D6845B104216018FFF768FE8B -:101A6000294604F1140001F0E3FD2046FFF7B0FE23 -:101A7000002383F3118838BD7047000001F0DEB801 -:101A8000012300F1300200F1500103700023436094 -:101A900042F8043B8A42D361FAD103814381704703 -:101AA00038B50446202383F31188002500F10C0388 -:101AB00000F13002416043F8045B9342FBD12046C1 -:101AC00001F0DEF80223237085F3118838BD000091 -:101AD00070B500EB8103054650690E461446DA6086 -:101AE00018B110220021FFF70FFAA06918B11022D7 -:101AF0000021FFF709FA31462846BDE8704001F0A1 -:101B0000D1B90000038900F13002002103F0010384 -:101B10000381438903F00103438100F1100343F87B -:101B2000041B9342FBD101F055BA0000F0B401252B -:101B300000EB810447898D40E4683D43A4694581F9 -:101B400023600023A2606360F0BC01F071BA000062 -:101B5000F0B4012500EB810407898D40E4683D4322 -:101B60006469058123600023A2606360F0BC01F01A -:101B7000E9BA0000022300F10C0200F1300110B5B7 -:101B8000037004460023A0F8883080F88A3080F87B -:101B90008B300381438142F8043B8A42FBD184F8B5 -:101BA0007030204601F012F963681B6823B12046AB -:101BB0000021BDE81040184710BD000002784368BE -:101BC00080F88C2005221B6802700BB10421184795 -:101BD00070470000436890F88C201B6802700BB1BE -:101BE000052118477047000090F8703070B5044622 -:101BF00013B1002380F8703004F18002204601F018 -:101C00000DFA63689B6863BB94F8805015F060061A -:101C100015D194F8813005F07F0545EA032540F29F -:101C200002339D4200F00E815BD8022D00F0DC8073 -:101C30003FD8002D00F08780012D00F0CF800021DB -:101C4000204601F0A3FC0021204601F093FC6368CC -:101C50001B6813B1062120469847062384F870308C -:101C600070BD204698470028CED094F8872094F87D -:101C7000863043EA0223A26F934238BFA36794F9E8 -:101C80008030A56F002B4FF0200380F2FD80002DE7 -:101C900000F0EC80092284F8702083F31188002181 -:101CA000A36F626F2046FFF753FF002383F3118871 -:101CB00070BDB5F5817F00F0B180B5F5407F49D0AA -:101CC000B5F5807FBBD194F88230012BB7D1B4F841 -:101CD000883023F00203A4F888306667A667E667B9 -:101CE000C3E740F201639D421ED8B5F5C06F3BD2F9 -:101CF000B5F5A06FA3D1B4F88030B3F5A06F0ED1C5 -:101D000094F88230204684F88A3001F0BDF8636888 -:101D10001B6813B101212046984703232370002339 -:101D20006367A367E367A0E7B5F5106F32D040F6AD -:101D300002439D4252D0B5F5006F80D104F18B0370 -:101D40006367012324E004F18803E56763670223E6 -:101D5000A3678AE794F88230012B7FF470AFB4F860 -:101D6000883043F00203B6E794F885202046616886 -:101D700094F884304D6843EA022394F8831094F871 -:101D80008220A84700283FF45AAF4368636703687E -:101D9000A367A4E72378042B10D1202383F31188B1 -:101DA0002046FFF7AFFE86F311886368032184F8AD -:101DB0008B601B6821700BB12046984794F88230E5 -:101DC000002BACD084F88B300423237063681B682D -:101DD000002BA4D0022120469847A0E7374B636729 -:101DE0000223A36700239DE794F88410204611F096 -:101DF000800F01F00F010ED001F002F9012806D08A -:101E000002287FF41CAF2E4BA067636767E72D4B5A -:101E1000A567636763E701F0E5F8EFE794F88230C0 -:101E2000002B7FF40CAF94F8843013F00F013FF4D3 -:101E300076AF1A06204602D501F0C0FB6FE701F02D -:101E4000B1FB6CE794F88230002B7FF4F8AE94F885 -:101E5000843013F00F013FF462AF1B06204602D519 -:101E600001F094FB5BE701F085FB58E7142284F84E -:101E7000702083F311882B462A4629462046FFF717 -:101E800055FE85F3118870BD5DB1152284F8702070 -:101E900083F311880021A36F626F2046FFF746FE8F -:101EA00003E70B2284F8702083F311882B462A461F -:101EB00029462046FFF74CFEE3E700BF2C4100080F -:101EC000244100082841000838B590F870300446D5 -:101ED000152B29D8DFE803F03E28282828283E289B -:101EE000280B293928282828282828283E3E90F819 -:101EF000871090F88620836F42EA01229A4214D913 -:101F0000C268128AB3FBF2F502FB15356DB92023C6 -:101F100083F311882B462A462946FFF719FE85F3DD -:101F200011880A2384F8703038BD142384F8703087 -:101F3000202383F31188002320461A461946FFF711 -:101F4000F5FD002383F3118838BDC36F03B19847B3 -:101F50000023E7E7002101F019FB0021204601F0F2 -:101F600009FB63681B6813B10621204698470623C6 -:101F7000D8E7000090F87020152A38B5044622D81A -:101F80000123934040F6416213421DD113F48015A2 -:101F90000FD19B0217D50B2380F87030202383F3D9 -:101FA00011882B462A462946FFF7D2FD85F3118872 -:101FB00038BDC3689B695B682BB9C36F03B1984791 -:101FC000002384F8703038BD002101F0DFFA0021D1 -:101FD000204601F0CFFA63681B6813B10621204642 -:101FE00098470623EDE70000024B00221B605B6070 -:101FF0009A607047AC280020002382680374054B68 -:102000001B6899689142FBD25A6803604260106075 -:1020100058607047AC28002008B5202383F311884E -:10202000037C032B05D0042B0DD02BB983F311882F -:1020300008BD436900221A604FF0FF334361FFF788 -:10204000DBFF0023F2E790E80C001A60026853609F -:10205000F2E70000002382680374054B1B6899684F -:102060009142FBD85A680360426010605860704724 -:10207000AC280020054B1969087418680268536081 -:102080001A60186101230374FEF79EB9AC28002082 -:102090004B1C30B5054687B00A4C10D0236901A807 -:1020A000094A00F067F92846FFF7E4FF049B13B1E3 -:1020B00001A800F09BF92369586907B030BDFFF70C -:1020C000D9FFF8E7AC2800201920000838B50C4DDE -:1020D000044641612B6981689A68914203D8BDE842 -:1020E0003840FFF789BF1846FFF786FF01232C61B0 -:1020F000014623742046BDE83840FEF765B900BFAD -:10210000AC280020044B1A681B6990689B689842B1 -:1021100094BF002001207047AC28002010B5084C67 -:10212000236820691A6854602260012223611A74AE -:10213000FFF790FF01462069BDE81040FEF744B963 -:10214000AC280020FEE7000010B5194CFFF74CFF4B -:1021500000F002F980221749204600F087F8012399 -:1021600044F8180C0374144B144AD96821F4E06144 -:102170000904090C0A431249DA60CA6842F0807205 -:10218000CA60104A1049C2F8B01F116841F001013D -:1021900011601022DA77202283F82220002383F3B3 -:1021A000118862B60948BDE8104000F081B800BF50 -:1021B000D42800203041000800ED00E00003FA05BB -:1021C000F0ED00E0001000E055CEACC5384100084D -:1021D0002DE9F84F1E4C4FF00008DFF87890656944 -:1021E00004F11407D9F82430266AAA689E1B964287 -:1021F0001CD34FF0200AAA68236AD5F80CB0B61A8F -:10220000134423622B68BB425F606361C5F80C8096 -:1022100001D101F0B7FB88F311882869D8478AF308 -:1022200011886569AB689E42E5D2DBE76269BA4214 -:102230000CD0916823628E1B9660A86802282CBF80 -:102240001818981CBDE8F84F01F0A2BBBDE8F88F44 -:10225000AC280020000C0040034B59685A68521A01 -:102260009042FBD8704700BF001000E0826002225D -:1022700002740022427470478368A3F17C0243F821 -:102280000C2C026943F83C2C426943F8382C074A6D -:1022900043F81C2CC268A3F1180043F8102C02224A -:1022A00003F8082C002203F8072C7047E10300080C -:1022B00010B5202383F31188FFF7DEFF00210446C9 -:1022C000FFF704FF002383F31188204610BD0000B0 -:1022D000024B1B6958610F20FFF7CCBEAC280020D1 -:1022E000202383F31188FFF7F3BF000008B50146F0 -:1022F000202383F311880820FFF7CAFE002383F30D -:10230000118808BD49B1064B42681B6918605A60C4 -:10231000136043600420FFF7BBBE4FF0FF307047EF -:10232000AC2800200368984206D01A68026050600A -:1023300018465961FFF760BE7047000038B5044683 -:102340000D462068844200D138BD036823605C607C -:102350004561FFF751FEF4E7054B03F114025A61A2 -:102360009A614FF0FF32DA6100221A62704700BFB3 -:10237000AC280020F8B5036102291A4B0646C2605A -:1023800038BF02215C6A184B1A461F4652F8145F88 -:1023900095420AD1586198611C6205604560816070 -:1023A0000819BDE8F84001F0E3BA186AAB68241ACE -:1023B0000C1902D3E41A2D6804E09C4202D2204496 -:1023C00001F0E6FAAB689C42F4D86B683560736044 -:1023D0001E604FF0FF336E60B460A968091BA960EE -:1023E000FB61F8BD000C0040AC28002010B41A4C72 -:1023F000234653F8141F814210D0416802680A60D6 -:10240000026851609A424FF00001C16003D09368A6 -:1024100081680B4493605DF8044B70470A680020A4 -:102420009A4262615360C86003D15DF8044B01F0C9 -:10243000A9BA936888680344206A9360074A526A7D -:10244000121A9342E7D9991A5DF8044B012998BFF3 -:10245000931C184401F09CBAAC280020000C0040EA -:1024600000207047FEE70000704700004FF0FF308B -:1024700070470000022906D0032906D0012906482A -:1024800018BF0020704705487047032A9ABF0448C8 -:1024900000EBC2000020704724420008D841000829 -:1024A0002422002070B59AB006460846144601ADB5 -:1024B000294600F095F82846FEF71EFDC0B2431CE1 -:1024C0005B0086E8180023700323023404F8013C03 -:1024D00000231946DAB20234904201D81AB070BD16 -:1024E000EA5C013304F8011C04F8022CF2E7000056 -:1024F00008B5202383F311880348FFF73FFA002330 -:1025000083F3118808BD00BFE82A002010B50446F7 -:10251000052916D8DFE801F016150316161D20232D -:1025200083F311880E4A0121FFF7D2FA20460D4AA3 -:102530000221FFF7CDFA0C48FFF7E6F9002383F3F9 -:10254000118810BD202383F311880748FFF7B2F9E3 -:10255000F4E7202383F311880348FFF7C9F9EDE777 -:10256000584100087C410008E82A002038B50C4D8D -:102570000C4C2A460C4904F10800FFF793FF05F1C3 -:10258000CA0204F110000949FFF78CFF05F5CA7271 -:1025900004F118000649BDE83840FFF783BF00BFCB -:1025A000B02F002024220020A8410008B2410008DA -:1025B000BD41000870B5044608460D46FEF79CFC78 -:1025C000C6B22046013403780BB9184670BD3246B6 -:1025D0002946FEF77BFC0028F3D1012070BD0000E6 -:1025E0002DE9F84F05460C46FEF786FC2B49C6B28E -:1025F0002846FFF7DFFF2A492846FFF7DBFF08B12F -:102600001036F6B2632E0DD8DFF89080DFF8909088 -:10261000244FDFF898A0DFF898B02E7846B92670DE -:10262000BDE8F88F29462046BDE8F84F01F020BDEF -:10263000252E2ED1072241462846FEF747FC70B9C9 -:10264000DBF800300735073444F8073CBBF80430AA -:1026500024F8033C9BF8063004F8013CDDE708222F -:1026600049462846FEF732FC98B9A21C0E4B13F8D7 -:10267000011F023209095345C95D02F8041C19788B -:1026800001F00F01C95D02F8031CF0D118340835C0 -:10269000C3E7267001350134BFE700BF444200089C -:1026A000BD41000854420008FFE7F11F0BE8F11F8D -:1026B0004C420008BFF34F8F044B1A695107FCD1FD -:1026C000D3F810215207F8D1704700BF0020005204 -:1026D00008B50D4B1B78ABB9FFF7ECFF0B4BDA6875 -:1026E000D10704D50A4A5A6002F188325A60D3F8F9 -:1026F0000C21D20706D5064AC3F8042102F188321C -:10270000C3F8042108BD00BF0E3200200020005293 -:102710002301674508B5114B1B78F3B9104B1A69B3 -:10272000510703D5DA6842F04002DA60D3F810218D -:10273000520705D5D3F80C2142F04002C3F80C2112 -:10274000FFF7B8FF064BDA6842F00102DA60D3F80F -:102750000C2142F00102C3F80C2108BD0E3200200A -:10276000002000520F289ABF00F58060400400202E -:10277000704700004FF40030704700001020704791 -:102780000F2808B50BD8FFF7EDFF00F500330268FE -:10279000013204D104308342F9D1012008BD002068 -:1027A00008BD00000F2810B504463FD8FFF782FF90 -:1027B000FFF78EFF1E484FF0FF33072C4361C0F830 -:1027C00014311DD80361FFF775FF230243F0240382 -:1027D000C360C36843F08003C36003695A07FCD435 -:1027E000FFF768FF2046FFF7BDFF4FF4003100F010 -:1027F000F7F8FFF78FFF2046BDE81040FFF7C0BF96 -:10280000C0F81031FFF756FFA4F108031B0243F094 -:102810002403C0F80C31D0F80C3143F08003C0F829 -:102820000C31D0F810315B07FBD4D9E7002010BD84 -:10283000002000522DE9F84F40EA020305460E46FB -:102840001746DB0656D13446DFF8C090DFF8C0A04B -:10285000FFF73EFF3B1B33441F2B04D8FFF75AFF03 -:102860000120BDE8F88F20222146284601F0F0FB28 -:10287000002837D005F17843214A22489342224B61 -:1028800098BF984603F5827392BFD346C8469B46CD -:10289000A3F1080388BF1846FFF70CFF4FF0FF3382 -:1028A00004F11C01A5EB040ECBF80030036843F0E3 -:1028B00002030360231FD8F8002012F0050FFAD19D -:1028C00053F8042F99424EF80320F4D1BFF34F8FF1 -:1028D000FFF7F0FE4FF0FF33CBF80030036823F032 -:1028E0000203036020222146284601F0B1FB20B1FB -:1028F000FFF710FF0020BDE8F88F20352034A9E74E -:10290000FFFF0F000C200052102000521021005237 -:102910001420005210B5084C237828B153B9FFF7A2 -:10292000D7FE0123237010BD23B12070BDE81040F5 -:10293000FFF7F0BE10BD00BF0E3200200244043984 -:10294000064BD2B210B5904200D110BD441C00B26B -:1029500053F8200041F8040FE0B2F4E7504000586B -:102960000F4B30B51C6F240405D41C6F1C671C6F03 -:1029700044F400441C670B4C024404392368D2B26F -:1029800043F480732360084B904200D130BD441C57 -:1029900051F8045F00B243F82050E0B2F4E700BF02 -:1029A00000440258004802585040005807B5012220 -:1029B00001A90020FFF7C2FF019803B05DF804FBF6 -:1029C00013B50446FFF7F2FFA04206D002A9012288 -:1029D000002041F8044DFFF7C3FF02B010BD000016 -:1029E0000144BFF34F8F064B884204D3BFF34F8F90 -:1029F000BFF36F8F7047C3F85C022030F4E700BF6D -:102A000000ED00E0034B1B685B0142BF0122024B5B -:102A10001A707047D04402580F320020014B1878CA -:102A2000704700BF0F320020064A536823F00103AD -:102A30005360EFF30983683383F30988002383F33A -:102A40001188704730EF00E010B5202383F3118820 -:102A5000104B5B6813F4006318D0F1EE103AEFF3FB -:102A600009844FF0807344F84C3C0B4BDB6844F80E -:102A7000083CA4F1680383F30988FFF743FB18B10E -:102A8000064B44F8503C10BD054BFAE783F3118820 -:102A900010BD00BF00ED00E030EF00E0F1030008E2 -:102AA000F4030008F0B5BFF34F8FBFF36F8F1D4BDA -:102AB0000021C3F85012BFF34F8FBFF36F8F5A69D5 -:102AC00042F400325A61BFF34F8FBFF36F8FC3F8E8 -:102AD0008410BFF34F8FD3F8802043F6E076C2F323 -:102AE000C904C2F34E32A507520102EA060E284677 -:102AF00021464EEA0007013900F14040C3F86072F8 -:102B00004F1CF6D1203A12F1200FEED1BFF34F8FB8 -:102B10005A6942F480325A61BFF34F8FBFF36F8F0F -:102B2000F0BD00BF00ED00E0FEE70000084A094BE1 -:102B300009498B4204D3094A0021934205D37047C7 -:102B400052F8040F43F8040BF3E743F8041BF4E7CF -:102B500018440008603300206033002060330020F8 -:102B6000D0F89430002230B51146D0F890409D68DE -:102B70004FF0FF3004EB421301329542C3F80019C5 -:102B8000C3F81019C3F80809C3F8001BC3F8101BD9 -:102B9000C3F8080BEED24FF00113C4F81C3830BD57 -:102BA00000EB81032DE9F04FD3F80CE04F1C4FEA06 -:102BB0004118DEF814403F03D4F800C06568D0F82F -:102BC000902065450AD30120D2F8343800FA01F18B -:102BD00023EA0101C2F83418BDE8F08FACEB05031D -:102BE000BEF81060B34228BF334602EB0806D6F8A1 -:102BF0001869B6B2B3EB860F13D8A6683A44994663 -:102C0000A6F1040A5AF804BFB9F1040FC2F800B0E3 -:102C100002D9A9F10409F5E71E442B44A6606360BC -:102C2000CCE70020BDE8F08F890141F0200101616F -:102C300003699B06FCD41220FFF70EBB10B50A4CAB -:102C40002046FEF71DFF094BC4F89030084BC4F82E -:102C50009430084C2046FEF713FF074BC4F8903021 -:102C6000064BC4F8943010BD10320020000008401C -:102C70008C420008AC32002000000440984200085A -:102C80000378012B70B505465DD1494BD0F89040D3 -:102C9000984259D1474B0E216520D3F8D82042F0F5 -:102CA0000062C3F8D820D3F8002142F00062C3F8D4 -:102CB0000021D3F80021D3F8802042F00062C3F84D -:102CC0008020D3F8802022F00062C3F88020D3F85F -:102CD0008030FEF74FFB384BE360384BC4F80038C8 -:102CE0000023D5F89060C4F8003EC02323604FF461 -:102CF0000413A3633369002BFCDA01230C20336136 -:102D0000FFF7AAFA3369DB07FCD41220FFF7A4FA15 -:102D10003369002BFCDA00262846A660FFF720FF67 -:102D20006B68C4F81068DB68C4F81468C4F81C68E1 -:102D3000002B3AD1224BA3614FF0FF336361A368AC -:102D400043F00103A36070BD1E4B9842C8D1194BDC -:102D50000E214D20D3F8D82042F00072C3F8D820BD -:102D6000D3F8002142F00072C3F80021D3F800210B -:102D7000D3F8802042F00072C3F88020D3F880207E -:102D800022F00072C3F88020D3F88020D3F8D82036 -:102D900022F08062C3F8D820D3F8002122F08062AC -:102DA000C3F80021D3F8003193E7074BC3E700BF16 -:102DB000103200200044025840140040030020025A -:102DC000003C30C0AC320020083C30C0F8B5D0F830 -:102DD0009040054600214FF000662046FFF724FF93 -:102DE000D5F8941000234FF001128F684FF0FF3098 -:102DF000C4F83438C4F81C2804EB431201339F4252 -:102E0000C2F80069C2F8006BC2F80809C2F8080BE2 -:102E1000F2D20B68D5F89020C5F898306362102381 -:102E20001361166916F01006FBD11220FFF714FA91 -:102E3000D4F8003823F4FE63C4F80038A36943F4DF -:102E4000402343F01003A3610923C4F81038C4F8E9 -:102E500014380B4BEB604FF0C043C4F8103B094BE8 -:102E6000C4F8003BC4F81069C4F80039D5F89830AC -:102E700003F1100243F48013C5F89820A362F8BD53 -:102E80006842000840800010D0F8902090F88A1026 -:102E9000D2F8003823F4FE6343EA0113C2F8003885 -:102EA000704700002DE9F0410EB20D4600EB860898 -:102EB000D8F80C100F6807F00303022B53D0032B34 -:102EC00053D03F4A3F4F012B18BF1746D0F89040D0 -:102ED0004FEA451E002205F1100C04EB0E03C3F867 -:102EE000102B8A69002A42D04A8A05F158033A43D6 -:102EF0005B01E2500123D4F81C2803FA0CF31343BE -:102F0000C4F81C38A64400234A69CEF8103905F1EC -:102F10003F03002A3BD00A8A04EB8303898B920883 -:102F2000012988BF4A43D0F89810561841EA024256 -:102F30002946C0F8986020465A60FFF775FED8F819 -:102F40000C301B8A43EA85531F4305F148035B019C -:102F5000E7500123D4F81C2803FA05F51543C4F8FB -:102F60001C58BDE8F081184FB0E7184FAEE704EBEE -:102F70004613D3F8002B22F40042C3F8002B0123A0 -:102F8000D4F81C2803FA0CF322EA0303B8E704EB95 -:102F900083030F4A04EB461629465A602046FFF782 -:102FA00043FED6F80039012223F4004302FA05F566 -:102FB000C6F80039D4F81C3823EA0505CFE700BF6E -:102FC00000800010008004100080081000800C10A9 -:102FD00000040002D0F894201268C0F89820FFF78F -:102FE000BFBD00005831D0F8903049015B5813F450 -:102FF000004004D013F4001F14BF012002207047CA -:103000004831D0F8903049015B5813F4004004D0A7 -:1030100013F4001F14BF01200220704700EB810150 -:10302000CB68196A0B6813604B68536070470000E7 -:1030300000EB810330B5DD68AA691368D36019B964 -:10304000402B84BF402313606B8A1468D0F8902013 -:103050001C4402EB4110013C09B2B4FBF3F463439E -:10306000033323F0030343EAC44343F0C043C0F8EF -:10307000103B2B6803F00303012B0ED1D2F8083864 -:1030800002EB411013F4807FD0F8003B14BF43F0F3 -:10309000805343F00053C0F8003B02EB4112D2F8DA -:1030A000003B43F00443C2F8003B30BD2DE9F04142 -:1030B000D0F8906005460C4606EB4113D3F8087B28 -:1030C0003A07C3F8087B08D5D6F814381B0704D58F -:1030D00000EB8103DB685B689847FA072FD5D6F8C9 -:1030E0001438DB072BD505EB8403D968CCB98B6981 -:1030F000488A5E68B6FBF0F200FB12628AB9186873 -:10310000DA6890420DD2121A83E81400202383F368 -:10311000118821462846FFF78BFF84F31188BDE80C -:10312000F081012303FA04F26B8923EA02036B8125 -:10313000CB6823B121462846BDE8F0411847BDE8D9 -:10314000F081000000EB81034A0170B5DD68D0F822 -:1031500090306C692668E66056BB1A444FF4002034 -:10316000C2F810092A6802F00302012A0AB20ED13D -:10317000D3F8080803EB421410F4807FD4F8000958 -:1031800014BF40F0805040F00050C4F8000903EB39 -:103190004212D2F8000940F00440C2F800090122AE -:1031A000D3F8340802FA01F10143C3F8341870BDB2 -:1031B00019B9402E84BF4020206020681A442E8A0E -:1031C000841940F00050013CB4FBF6F440EAC440DE -:1031D000C6E700002DE9F041D0F8906004460D46A6 -:1031E00006EB4113D3F80879C3F80879FB071CD51F -:1031F000D6F81038DA0718D500EB8103D3F80CE0C5 -:10320000DEF81430D3F800C0DA6894451BD2A2EB84 -:103210000C024FF000081A60C3F80480202383F3E7 -:103220001188FFF78FFF88F311883B0618D501231B -:10323000D6F83428AB40134212D029462046BDE8C8 -:10324000F041FFF7ADBC012303FA01F2038923EA41 -:1032500002030381DEF80830002BE6D09847E4E74C -:10326000BDE8F0812DE9F047D0F8905004466E6932 -:10327000AB691E40F1046E6103D5BDE8F047FEF76F -:1032800079BC002E12DAD5F8003E9A0705D0D5F8A1 -:10329000003E23F00303C5F8003ED5F8043820466D -:1032A00023F00103C5F80438FEF794FC330502D57A -:1032B0002046FEF783FCB7040CD5D5F8083813F088 -:1032C000060FEB6823F470530CBF43F4105343F420 -:1032D000A053EB60300704D56368DB680BB1204670 -:1032E0009847F10200F1A180B2020BD5D4F890808A -:1032F00000274FF00109D4F89430F9B29B688B4253 -:1033000080F0D280F3061AD5D4F890100A6AC2F37E -:103310000A1702F00F0302F4F012B2F5802F00F04A -:10332000EB80B2F5402F0AD104EB830301F5805105 -:10333000DB68186A00231A469F4240F0D1803003B0 -:10334000D5F8185835D5E90303D500212046FFF7F5 -:10335000ADFEAA0303D501212046FFF7A7FE6B03AC -:1033600003D502212046FFF7A1FE2F0303D5032139 -:103370002046FFF79BFEE80203D504212046FFF715 -:1033800095FEA90203D505212046FFF78FFE6A02AC -:1033900003D506212046FFF789FE2B0203D507211E -:1033A0002046FFF783FEEF0103D508212046FFF7F3 -:1033B0007DFE700340F1C780E90703D50021204658 -:1033C000FFF708FFAA0703D501212046FFF702FFF8 -:1033D0006B0703D502212046FFF7FCFE2F0703D51C -:1033E00003212046FFF7F6FEEE0603D50421204612 -:1033F000FFF7F0FEA80603D505212046FFF7EAFEF9 -:10340000690603D506212046FFF7E4FE2A0603D508 -:1034100007212046FFF7DEFEEB0540F194802046B1 -:103420000821BDE8F047FFF7D5BED4F890904FF0E3 -:1034300000084FF0010AD4F894305FFA88F79B68CF -:10344000BB42FFF451AF09EB4713D3F8002902F454 -:103450004022B2F5802F24D1D3F80029002A20DAA7 -:10346000D3F8002942F09042C3F80029D3F800298C -:10347000002AFBDB3946D4F89000FFF7D5FB228900 -:103480000AFA07F322EA0303238104EB8703DB68CC -:103490009B6813B139462046984739462046FFF7C6 -:1034A0007FFB08F10108C6E708EB4112D2F8003BA8 -:1034B00003F44023B3F5802F10D1D2F8003B002B4A -:1034C0000CDA628909FA01F322EA0303638104EB4F -:1034D0008103DB68DB680BB12046984701370AE7B8 -:1034E00013F0030F00D10A68072B03F101039EBFFD -:1034F0000270120A01301FE704EB830301F58051CB -:10350000DA6890690268D0F808C04068A2EB000E43 -:1035100000221046974208D1DB689B699A683A44BA -:103520009A605A6817445F6009E712F0030F00D1F0 -:103530000868964502F1010282BF8CF80000000A7B -:103540000CF1010CE6E7BDE8F087000008B5034880 -:10355000FFF788FEBDE80840FFF776BA103200207A -:1035600008B50348FFF77EFEBDE80840FFF76CBAD8 -:10357000AC320020D0F8903003EB4111D1F8003B81 -:1035800043F40013C1F8003B70470000D0F89030BE -:1035900003EB4111D1F8003943F40013C1F80039AD -:1035A00070470000D0F8903003EB4111D1F8003B98 -:1035B00023F40013C1F8003B70470000D0F89030AE -:1035C00003EB4111D1F8003923F40013C1F800399D -:1035D0007047000030B50433039C0172002104FBE6 -:1035E0000325C361049B00600363059B4060C160C9 -:1035F000426102618561046242628162C162436329 -:1036000030BD00000022416AC260416101616FF07B -:1036100001018262C262FEF791BE00000369426945 -:10362000934203D1C2680AB100207047181D704749 -:10363000036919600021C2680132C260C269134483 -:1036400082699342036124BF436A0361FEF76ABE45 -:1036500038B504460D46E3683BB162690020131D8E -:103660001268A3621344E36238BD237A33B9294652 -:103670002046FEF747FE0028EDDA38BD6FF0010066 -:10368000FBE70000C368C269013BC36043691344A0 -:1036900082699342436124BF436A4361002383628A -:1036A000036B03B11847704770B52023044683F3BA -:1036B0001188866A3EB9FFF7CBFF054618B186F33D -:1036C0001188284670BDA36AE26A13F8015B934231 -:1036D000A36202D32046FFF7D5FF002383F31188AE -:1036E000EFE700002DE9F84F04460E469046994654 -:1036F000202787F311880025AA46D4F828B0BBF10B -:10370000000F09D149462046FFF7A2FF20B18BF3F5 -:1037100011882846BDE8F88FA16AA8EB050BE36A7B -:103720005B1A9B4528BF9B46BBF1400F1BD9334614 -:1037300001F1400251F8040B914243F8040BF9D116 -:10374000A36A403640354033A362A26AE36A9A42D4 -:1037500002D32046FFF796FF8AF311884545D8D259 -:1037600087F31188C9E730465A46FDF7A5FBA36ADF -:103770005E445D445B44A362E7E7000010B5029C31 -:1037800004330172C36103FB0421002300608362E0 -:10379000C362039B40600363049BC4604261026197 -:1037A000816104624262436310BD0000026A6FF0EF -:1037B0000101C260426A4261026100228262C26209 -:1037C000FEF7BCBD436902699A4203D1C2680AB1DF -:1037D00000207047184650F8043B0B60704700000B -:1037E000C3680021C2690133C3604369134482691D -:1037F0009342436124BF436A4361FEF793BD0000D7 -:1038000038B504460D46E3683BB1236900201A1D14 -:10381000A262E2691344E36238BD237A33B92946D0 -:103820002046FEF76FFD0028EDDA38BD6FF001008D -:10383000FBE7000003691960C268013AC260C2690F -:10384000134482699342036124BF436A03610023E6 -:103850008362036B03B118477047000070B52023E3 -:1038600004460E4683F31188856A35B91146FFF781 -:10387000C7FF10B185F3118870BDA36A1E70A36ADB -:10388000E26A01339342A36204D3E169204604391A -:10389000FFF7D0FF002080F3118870BD2DE9F84FAD -:1038A00004460D4691469A464FF0200888F3118849 -:1038B0000026B346A76A4FB951462046FFF7A0FF3E -:1038C00020B187F311883046BDE8F88FA06AA9EBD4 -:1038D0000603E76A3F1A9F4228BF1F46402F1BD9A5 -:1038E00005F1400355F8042B9D4240F8042BF9D113 -:1038F000A36A40364033A362A26AE36A9A4204D3C1 -:10390000E16920460439FFF795FF8BF311884E4596 -:10391000D9D288F31188CDE729463A46FDF7CCFA8B -:10392000A36A3D443E443B44A362E5E70269436920 -:103930009A4203D1C3681BB9184670470023FBE7BE -:10394000836A002BF8D0043B9B1AF5D01360C36840 -:10395000013BC360C3691A4483699A42026124BF70 -:10396000436A0361002383620123E5E700F0BCB8EA -:10397000034B002258631A610222DA60704700BFCD -:10398000000C00400022014BDA607047000C004040 -:10399000014B5863704700BF000C00404B68436008 -:1039A0008B688360CB68C3600B6943614B690362BA -:1039B0008B6943620B6803607047000008B52C4BAD -:1039C00040F2FF712B48D3F888200A43C3F88820BF -:1039D000D3F8882022F4FF6222F00702C3F888207F -:1039E000D3F88820D3F8E0200A43C3F8E020D3F8C6 -:1039F00008210A43C3F808211F4AD3F808311146A9 -:103A0000FFF7CCFF1D4802F11C01FFF7C7FF1C4860 -:103A100002F13801FFF7C2FF1A4802F15401FFF723 -:103A2000BDFF194802F17001FFF7B8FF174802F116 -:103A30008C01FFF7B3FF164802F1A801FFF7AEFFB4 -:103A4000144802F1C401FFF7A9FF134802F1E00195 -:103A5000FFF7A4FF114802F1FC01FFF79FFF104898 -:103A600002F58C71FFF79AFFBDE8084000F016B927 -:103A70000044025800000258A44200080004025802 -:103A800000080258000C0258001002580014025896 -:103A900000180258001C0258002002580024025846 -:103AA0000028025808B500F0C1FAFEF74DFBFEF7FA -:103AB000A9FFBDE80840FEF759BD000070470000AF -:103AC000084B1A69920710B508D500241C61202301 -:103AD00083F31188FEF77CFB84F31188BDE8104066 -:103AE000FEF7B2BF000C0040124B08213220D3F881 -:103AF000E82042F00802C3F8E820D3F8102142F091 -:103B00000802C3F810210C4AD3F81031D36B43F0EC -:103B10000803D363C722094B9A624FF0FF32DA627F -:103B200000229A615A63DA605A6001225A611A606F -:103B3000FDF720BC004402580010005C000C00405F -:103B4000F8B5514B0024D3F880204FF0FF32C3F872 -:103B50008020D3F88010C3F88040D3F88010D3F8C9 -:103B60008410C3F88420D3F88410C3F88440D3F8B9 -:103B70008410D96F41F0FF4141F4FF0141F4DF416E -:103B800041F07F01D967D96F21F0FF4121F4FF0196 -:103B900021F4DF4121F07F01D967D96FD3F8881074 -:103BA0006FEA41516FEA5151C3F88810D3F8881079 -:103BB000C1F30A01C3F88810D3F88810D3F8901025 -:103BC000C3F89020D3F89010C3F89040D3F8901029 -:103BD000D3F89410C3F89420D3F89410C3F8944009 -:103BE000D3F89410D3F89810C3F89820D3F898100D -:103BF000C3F89840D3F89810D3F88C10C3F88C20F1 -:103C0000D3F88C10C3F88C40D3F88C10D3F89C10E8 -:103C1000C3F89C20D3F89C20C3F89C40D3F89C3078 -:103C200000F0E4F9194B07229A60194ADA60194A40 -:103C30001A6105225A60184A536A43F4803353626A -:103C4000C2F88440BFF34F8FD2F8803043F6E0765D -:103C5000C3F3C904C3F34E33A5075B0103EA060EA1 -:103C6000284621464EEA0007013900F14040C2F8DB -:103C700074724F1CF6D1203B13F1200FEED1BFF32D -:103C80004F8FBFF36F8FF8BD0044025890ED00E0F6 -:103C9000000004301B00080300ED00E00122574B38 -:103CA00057491A60574B19609A60574ADA600022E8 -:103CB0001A614FF440429A619A699004FCD51A68DF -:103CC00042F480721A60514B1A6F12F4407F04D094 -:103CD0004FF480321A6700221A671A6842F0010214 -:103CE0001A604A4A134611684907FCD50021116140 -:103CF0001A6912F03802FBD1012119604FF080419E -:103D000059605A67424ADA62424A1A611A6842F4B2 -:103D100080321A603D4B1A689203FCD53E4A3F49F7 -:103D20009A6200225A631963A1F58021DA634B3944 -:103D300099635A643A4A1A643A4ADA621A6842F053 -:103D4000A8521A60314B1A6802F02852B2F1285F6B -:103D5000F9D1482222219A614FF48862DA61402227 -:103D60001A624FF00052DA641A652F4A5A652F4AD8 -:103D70009A652F4A11601A6942F003021A61234BB7 -:103D80001A6902F03802182AFAD1D3F8DC2042F07E -:103D90000052C3F8DC20D3F8042142F00052C3F8EB -:103DA0000421D3F80421D3F8DC2042F08042C3F888 -:103DB000DC20D3F8042142F08042C3F80421D3F878 -:103DC0000421D3F8DC2042F00042C3F8DC20D3F811 -:103DD000042142F00042C3F80421D3F80421D3F8AF -:103DE000F42042F00202C3F8F420D3F81C2142F080 -:103DF0000202C3F81C21D3F81C317047088100511E -:103E000000C000F0004802580200000100440258BF -:103E10000000FF01008890081210200063020701D3 -:103E200047040508FD0BFF010010E0000001010040 -:103E300000200052084A08B5936811680B4003F04F -:103E40000103936023B1054A13680BB1506898478A -:103E5000BDE80840FEF7F8BD80000058C0230020F0 -:103E6000084A08B5936811680B4003F00203936099 -:103E700023B1054A93680BB1D0689847BDE8084064 -:103E8000FEF7E2BD80000058C0230020084A08B5B4 -:103E9000936811680B4003F00403936023B1054A53 -:103EA00013690BB150699847BDE80840FEF7CCBDD7 -:103EB00080000058C0230020084A08B593681168A4 -:103EC0000B4003F00803936023B1054A93690BB1DB -:103ED000D0699847BDE80840FEF7B6BD800000589D -:103EE000C0230020084A08B5936811680B4003F00E -:103EF0001003936023B1054A136A0BB1506A9847C7 -:103F0000BDE80840FEF7A0BD80000058C023002097 -:103F1000174B10B59C681A68144004F478729A60C4 -:103F2000A30604D5134A936A0BB1D06A984760067A -:103F300004D5104A136B0BB1506B9847210604D57A -:103F40000C4A936B0BB1D06B9847E20504D5094A34 -:103F5000136C0BB1506C9847A30504D5054A936CBC -:103F60000BB1D06C9847BDE81040FEF76DBD00BFA7 -:103F700080000058C02300201A4B10B59C681A68B6 -:103F8000144004F47C429A60620504D5164A136D0D -:103F90000BB1506D9847230504D5134A936D0BB1AF -:103FA000D06D9847E00404D50F4A136E0BB1506EE4 -:103FB0009847A10404D50C4A936E0BB1D06E984774 -:103FC000620404D5084A136F0BB1506F984723045D -:103FD00004D5054A936F0BB1D06F9847BDE81040E8 -:103FE000FEF732BD80000058C0230020062108B52E -:103FF0000846FDF7BFF906210720FDF7BBF90621AA -:104000000820FDF7B7F906210920FDF7B3F90621CD -:104010000A20FDF7AFF906211720FDF7ABF90621BD -:104020002820BDE80840FDF7A5B9000008B5FFF756 -:1040300087FDFDF775F8FDF747FBFDF71FFDFDF761 -:10404000F1FBFFF73BFDBDE80840FFF78FBC000028 -:1040500010B501390244904201D1002010BD10F882 -:10406000013B11F8014FA342F5D0181B10BD000011 -:10407000034611F8012B03F8012B002AF9D17047F0 -:10408000121012131211000001105A0003105900EF -:104090000120580003205600E82A00204024002078 -:1040A00053544D333248373F3F3F0053544D333222 -:1040B000483734332F3735330000000000960000B6 -:1040C00000000000000000000000000000000000F0 -:1040D0000000000000000000911600087D16000896 -:1040E000B9160008A5160008B11600089D160008AC -:1040F0008916000875160008C516000800000000A3 -:104100009D17000889170008C5170008B117000897 -:10411000BD170008A91700089517000881170008A7 -:10412000211800080000000001000000000000004D -:104130006D61696E0000000050410008F028002009 -:10414000E82A0020010000004521000800000000CE -:1041500069646C65000000000200000000000000BF -:10416000C9190008351A000840004000802F0020BF -:10417000902F00200200000000000000030000005B -:1041800000000000791A0008000000001000000084 -:10419000A02F00200000000001000000000000002F -:1041A00010320020010102004172647550696C6F89 -:1041B000740025424F415244252D424C0025534561 -:1041C0005249414C250000000D25000875240008C7 -:1041D00035190008F124000843000000E041000800 -:1041E00009024300020100C032090400000102027A -:1041F000010005240010010524010001042402022D -:104200000524060001070582030800FF09040100D8 -:10421000020A0000000705010240000007058102B4 -:1042200040000000120000002C42000812011001A2 -:104230000200004009124157000201020301000080 -:104240000403090425424F4152442500426561732D -:1042500074483700303132333435363738394142DB -:10426000434445460000000000000000E91B000830 -:10427000C91E0008751F0008400040004833002098 -:104280004833002001000000583300208000000067 -:1042900040010000080000000001000000040000D0 -:1042A000080000000001A86A00000000AAAAAAAA4B -:1042B00000011440FFFF00000000000070A70A008A -:1042C0000000000000000000AAAAAAAA0000000046 -:1042D000FFFF0000000000000000000000000004DC -:1042E00000000000AAAAAAAA00000000FFDF000048 -:1042F00000000000000000000000000000000000BE -:10430000AAAAAAAA00000000FFFF00000000000007 -:10431000000000000001000000000000AAAAAAAAF4 -:1043200000010000FFFF000000000000000000008E -:104330000000000000000000AAAAAAAA00000000D5 -:10434000FFFF00000000000000000000000000006F -:1043500000000000AAAAAAAA00000000FFFF0000B7 -:10436000000000000000000000000000000000004D -:10437000AAAAAAAA00000000FFFF00000000000097 -:10438000000000000000000000000000AAAAAAAA85 -:1043900000000000FFFF000000000000000000001F -:1043A0000000000000000000AAAAAAAA0000000065 -:1043B000FFFF0000000000000000000000000000FF -:1043C00000000000AAAAAAAA00000000FFFF000047 -:1043D0000000000000000000FF00000000000000DE -:1043E000A04000083F00000050040000AB4000085F -:1043F0003F000000009600000000080004000000DC -:104400004042000800000000000000000000000022 -:0C441000000000000000000000000000A0 +:100310006F8F02F091FB02F033FB03F0CDFA4FF048 +:1003200055301F491B4A91423CBF41F8040BFAE784 +:100330001C49194A91423CBF41F8040BFAE71A499B +:100340001A4A1B4B9A423EBF51F8040B42F8040B69 +:10035000F8E700201749184A91423CBF41F8040BC6 +:10036000FAE702F04BFB03F01FFB144C144DAC42B8 +:1003700003DA54F8041B8847F9E700F041F8114C00 +:10038000114DAC4203DA54F8041B8847F9E702F038 +:1003900033BB0000000600200022002000000008FF +:1003A000000000200006002010430008002200206A +:1003B0005C22002060220020FC460020A0020008F1 +:1003C000A0020008A0020008A00200082DE9F04FDA +:1003D0002DED108AC1F80CD0D0F80CD0BDEC108AED +:1003E000BDE8F08F002383F311882846A047002042 +:1003F00001F056FEFEE701F0EBFD00DFFEE7000036 +:1004000038B500F0DDFC30B1164B00220E211A7217 +:100410005A729972DA7202F011FA054602F044FA41 +:100420000446D0B9104B9D4219D001339D4241F290 +:10043000883512BF044600250124002002F008FA86 +:100440000CB100F059F800F057FD00F0FFFB284612 +:1004500000F004F900F050F8F9E70025EDE7054653 +:10046000EBE700BF00220020010007B008B500F054 +:10047000B9FBA0F120035842584108BD07B541F22D +:100480001203022101A8ADF8043000F0C9FB03B04B +:100490005DF804FB202310B583F311881248C3686C +:1004A0000BB101F083FE0023104A4FF47A710E481D +:1004B00001F040FE002383F311880D4C236813B133 +:1004C0002368013B2360636813B16368013B636089 +:1004D000084B1B7833B9636823B9022000F086FC0F +:1004E0003223636010BD00BF602200209504000825 +:1004F0007C23002074220020F8B5514B514A1C4641 +:100500001968013100F09B8004339342F8D162688E +:100510004D4B9A4240F293804C4B9B6803F1006331 +:1005200003F500339A4280F08A80002000F0A8FB97 +:100530000220474B187000F04FFC464B0021D3F8C7 +:10054000E820C3F8E810D3F81021C3F81011D3F84D +:100550001021D3F8EC20C3F8EC10D3F81421C3F821 +:100560001411D3F81421D3F8F020C3F8F010D3F805 +:100570001821C3F81811D3F81821D3F8802042F0BD +:100580000062C3F88020D3F8802022F00062C3F814 +:100590008020D3F88020D3F8802042F00072C3F886 +:1005A0008020D3F8802022F00072C3F88020D3F896 +:1005B000803072B64FF0E023C3F8084DD4E9000450 +:1005C000BFF34F8FBFF36F8F234AC2F88410BFF37E +:1005D0004F8F536923F480335361BFF34F8FD2F8A9 +:1005E000803043F6E076C3F3C905C3F34E335B01B5 +:1005F00003EA060C29464CEA81770139C2F8747285 +:10060000F9D2203B13F1200FF2D1BFF34F8FBFF38C +:100610006F8FBFF34F8FBFF36F8F536923F4003396 +:1006200053610023C2F85032BFF34F8FBFF36F8F77 +:10063000202383F31188854680F308882047F8BD7E +:100640000000020820000208FFFF0108002200202D +:10065000742200200044025800ED00E02DE9F04F24 +:1006600093B0A94B2022FF2100900AA89D6800F0BA +:10067000EDFBA64A1378A3B90121A5481170C36008 +:10068000202383F31188C3680BB101F08FFD002391 +:10069000A04A4FF47A719E4801F04CFD002383F389 +:1006A0001188009B9C4A03B1136000239B49009C66 +:1006B00098469B461E469A460B705360012000F0F8 +:1006C0008BFB24B1944B1B68002B00F0168200209A +:1006D00000F088FA0390039B002BF2DB012000F06E +:1006E00071FB039B213B162BE8D801A252F823F0A3 +:1006F0004D0700087507000809080008BD06000836 +:10070000BD060008BD0600089D0800086F0A000825 +:1007100089090008EB090008130A0008390A0008D3 +:10072000BD0600084B0A0008BD060008BD0A000807 +:10073000ED070008BD060008010B00085907000876 +:10074000ED070008BD060008EB0900080220FFF7CE +:100750008DFE002840F0FB81009B022105A8B8F126 +:10076000000F08BF1C4641F21233ADF8143000F000 +:1007700057FAA3E74FF47A7000F034FA071EEBDB68 +:100780000220FFF773FE0028E6D0013F052F00F29C +:10079000E081DFE807F0030A0D101336052304217A +:1007A00005A8059300F03CFA17E004215648F9E744 +:1007B00004215B48F6E704215A48F3E74FF01C098F +:1007C000484609F1040900F05DFA0421059005A8E6 +:1007D00000F026FAB9F12C0FF2D101204FF0000AF7 +:1007E00000FA07F747EA0B0B5FFA8BFB00F07AFB86 +:1007F00026B10BF00B030B2B08BF0024FFF73EFEC6 +:100800005CE704214848CDE7002EA5D00BF00B0390 +:100810000B2BA1D10220FFF729FE074600289BD011 +:1008200001203E4E00F02CFA4FF0000802203070FC +:1008300000F0D2FA5FFA88F9484600F031FA04462F +:1008400090B1484608F1010800F03AFA0028F1D1C9 +:10085000B846044641F21213022105A83E46ADF8FF +:10086000143000F0DDF929E701232546022033701A +:1008700000F0A8FA244B9B68AB4207D9284600F049 +:1008800001FA013040F068810435F3E70025234B7D +:10089000B8463E461D70204B5D60A7E7002E3FF432 +:1008A0005BAF0BF00B030B2B7FF456AF02201B4BFF +:1008B000187000F091FA322000F094F9B0F10009BC +:1008C000FFF64AAF19F003077FF446AF0E4A09EB73 +:1008D0000503926893423FF63FAFB9F5807F3FF73B +:1008E0003BAF124BB945019322DD4FF47A7000F013 +:1008F00079F90390039A002AFFF62EAF039A013785 +:10090000019B03F8012BEDE7002200207823002053 +:1009100060220020950400087C230020742200201F +:1009200004220020082200200C220020782200202F +:10093000C820FFF79BFD074600283FF40DAF1F2D91 +:1009400011D8C5F120020AAB25F0030084494A45BD +:10095000184428BF4A46019200F052FA019AFF213A +:100960007F4800F073FA4FEAA903C9F387027C4974 +:100970002846019300F072FA064600283FF46AAF59 +:10098000019B05EB830531E70220FFF76FFD00288F +:100990003FF4E2AE00F0B0F900283FF4DDAE0027EE +:1009A000B946704B9B68BB4218D91F2F11D80A9BC0 +:1009B00001330ED027F0030312AA134453F8203C4E +:1009C00005934846042205A9043700F031FB81460F +:1009D000E7E7384600F056F90590F2E7CDF81490B5 +:1009E000042105A800F01CF900E70023642104A8F5 +:1009F000049300F00BF900287FF4AEAE0220FFF75D +:100A000035FD00283FF4A8AE049800F06BF905907E +:100A1000E6E70023642104A8049300F0F7F8002817 +:100A20007FF49AAE0220FFF721FD00283FF494AE38 +:100A3000049800F059F9EAE70220FFF717FD0028B3 +:100A40003FF48AAE00F068F9E1E70220FFF70EFDFF +:100A500000283FF481AE05A9142000F063F9074691 +:100A60000421049004A800F0DBF83946B9E73220ED +:100A700000F0B8F8071EFFF66FAEBB077FF46CAE50 +:100A8000384A07EB0A03926893423FF665AE0220AC +:100A9000FFF7ECFC00283FF45FAE27F00307574454 +:100AA000BA453FF4A3AE50460AF1040A00F0EAF852 +:100AB0000421059005A800F0B3F8F1E74FF47A702F +:100AC000FFF7D4FC00283FF447AE00F015F90028EA +:100AD00044D00A9B01330BD008220AA9002000F061 +:100AE000BDF900283AD02022FF210AA800F0AEF973 +:100AF000FFF7C4FC1C4801F0D9FA13B0BDE8F08F31 +:100B0000002E3FF429AE0BF00B030B2B7FF424AE29 +:100B10000023642105A8059300F078F80746002813 +:100B20007FF41AAE0220FFF7A1FC814600283FF4B3 +:100B300013AEFFF7A3FC41F2883001F0B7FA059835 +:100B400000F014FA4E463C4600F0CCF9B6E50646F5 +:100B50004CE64FF0000AFFE5B8467BE6374679E6FB +:100B60007822002000220020A0860100F7B50C4664 +:100B7000184E4FF47A71054602FB01F396F90020F6 +:100B8000501C0BD11448294601930268176A22466B +:100B9000B8478442019B03D1002310E0002AF1D022 +:100BA00096F90020511C01D0012A0DD10B4829468D +:100BB0000268166A2246B047844205D10123084ADA +:100BC0000120137003B0F0BD4FF4FA7001F06EFA1B +:100BD0000020F7E710220020F0290020CC2300207D +:100BE000C8230020002307B5024601210DF10700AC +:100BF0008DF80730FFF7BAFF20B19DF8070003B06A +:100C00005DF804FB4FF0FF30F9E700000A460421CD +:100C100008B5FFF7ABFF80F00100C0B2404208BD4D +:100C2000074B0A4630B41978064B53F82140014669 +:100C300023682046DD69044BAC4630BC604700BFEA +:100C4000C8230020E83F0008A086010070B50A4EC6 +:100C500000240A4D01F0D8FC308028683388834294 +:100C600008D901F0CDFC2B6804440133B4F5003FF2 +:100C70002B60F2D370BD00BFCA2300208423002064 +:100C800001F092BD00F1006000F50030006870478F +:100C900000F10060920000F5003001F011BD00008D +:100CA000054B1A68054B1B889B1A834202D91044D6 +:100CB00001F0A6BC0020704784230020CA23002036 +:100CC00038B5074D04462868204401F09FFC28B938 +:100CD00028682044BDE8384001F0AABC38BD00BFF8 +:100CE000842300200020704700F1FF5000F58F1092 +:100CF000D0F8000870470000064991F8243033B15D +:100D000000230822086A81F82430FFF7C1BF0120C0 +:100D1000704700BF88230020014B1868704700BF50 +:100D20000010005C244BF0B51A680446234BC2F354 +:100D30000B06120C1F885868BE4293F9085028D041 +:100D40009F89BE4206D101200C2505FB003358685F +:100D500093F9085041F201039A421CD041F2030377 +:100D60009A421AD042F201039A4218D042F2030387 +:100D70009A4208BF5625621E0B46441E0A449342FF +:100D80000FD214F9016F581C6EB1034600F8016CC4 +:100D9000F5E70020D8E75A25EDE75925EBE7582578 +:100DA000E9E7184605E02C2482421C7001D9981C02 +:100DB0005D70401AF0BD00BF0010005C14220020DE +:100DC00000207047022803D1024B4FF000529A6175 +:100DD000704700BF00080258022803D1024B4FF4AD +:100DE00000529A61704700BF00080258022804D1DF +:100DF000024A536983F40053536170470008025854 +:100E0000002310B5934203D0CC5CC4540133F9E7FE +:100E100010BD0000013810B510F9013F3BB191F948 +:100E200000409C4203D11AB10131013AF4E71AB1F2 +:100E300091F90020981A10BD1046FCE703460246BF +:100E4000D01A12F9011B0029FAD170470244034657 +:100E5000934202D003F8011BFAE770472DE9F843EB +:100E60001F4D14460746884695F8242052BBDFF8EC +:100E700070909CB395F824302BB92022FF2148466E +:100E80002F62FFF7E3FF95F824004146C0F1080206 +:100E900005EB8000A24228BF2246D6B29200FFF79F +:100EA000AFFF95F82430A41B17441E449044E4B2CD +:100EB000F6B2082E85F82460DBD1FFF71DFF00286D +:100EC000D7D108E02B6A03EB82038342CFD0FFF730 +:100ED00013FF0028CBD10020BDE8F8830120FBE7F9 +:100EE00088230020024B1A78024B1A70704700BF0B +:100EF000C82300201022002038B5164C164D20467D +:100F000000F0FAFB2946204600F022FC2D68134829 +:100F1000D5F89020D2F8043843F00203C2F8043820 +:100F200001F0C4F80E49284600F020FDD5F89020C5 +:100F30000C48D2F804380C49A04223F00203C2F84E +:100F400004384FF4E1330B6003D0BDE8384000F0C3 +:100F500031BB38BDF0290020F440000840420F00AA +:100F6000FC400008CC230020B023002038B50B4BF8 +:100F700004461A780A4B53F822500A4B9D420CD073 +:100F8000094B002118221846FFF760FF0460014654 +:100F90002846BDE8384000F00DBB38BDC82300200E +:100FA000E83F0008F0290020B023002000B59BB0E6 +:100FB000EFF3098168226846FFF722FFEFF305830C +:100FC000044B9A6BDA6A9A6A9A6A9A6A9A6A9A6A75 +:100FD0009B6AFEE700ED00E000B59BB0EFF30981EE +:100FE00068226846FFF70CFFEFF30583044B9A6B0A +:100FF0009A6A9A6A9A6A9A6A9A6A9B6AFEE700BF34 +:1010000000ED00E000B59BB0EFF30981682268466F +:10101000FFF7F6FEEFF30583034B5A6B9A6A9A6A61 +:101020009A6A9A6A9B6AFEE700ED00E0FEE700001C +:1010300030B50A44084D91420DD011F8013B58409B +:10104000082340F30004013B2C4013F0FF0384EA23 +:101050005000F6D1EFE730BD2083B8ED0268436859 +:101060001143016003B1184770470000024A13683A +:1010700043F0C003136070470010014013B50E4CDD +:10108000204600F08BFA04F1140000234FF40072A4 +:101090000A49009400F04CF9094B4FF400720949D9 +:1010A00004F13800009400F0C5F9074A074BC4E981 +:1010B000172302B010BD00BFCC230020382400202D +:1010C0006D100008382600200010014000E1F505F1 +:1010D000037C30B5244C002918BF0C46012B11D1DC +:1010E000224B98420ED1224BD3F8F02042F010024E +:1010F000C3F8F020D3F8182142F01002C3F81821E9 +:10110000D3F818312268036EC16D03EB5203846675 +:10111000B3FBF2F36268150442BF23F0070503F046 +:10112000070343EA4503CB60A36843F040034B60E9 +:10113000E36843F001038B6042F4967343F00103CC +:101140000B604FF0FF330B62510505D512F01022F2 +:1011500005D0B2F1805F04D080F8643030BD7F23C9 +:10116000FAE73F23F8E700BFF03F0008CC23002058 +:10117000004402582DE9F047C66D05463768F4690A +:101180002107346219D014F0080118BF8021E2074A +:1011900048BF41F02001A3074FF0200348BF41F0B2 +:1011A0004001600748BF41F4807183F31188281D16 +:1011B000FFF754FF002383F31188E2050AD52023AB +:1011C00083F311884FF40071281DFFF747FF0023B8 +:1011D00083F311884FF020094FF0000A14F0200823 +:1011E00038D13B0616D54FF0200905F1380A200604 +:1011F00010D589F31188504600F050F9002836DAEE +:101200000821281DFFF72AFF27F080033360002301 +:1012100083F31188790614D5620612D5202383F34F +:101220001188D5E913239A4208D12B6C33B127F0EA +:1012300040071021281DFFF711FF3760002383F3BB +:101240001188E30618D5AA6E1369ABB15069BDE8E1 +:10125000F047184789F31188736A284695F8641097 +:10126000194000F0B5F98AF31188F469B6E7B06265 +:1012700088F31188F469BAE7BDE8F087F8B5154638 +:10128000826804460B46AA4200D28568A169266995 +:10129000761AB5420BD218462A46FFF7B1FDA3696C +:1012A0002B44A3612846A3685B1BA360F8BD0CD93F +:1012B000AF1B18463246FFF7A3FD3A46E1683044BB +:1012C000FFF79EFDE3683B44EBE718462A46FFF72D +:1012D00097FDE368E5E7000083689342F7B50446AD +:1012E000154600D28568D4E90460361AB5420BD29F +:1012F0002A46FFF785FD63692B4463612846A3688E +:101300005B1BA36003B0F0BD0DD93246AF1B01914A +:10131000FFF776FD01993A46E0683144FFF770FD2A +:10132000E3683B44E9E72A46FFF76AFDE368E4E740 +:1013300010B50A440024C361029B8460C16002614D +:101340000362C0E90000C0E9051110BD08B5D0E98D +:101350000532934201D1826882B98268013282608B +:101360005A1C426119700021D0E904329A4224BF0C +:10137000C368436100F0B6FE002008BD4FF0FF30A7 +:10138000FBE7000070B5202304460E4683F3118866 +:10139000A568A5B1A368A269013BA360531CA36122 +:1013A00015782269934224BFE368A361E3690BB116 +:1013B00020469847002383F31188284607E03146EA +:1013C000204600F07FFE0028E2DA85F3118870BD28 +:1013D0002DE9F74F04460E4617469846D0F81C9064 +:1013E0004FF0200A8AF311884FF0000B154665B1C3 +:1013F0002A4631462046FFF741FF034660B9414681 +:10140000204600F05FFE0028F1D0002383F311880E +:10141000781B03B0BDE8F08FB9F1000F03D0019045 +:101420002046C847019B8BF31188ED1A1E448AF3AE +:101430001188DCE7C160C361009B82600362C0E980 +:1014400005111144C0E9000001617047F8B5044678 +:101450000D461646202383F31188A768A7B1A36819 +:10146000013BA36063695A1C62611D70D4E90432B8 +:101470009A4224BFE3686361E3690BB12046984751 +:10148000002080F3118807E03146204600F01AFE64 +:101490000028E2DA87F31188F8BD0000D0E90523BF +:1014A00010B59A4201D182687AB98268002101326E +:1014B00082605A1C82611C7803699A4224BFC36807 +:1014C000836100F00FFE204610BD4FF0FF30FBE7B8 +:1014D0002DE9F74F04460E4617469846D0F81C9063 +:1014E0004FF0200A8AF311884FF0000B154665B1C2 +:1014F0002A4631462046FFF7EFFE034660B94146D3 +:10150000204600F0DFFD0028F1D0002383F311888E +:10151000781B03B0BDE8F08FB9F1000F03D0019044 +:101520002046C847019B8BF31188ED1A1E448AF3AD +:101530001188DCE7026843681143016003B1184772 +:10154000704700001430FFF743BF00004FF0FF3337 +:101550001430FFF73DBF00003830FFF7B9BF00007F +:101560004FF0FF333830FFF7B3BF00001430FFF700 +:1015700009BF00004FF0FF311430FFF703BF000038 +:101580003830FFF763BF00004FF0FF323830FFF70D +:101590005DBF000000207047FFF770BD044B036083 +:1015A00000234360C0E9023301230374704700BF86 +:1015B0000840000810B52023044683F31188FFF784 +:1015C00087FD02232374002383F3118810BD0000DC +:1015D00038B5C36904460D461BB904210844FFF71A +:1015E000A9FF294604F11400FFF7B0FE002806DA2F +:1015F000201D4FF48061BDE83840FFF79BBF38BD28 +:10160000026843681143016003B118477047000046 +:1016100013B5406B00F58054D4F8A4381A681178DB +:10162000042914D1017C022911D1197901231289CD +:101630008B4013420BD101A94C3002F03DF8D4F895 +:10164000A4480246019B2179206800F0DFF902B02E +:1016500010BD0000143001F0BFBF00004FF0FF3399 +:10166000143001F0B9BF00004C3002F091B8000016 +:101670004FF0FF334C3002F08BB80000143001F013 +:101680008DBF00004FF0FF31143001F087BF000024 +:101690004C3002F05DB800004FF0FF324C3002F0E9 +:1016A00057B800000020704710B500F58054D4F8FA +:1016B000A4381A681178042917D1017C022914D1A1 +:1016C0005979012352898B4013420ED1143001F015 +:1016D0001FFF024648B1D4F8A4484FF44073617923 +:1016E0002068BDE8104000F07FB910BD406BFFF7E7 +:1016F000DBBF0000704700007FB5124B01250426B8 +:10170000044603600023057400F184024360294607 +:10171000C0E902330C4B0290143001934FF4407334 +:10172000009601F0D1FE094B04F69442294604F1DB +:101730004C000294CDE900634FF4407301F098FF30 +:1017400004B070BD30400008ED1600081116000806 +:101750000A68202383F311880B790B3342F82300A6 +:101760004B79133342F823008B7913B10B3342F8D2 +:10177000230000F58053C3F8A41802230374002348 +:1017800083F311887047000038B5037F044613B116 +:1017900090F85430ABB90125201D0221FFF730FF2E +:1017A00004F114006FF00101257700F0A7FC04F1AB +:1017B0004C0084F854506FF00101BDE8384000F04F +:1017C0009DBC38BD10B5012104460430FFF718FF59 +:1017D0000023237784F8543010BD000038B5044648 +:1017E0000025143001F088FE04F14C00257701F04B +:1017F00057FF201D84F854500121FFF701FF2046B8 +:10180000BDE83840FFF750BF90F8803003F0600328 +:10181000202B06D190F881200023212A03D81F2AEB +:1018200006D800207047222AFBD1C0E91D3303E00F +:10183000034A426707228267C3670120704700BFDF +:101840002C22002037B500F58055D5F8A4381A6849 +:10185000117804291AD1017C022917D119790123A1 +:1018600012898B40134211D100F14C04204601F043 +:10187000D7FF58B101A9204601F01EFFD5F8A448B2 +:101880000246019B2179206800F0C0F803B030BD0A +:1018900001F10B03F0B550F8236085B004460D4606 +:1018A000FEB1202383F3118804EB8507301D082146 +:1018B000FFF7A6FEFB6806F14C005B691B681BB1D5 +:1018C000019001F007FF019803A901F0F5FE02461F +:1018D00048B1039B2946204600F098F8002383F383 +:1018E000118805B0F0BDFB685A691268002AF5D06E +:1018F0001B8A013B1340F1D104F18002EAE70000AA +:10190000133138B550F82140ECB1202383F311880E +:1019100004F58053D3F8A4281368527903EB8203AB +:10192000DB689B695D6845B104216018FFF768FEBC +:10193000294604F1140001F0F5FD2046FFF7B4FE3E +:10194000002383F3118838BD7047000001F0E8B828 +:1019500001234022002110B5044600F8303BFFF778 +:1019600075FA0023C4E9013310BD000010B520232F +:10197000044683F311882422416000210C30FFF7D4 +:1019800065FA204601F0EEF802232370002383F36A +:10199000118810BD70B500EB8103054650690E46F5 +:1019A0001446DA6018B110220021FFF74FFAA0693F +:1019B00018B110220021FFF749FA31462846BDE848 +:1019C000704001F0E1B9000083682022002103F09B +:1019D000011310B5044683601030FFF737FA204634 +:1019E000BDE8104001F05CBAF0B4012500EB8104C1 +:1019F00047898D40E4683D43A46945812360002305 +:101A0000A2606360F0BC01F079BA0000F0B4012577 +:101A100000EB810407898D40E4683D4364690581DA +:101A200023600023A2606360F0BC01F0EFBA000005 +:101A300070B5022300250446242203702946C0F80D +:101A400088500C3040F8045CFFF700FA204684F818 +:101A5000705001F02DF963681B6823B129462046B8 +:101A6000BDE87040184770BD037880F88C300523BE +:101A7000037043681B6810B504460BB104219847F6 +:101A80000023A36010BD000090F88C204368027012 +:101A90001B680BB1052118477047000070B590F81E +:101AA0007030044613B1002380F8703004F18002D6 +:101AB000204601F019FA63689B68B3B994F8803046 +:101AC00013F0600535D00021204601F0C3FC002151 +:101AD000204601F0B3FC63681B6813B10621204661 +:101AE0009847062384F8703070BD20469847002838 +:101AF000E4D0B4F88630A26F9A4288BFA36794F905 +:101B00008030A56F002B4FF0200380F20381002D61 +:101B100000F0F280092284F8702083F311880021FC +:101B20002046D4E91D23FFF771FF002383F31188BA +:101B3000DAE794F8812003F07F0343EA022340F2BE +:101B40000232934200F0C58021D8B3F5807F48D09F +:101B50000DD8012B3FD0022B00F09380002BB2D187 +:101B600004F1880262670222A267E367C1E7B3F566 +:101B7000817F00F09B80B3F5407FA4D194F8823040 +:101B8000012BA0D1B4F8883043F0020332E0B3F562 +:101B9000006F4DD017D8B3F5A06F31D0A3F5C06357 +:101BA000012B90D86368204694F882205E6894F8F0 +:101BB0008310B4F88430B047002884D0436863674A +:101BC0000368A3671AE0B3F5106F36D040F60242FF +:101BD00093427FF478AF5C4B63670223A3670023D3 +:101BE000C3E794F88230012B7FF46DAFB4F88830EE +:101BF00023F00203A4F88830C4E91D55E56778E7AF +:101C0000B4F88030B3F5A06F0ED194F8823020463E +:101C100084F88A3001F0AAF863681B6813B10121C7 +:101C200020469847032323700023C4E91D339CE713 +:101C300004F18B0363670123C3E72378042B10D1DE +:101C4000202383F311882046FFF7BEFE85F3118819 +:101C50000321636884F88B5021701B680BB1204608 +:101C6000984794F88230002BDED084F88B30042320 +:101C7000237063681B68002BD6D00221204698474A +:101C8000D2E794F8843020461D0603F00F010AD5F0 +:101C900001F01CF9012804D002287FF414AF2B4B6B +:101CA0009AE72B4B98E701F003F9F3E794F88230B9 +:101CB000002B7FF408AF94F8843013F00F01B3D0F9 +:101CC0001A06204602D501F0DDFBADE701F0CEFBA0 +:101CD000AAE794F88230002B7FF4F5AE94F88430B4 +:101CE00013F00F01A0D01B06204602D501F0B2FB75 +:101CF0009AE701F0A3FB97E7142284F8702083F39E +:101D000011882B462A4629462046FFF76DFE85F3AB +:101D10001188E9E65DB1152284F8702083F31188FB +:101D200000212046D4E91D23FFF75EFEFDE60B22CD +:101D300084F8702083F311882B462A4629462046D2 +:101D4000FFF764FEE3E700BF60400008584000086A +:101D50005C40000838B590F870300446002B3ED047 +:101D6000063BDAB20F2A34D80F2B32D8DFE803F063 +:101D70003731310822323131313131313131373778 +:101D8000856FB0F886309D4214D2C3681B8AB5FBBC +:101D9000F3F203FB12556DB9202383F311882B4610 +:101DA0002A462946FFF732FE85F311880A2384F874 +:101DB00070300EE0142384F87030202383F31188F0 +:101DC000002320461A461946FFF70EFE002383F330 +:101DD000118838BDC36F03B198470023E7E700219E +:101DE000204601F037FB0021204601F027FB636805 +:101DF0001B6813B10621204698470623D7E7000049 +:101E000010B590F870300446142B29D017D8062B43 +:101E100005D001D81BB110BD093B022BFBD8002116 +:101E2000204601F017FB0021204601F007FB636804 +:101E30001B6813B1062120469847062319E0152B8D +:101E4000E9D10B2380F87030202383F3118800231D +:101E50001A461946FFF7DAFD002383F31188DAE703 +:101E6000C3689B695B68002BD5D1C36F03B19847EA +:101E7000002384F87030CEE7024B0022C3E9003320 +:101E80009A60704738280020002382680374054B4D +:101E90001B6899689142FBD25A68036042601060E7 +:101EA000586070473828002008B5202383F3118834 +:101EB000037C032B05D0042B0DD02BB983F31188A1 +:101EC00008BD436900221A604FF0FF334361FFF7FA +:101ED000DBFF0023F2E7D0E9003213605A60F3E73A +:101EE000002382680374054B1B6899689142FBD8F4 +:101EF0005A680360426010605860704738280020BC +:101F0000054B196908741868026853601A601861F3 +:101F100001230374FEF75ABA382800204B1C30B551 +:101F2000044687B00A4D10D02B6901A8094A00F079 +:101F300001F92046FFF7E4FF049B13B101A800F06C +:101F400035F92B69586907B030BDFFF7D9FFF8E7BD +:101F500038280020A91E000838B50C4D0446416100 +:101F60002B6981689A68914203D8BDE83840FFF731 +:101F70008BBF1846FFF7B4FF01232C610146237481 +:101F80002046BDE83840FEF721BA00BF38280020BF +:101F9000044B1A681B6990689B68984294BF0020A4 +:101FA000012070473828002010B5084C23682069AC +:101FB0001A6854602260012223611A74FFF790FFAF +:101FC00001462069BDE81040FEF700BA382800201D +:101FD000FFF7EABFFEE7000010B50C4CFFF74CFF1F +:101FE00000F09AF880220A49204600F021F80123E7 +:101FF00044F8180C037400F071FC002383F311887B +:1020000062B60448BDE8104000F032B860280020F5 +:10201000644000087440000800F002B9034A5168A7 +:1020200053685B1A9842FBD8704700BF001000E06D +:1020300082600222028270478368A3F17C0243F827 +:102040000C2C026943F83C2C426943F8382C074AAF +:1020500043F81C2CC268A3F1180043F8102C02228C +:1020600003F8082C002203F8072C7047E50300084A +:1020700010B5202383F31188FFF7DEFF002104460B +:10208000FFF76AFF002383F31188204610BD00008C +:10209000024B1B6958610F20FFF732BF3828002020 +:1020A000202383F31188FFF7F3BF000008B5014632 +:1020B000202383F311880820FFF730FF002383F3E8 +:1020C000118808BD49B1064B42681B6918605A6007 +:1020D000136043600420FFF721BF4FF0FF307047CB +:1020E000382800200368984206D01A6802605060C1 +:1020F00018465961FFF7C8BE7047000038B504465E +:102100000D462068844200D138BD036823605C60BE +:102110004561FFF7B9FEF4E7054B4FF0FF3103F1DE +:102120001402C3E905220022C3E90712704700BF69 +:102130003828002070B51C4E05460C46C0E9032324 +:1021400001F0A4FB334653F8142F9A420DD13062AC +:102150000A2C2CBF00190A302A60C5E90124C6E9FF +:102160000555BDE8704001F07BBB316A431AE318A6 +:1021700038BF1C469368A34202D9081901F080FBBE +:1021800073699A6894420CD85A68AC602B606A6094 +:1021900015609A685D60121B9A604FF0FF33F3611F +:1021A00070BDA41A1B68ECE73828002038B51B4C1A +:1021B000636998420DD08168D0E9003213605A609B +:1021C0000022C2609A680A449A604FF0FF33E361CC +:1021D00038BD03682246002142F8143F93425A60FA +:1021E000C16003D1BDE8384001F044BB9A68816802 +:1021F000256A0A449A6001F049FB6369411B9A68A9 +:102200008A42E5D9AB181D1A206A092D98BF01F141 +:102210000A02BDE83840104401F032BB38280020E3 +:102220002DE9F041184C002704F11406656901F00E +:102230002DFB236AAA68C11A8A4215D81344D5F81F +:102240000C802362D5E9003213605A606369EF6045 +:10225000B34201D101F00EFB87F311882869C04712 +:10226000202383F31188E1E76169B14209D0134467 +:102270001B1ABDE8F0410A2B2CBFC0180A3001F030 +:10228000FFBABDE8F08100BF382800200020704769 +:10229000FEE70000704700004FF0FF30704700007D +:1022A00002290CD0032904D00129074818BF0020B7 +:1022B0007047032A05D8054800EBC2007047044860 +:1022C00070470020704700BF584100083C220020A2 +:1022D0000C41000870B59AB005460846144601A99D +:1022E00000F0C2F801A8FEF7A9FD431C0022C6B207 +:1022F0005B001046C5E9003423700323023404F860 +:10230000013C01ABD1B202348E4201D81AB070BD8B +:1023100013F8011B013204F8010C04F8021CF1E768 +:1023200008B5202383F311880348FFF78BFA0023B5 +:1023300083F3118808BD00BFF029002090F8803099 +:1023400003F01F02012A07D190F881200B2A03D144 +:102350000023C0E91D3315E003F06003202B08D1F2 +:10236000B0F884302BB990F88120212A03D81F2A95 +:1023700004D8FFF749BA222AEBD0FAE7034A4267AA +:1023800007228267C3670120704700BF3322002005 +:1023900007B5052917D8DFE801F019160319192028 +:1023A000202383F31188104A01210190FFF7F2FAEC +:1023B000019802210D4AFFF7EDFA0D48FFF70EFADA +:1023C000002383F3118803B05DF804FB202383F31B +:1023D00011880748FFF7D8F9F2E7202383F3118823 +:1023E0000348FFF7EFF9EBE7AC400008D0400008E6 +:1023F000F029002038B50C4D0C4C2A460C4904F14C +:102400000800FFF767FF05F1CA0204F1100009494F +:10241000FFF760FF05F5CA7204F118000649BDE830 +:102420003840FFF757BF00BFC84200203C220020C1 +:102430008C40000896400008A140000870B5044692 +:1024400008460D46FEF7FAFCC6B220460134037872 +:102450000BB9184670BD32462946FEF7DBFC002852 +:10246000F3D10120F6E700002DE9F84F05460C46B0 +:10247000FEF7E4FC2C49C6B22846FFF7DFFF08B19F +:102480000236F6B229492846FFF7D8FF08B11036C0 +:10249000F6B2632E0DD8DFF89080DFF89090244FCD +:1024A000DFF894A0DFF894B02E7846B92670BDE826 +:1024B000F88F29462046BDE8F84F01F07FBD252E54 +:1024C0002ED1072241462846FEF7A4FC70B9DBF85E +:1024D00000300735093444F8093CDBF8043044F88F +:1024E000053C9BF8083004F8013CDDE7082249462A +:1024F0002846FEF78FFC98B9A21C0E4B19780232C1 +:102500000909C95D02F8041C13F8011B01F00F0151 +:102510005345C95D02F8031CF0D118340835C3E7F0 +:10252000013504F8016BBFE778410008A1400008BD +:102530008A41000800E8F11F0CE8F11F8041000803 +:10254000BFF34F8F044B1A695107FCD1D3F8102108 +:102550005207F8D1704700BF0020005208B50D4B5C +:102560001B78ABB9FFF7ECFF0B4BDA68D10704D54A +:102570000A4A5A6002F188325A60D3F80C21D20715 +:1025800006D5064AC3F8042102F18832C3F80421B3 +:1025900008BD00BF264500200020005223016745EA +:1025A00008B5114B1B78F3B9104B1A69510703D5C5 +:1025B000DA6842F04002DA60D3F81021520705D5FC +:1025C000D3F80C2142F04002C3F80C21FFF7B8FF0A +:1025D000064BDA6842F00102DA60D3F80C2142F0CF +:1025E0000102C3F80C2108BD26450020002000523E +:1025F0000F289ABF00F5806040040020704700005B +:102600004FF4003070470000102070470F2808B5C5 +:102610000BD8FFF7EDFF00F500330268013204D15B +:1026200004308342F9D1012008BD0020FCE70000FE +:102630000F2838B505463FD8FFF782FF1F4CFFF73C +:102640008DFF4FF0FF3307286361C4F814311DD8A4 +:102650002361FFF775FF030243F02403E360E3689F +:1026600043F08003E36023695A07FCD42846FFF750 +:1026700067FFFFF7BDFF4FF4003100F0F7F8284681 +:10268000FFF78EFFBDE83840FFF7C0BFC4F8103138 +:10269000FFF756FFA0F108031B0243F02403C4F820 +:1026A0000C31D4F80C3143F08003C4F80C31D4F869 +:1026B00010315B07FBD4D9E7002038BD0020005261 +:1026C0002DE9F84F05460C46104645EA0203DB06A5 +:1026D0005DD122F003022B462A44934209D120F017 +:1026E0001F00DFF8BCB0DFF8BCA0FFF737FF2718EA +:1026F00044E0196801314AD10433EEE705F178432B +:1027000024492548B3F5801F244B38BF184603F1F0 +:10271000F80339BF8846D9469846D146FFF710FFDF +:102720004FF0FF33A5EB040C04F11C010360D8F853 +:10273000003043F00203C8F80030231FD9F80060CE +:1027400016F00506FAD153F8042F99424CF80320ED +:10275000F4D1BFF34F8FFFF7F3FE4FF0FF3320228A +:10276000214603602846D8F8003023F00203C8F859 +:10277000003001F013FC40B920352034BC42BDD1FB +:102780000120FFF70DFFBDE8F88F3046F9E7002084 +:10279000F9E700BF0C20005214210052142000520F +:1027A000102000521021005210B5084C237828B197 +:1027B0001BB9FFF7D3FE0123237010BD002BFCD003 +:1027C0002070BDE81040FFF7EBBE00BF264500209B +:1027D0000244074BD2B210B5904200D110BD441C48 +:1027E00000B253F8200041F8040BE0B2F4E700BF58 +:1027F000504000580E4B30B51C6F240405D41C6F9C +:102800001C671C6F44F400441C670A4C0244236894 +:10281000D2B243F480732360074B904200D130BDA5 +:10282000441C51F8045B00B243F82050E0B2F4E7D6 +:1028300000440258004802585040005807B5012291 +:1028400001A90020FFF7C4FF019803B05DF804FB65 +:1028500013B50446FFF7F2FFA04205D0012201A9FB +:1028600000200194FFF7C6FF02B010BD0144BFF382 +:102870004F8F064B884204D3BFF34F8FBFF36F8F48 +:102880007047C3F85C022030F4E700BF00ED00E0C1 +:10289000034B1A681AB9034AD2F8D0241A60704759 +:1028A000284500200040025808B5FFF7F1FF024B11 +:1028B0001868C0F3806008BD28450020EFF3098345 +:1028C000054968334A6B22F001024A6383F30988A1 +:1028D000002383F31188704700EF00E0202080F38D +:1028E000118862B60D4B0E4AD96821F4E0610904E3 +:1028F000090C0A430B49DA60D3F8FC2042F08072DD +:10290000C3F8FC20084AC2F8B01F116841F0010169 +:1029100011601022DA7783F82200704700ED00E0A2 +:102920000003FA0555CEACC5001000E0202310B519 +:1029300083F311880E4B5B6813F4006314D0F1EE3F +:10294000103AEFF309844FF08073683CE361094B60 +:10295000DB6B236684F30988FFF71AFB10B1064B83 +:10296000A36110BD054BFBE783F31188F9E700BFB6 +:1029700000ED00E000EF00E0F7030008FA030008B4 +:1029800070B5BFF34F8FBFF36F8F1A4A0021C2F8A3 +:102990005012BFF34F8FBFF36F8F536943F400336F +:1029A0005361BFF34F8FBFF36F8FC2F88410BFF333 +:1029B0004F8FD2F8803043F6E074C3F3C900C3F3FD +:1029C0004E335B0103EA0406014646EA817501398C +:1029D000C2F86052F9D2203B13F1200FF2D1BFF3BD +:1029E0004F8F536943F480335361BFF34F8FBFF36D +:1029F0006F8F70BD00ED00E0FEE700000A4B0B4852 +:102A00000B4A90420BD30B4BC11EDA1C121A22F058 +:102A100003028B4238BF00220021FEF717BA53F899 +:102A2000041B40F8041BECE76C430008FC46002044 +:102A3000FC460020FC4600207047000070B5D0E93D +:102A4000244300224FF0FF359E6804EB42135101EE +:102A5000D3F80009002805DAD3F8000940F08040D7 +:102A6000C3F80009D3F8000B002805DAD3F8000BEF +:102A700040F08040C3F8000B013263189642C3F85F +:102A80000859C3F8085BE0D24FF00113C4F81C38B2 +:102A900070BD000000EB8103D3F80CC02DE9F043BA +:102AA000DCF814204E1CD0F89050D2F800E005EB72 +:102AB000063605EB4118506870450AD30122D5F857 +:102AC000343802FA01F123EA0101C5F83418BDE8EF +:102AD000F083AEEB0003BCF81040A34228BF2346AE +:102AE000D8F81849A4B2B3EB840FF0D89468A4F1D5 +:102AF000040959F8047F3760A4EB09071F44042F29 +:102B0000F7D81C44034494605360D4E7890141F032 +:102B10002001016103699B06FCD41220FFF77EBAF5 +:102B200010B50A4C2046FEF713FF094BC4F890304D +:102B3000084BC4F89430084C2046FEF709FF074BB9 +:102B4000C4F89030064BC4F8943010BD2C450020DA +:102B500000000840C0410008C845002000000440B3 +:102B6000CC41000870B503780546012B5DD1494B77 +:102B7000D0F89040984259D1474B0E216520D3F8A8 +:102B8000D82042F00062C3F8D820D3F8002142F0E8 +:102B90000062C3F80021D3F80021D3F8802042F06E +:102BA0000062C3F88020D3F8802022F00062C3F8CE +:102BB0008020D3F8803000F071FC384BE360384B54 +:102BC000C4F800380023D5F89060C4F8003EC02354 +:102BD00023604FF40413A3633369002BFCDA012351 +:102BE0000C203361FFF71AFA3369DB07FCD412209B +:102BF000FFF714FA3369002BFCDA00262846A6609A +:102C0000FFF71CFF6B68C4F81068DB68C4F8146831 +:102C1000C4F81C68002B3AD1224BA3614FF0FF335C +:102C20006361A36843F00103A36070BD1E4B98422B +:102C3000C8D1194B0E214D20D3F8D82042F0007294 +:102C4000C3F8D820D3F8002142F00072C3F8002165 +:102C5000D3F80021D3F8802042F00072C3F880201E +:102C6000D3F8802022F00072C3F88020D3F88020AF +:102C7000D3F8D82022F08062C3F8D820D3F80021FE +:102C800022F08062C3F80021D3F8003193E7074BAC +:102C9000C3E700BF2C450020004402584014004008 +:102CA00003002002003C30C0C8450020083C30C072 +:102CB000F8B5D0F89040054600214FF00066204658 +:102CC000FFF724FFD5F8941000234FF001128F680E +:102CD0004FF0FF30C4F83438C4F81C2804EB43121A +:102CE00001339F42C2F80069C2F8006BC2F80809BC +:102CF000C2F8080BF2D20B68D5F89020C5F89830CE +:102D0000636210231361166916F01006FBD11220BE +:102D1000FFF784F9D4F8003823F4FE63C4F80038D0 +:102D2000A36943F4402343F01003A3610923C4F8CB +:102D30001038C4F814380B4BEB604FF0C043C4F8A4 +:102D4000103B094BC4F8003BC4F81069C4F80039C3 +:102D5000D5F8983003F1100243F48013C5F8982099 +:102D6000A362F8BD9C41000840800010D0F890207C +:102D700090F88A10D2F8003823F4FE6343EA011376 +:102D8000C2F80038704700002DE9F84300EB8103DA +:102D9000D0F890500C468046DA680FFA81F9480165 +:102DA000166806F00306731E022B05EB41134FF065 +:102DB000000194BFB604384EC3F8101B4FF0010158 +:102DC00004F1100398BF06F1805601FA03F39169EC +:102DD00098BF06F5004600293AD0578A04F15801F9 +:102DE000374349016F50D5F81C180B430021C5F833 +:102DF0001C382B180127C3F81019A7405369611E0E +:102E00009BB3138A928B9B08012A88BF5343D8F83F +:102E10009820981842EA034301F140022146C8F87D +:102E20009800284605EB82025360FFF76FFE08EB1F +:102E30008900C3681B8A43EA845348341E436401F3 +:102E40002E51D5F81C381F43C5F81C78BDE8F8830F +:102E500005EB4917D7F8001B21F40041C7F8001B08 +:102E6000D5F81C1821EA0303C0E704F13F030B4A1D +:102E70002846214605EB83035A60FFF747FE05EB22 +:102E80004910D0F8003923F40043C0F80039D5F8D0 +:102E90001C3823EA0707D7E700800010000400026F +:102EA000D0F894201268C0F89820FFF7C7BD000042 +:102EB0005831D0F8903049015B5813F4004004D0E9 +:102EC00013F4001F0CBF0220012070474831D0F8D6 +:102ED000903049015B5813F4004004D013F4001FF4 +:102EE0000CBF02200120704700EB8101CB68196AFA +:102EF0000B6813604B6853607047000000EB810360 +:102F000030B5DD68AA691368D36019B9402B84BF56 +:102F1000402313606B8A1468D0F890201C4402EBA5 +:102F20004110013C09B2B4FBF3F46343033323F0D3 +:102F3000030343EAC44343F0C043C0F8103B2B688B +:102F400003F00303012B0ED1D2F8083802EB411035 +:102F500013F4807FD0F8003B14BF43F0805343F05C +:102F60000053C0F8003B02EB4112D2F8003B43F0A3 +:102F70000443C2F8003B30BD2DE9F041D0F8906029 +:102F800005460C4606EB4113D3F8087B3A07C3F815 +:102F9000087B08D5D6F814381B0704D500EB81034D +:102FA000DB685B689847FA071FD5D6F81438DB074B +:102FB0001BD505EB8403D968CCB98B69488A5A685C +:102FC000B2FBF0F600FB16228AB91868DA68904264 +:102FD0000DD2121AC3E90024202383F3118821465D +:102FE0002846FFF78BFF84F31188BDE8F0810123A9 +:102FF00003FA04F26B8923EA02036B81CB68002B8E +:10300000F3D021462846BDE8F041184700EB810384 +:103010004A0170B5DD68D0F890306C692668E660CA +:1030200056BB1A444FF40020C2F810092A6802F077 +:103030000302012A0AB20ED1D3F8080803EB4214A6 +:1030400010F4807FD4F8000914BF40F0805040F0A5 +:103050000050C4F8000903EB4212D2F8000940F016 +:103060000440C2F800090122D3F8340802FA01F141 +:103070000143C3F8341870BD19B9402E84BF4020F5 +:10308000206020681A442E8A8419013CB4FBF6F4AF +:1030900040EAC44040F00050C6E700002DE9F0418E +:1030A000D0F8906004460D4606EB4113D3F808793A +:1030B000C3F80879FB071CD5D6F81038DA0718D5FD +:1030C00000EB8103D3F80CC0DCF81430D3F800E037 +:1030D000DA6896451BD2A2EB0E024FF000081A6088 +:1030E000C3F80480202383F31188FFF78FFF88F350 +:1030F00011883B0618D50123D6F83428AB4013427B +:1031000012D029462046BDE8F041FFF7C3BC012399 +:1031100003FA01F2038923EA02030381DCF8083091 +:10312000002BE6D09847E4E7BDE8F0812DE9F84FA1 +:10313000D0F8905004466E69AB691E4016F4805872 +:103140006E6103D0BDE8F84FFEF772BC002E12DAB4 +:10315000D5F8003E9F0705D0D5F8003E23F00303C5 +:10316000C5F8003ED5F80438204623F00103C5F821 +:103170000438FEF789FC300505D52046FFF75EFCD4 +:103180002046FEF771FCB1040CD5D5F8083813F0D1 +:10319000060FEB6823F470530CBF43F4105343F451 +:1031A000A053EB60320704D56368DB680BB120469F +:1031B0009847F30200F1BA80B70226D5D4F8909070 +:1031C00000274FF0010A09EB4712D2F8003B03F445 +:1031D0004023B3F5802F11D1D2F8003B002B0DDA3C +:1031E00062890AFA07F322EA0303638104EB870387 +:1031F000DB68DB6813B13946204698470137D4F8BD +:103200009430FFB29B689F42DDD9F00619D5D4F8FF +:103210009000026AC2F30A1702F00F0302F4F012E0 +:10322000B2F5802F00F0CC80B2F5402F09D104EB2D +:103230008303002200F58050DB681B6A974240F050 +:10324000B2803003D5F8185835D5E90303D50021ED +:103250002046FFF791FEAA0303D501212046FFF780 +:103260008BFE6B0303D502212046FFF785FE2F035B +:1032700003D503212046FFF77FFEE80203D5042192 +:103280002046FFF779FEA90203D505212046FFF766 +:1032900073FE6A0203D506212046FFF76DFE2B025E +:1032A00003D507212046FFF767FEEF0103D508216C +:1032B0002046FFF761FE700340F1A980E90703D5BE +:1032C00000212046FFF7EAFEAA0703D50121204688 +:1032D000FFF7E4FE6B0703D502212046FFF7DEFE71 +:1032E0002F0703D503212046FFF7D8FEEE0603D5AE +:1032F00004212046FFF7D2FEA80603D5052120466B +:10330000FFF7CCFE690603D506212046FFF7C6FE6F +:103310002A0603D507212046FFF7C0FEEB0576D528 +:1033200020460821BDE8F84FFFF7B8BED4F89090CA +:1033300000274FF0010AD4F894305FFA87FB9B68AE +:103340009B453FF639AF09EB4B13D3F8002902F444 +:103350004022B2F5802F24D1D3F80029002A20DAA8 +:10336000D3F8002942F09042C3F80029D3F800298D +:10337000002AFBDB5946D4F89000FFF7C7FB2289EF +:103380000AFA0BF322EA0303238104EB8B03DB68C5 +:103390009B6813B159462046984759462046FFF787 +:1033A00079FB0137C7E7910701D1D0F80080072AE0 +:1033B00002F101029CBF03F8018B4FEA18283DE798 +:1033C00004EB830300F58050DA68D2F818C0DCF80B +:1033D0000820DCE9001CA1EB0C0C00218F4208D175 +:1033E000DB689B699A683A449A605A683A445A6022 +:1033F00027E711F0030F01D1D0F800808C4501F1CF +:10340000010184BF02F8018B4FEA1828E6E7BDE806 +:10341000F88F000008B50348FFF788FEBDE80840B4 +:10342000FFF784BA2C45002008B50348FFF77EFE5D +:10343000BDE80840FFF77ABAC8450020D0F89030C0 +:1034400003EB4111D1F8003B43F40013C1F8003BFA +:1034500070470000D0F8903003EB4111D1F80039EB +:1034600043F40013C1F8003970470000D0F89030E1 +:1034700003EB4111D1F8003B23F40013C1F8003BEA +:1034800070470000D0F8903003EB4111D1F80039BB +:1034900023F40013C1F8003970470000090100F15E +:1034A0006043012203F56143C9B283F8001300F0C1 +:1034B0001F039A4043099B0003F1604303F56143F6 +:1034C000C3F880211A60704730B50433039C017241 +:1034D000002104FB0325C160C0E90653049B03637C +:1034E000059BC0E90000C0E90422C0E90842C0E928 +:1034F0000A11436330BD00000022416AC260C0E986 +:103500000411C0E90A226FF00101FEF7F7BD0000C7 +:10351000D0E90432934201D1C2680AB9181D70473C +:1035200000207047036919600021C2680132C2603F +:10353000C269134482699342036124BF436A0361F1 +:10354000FEF7D0BD38B504460D46E3683BB162696D +:103550000020131D1268A3621344E36207E0237A7C +:1035600033B929462046FEF7ADFD0028EDDA38BD17 +:103570006FF00100FBE70000C368C269013BC36054 +:103580004369134482699342436124BF436A4361A0 +:1035900000238362036B03B11847704770B5202383 +:1035A000044683F31188866A3EB9FFF7CBFF0546D0 +:1035B00018B186F31188284670BDA36AE26A13F831 +:1035C000015B9342A36202D32046FFF7D5FF00239D +:1035D00083F31188EFE700002DE9F84F04460E460B +:1035E000174698464FF0200989F311880025AA460E +:1035F000D4F828B0BBF1000F09D141462046FFF7AF +:10360000A1FF20B18BF311882846BDE8F88FD4E9DB +:103610000A12A7EB050B521A934528BF9346BBF13C +:10362000400F1BD9334601F1400251F8040B91427F +:1036300043F8040BF9D1A36A403640354033A36206 +:10364000D4E90A239A4202D32046FFF795FF8AF372 +:103650001188BD42D8D289F31188C9E730465A464D +:10366000FDF7CEFBA36A5E445D445B44A362E7E7DB +:1036700010B5029C0433017204FB0321C460C0E94D +:1036800006130023C0E90A33039B0363049BC0E9CC +:103690000000C0E90422C0E90842436310BD0000F5 +:1036A000026A6FF00101C260426AC0E9042200228E +:1036B000C0E90A22FEF722BDD0E904239A4201D1D3 +:1036C000C26822B9184650F8043B0B6070470020CE +:1036D00070470000C3680021C2690133C3604369B9 +:1036E000134482699342436124BF436A4361FEF7F6 +:1036F000F9BC000038B504460D46E3683BB12369C8 +:1037000000201A1DA262E2691344E36207E0237AF3 +:1037100033B929462046FEF7D5FC0028EDDA38BD3E +:103720006FF00100FBE7000003691960C268013A0D +:10373000C260C269134482699342036124BF436A31 +:10374000036100238362036B03B1184770470000D5 +:1037500070B520230D460446114683F31188866A0E +:103760002EB9FFF7C7FF10B186F3118870BDA36AA9 +:103770001D70A36AE26A01339342A36204D3E16934 +:1037800020460439FFF7D0FF002080F31188EDE7D1 +:103790002DE9F84F04460D46904699464FF0200A11 +:1037A0008AF311880026B346A76A4FB949462046D6 +:1037B000FFF7A0FF20B187F311883046BDE8F88FEE +:1037C000D4E90A073A1AA8EB0607974228BF17461A +:1037D000402F1BD905F1400355F8042B9D4240F8BA +:1037E000042BF9D1A36A40364033A362D4E90A23FB +:1037F0009A4204D3E16920460439FFF795FF8BF321 +:1038000011884645D9D28AF31188CDE729463A4630 +:10381000FDF7F6FAA36A3D443E443B44A362E5E764 +:10382000D0E904239A4217D1C3689BB1836A8BB154 +:10383000043B9B1A0ED01360C368013BC360C3698D +:103840001A4483699A42026124BF436A03610023D8 +:1038500083620123184670470023FBE700F0CEB8CF +:10386000034B002258631A610222DA60704700BFDE +:10387000000C0040014B0022DA607047000C004051 +:10388000014B5863704700BF000C0040014B586A61 +:10389000704700BF000C00404B6843608B6883603A +:1038A000CB68C3600B6943614B6903628B694362F8 +:1038B0000B6803607047000008B5364B40F2FF719B +:1038C0003548D3F888200A43C3F88820D3F88820E5 +:1038D00022F4FF6222F00702C3F88820D3F8882080 +:1038E000D3F8E0200A43C3F8E020D3F808210A43C4 +:1038F000C3F80821294AD3F808311146FFF7CCFF55 +:1039000000F5806002F11C01FFF7C6FF00F5806042 +:1039100002F13801FFF7C0FF00F5806002F15401A9 +:10392000FFF7BAFF00F5806002F17001FFF7B4FF06 +:1039300000F5806002F18C01FFF7AEFF00F58060BA +:1039400002F1A801FFF7A8FF00F5806002F1C401B1 +:10395000FFF7A2FF00F5806002F1E001FFF79CFF96 +:1039600000F5806002F1FC01FFF796FF02F58C7113 +:1039700000F58060FFF790FF00F0F4F8084B052297 +:10398000C3F898204FF06052C3F89C20054AC3F852 +:10399000A02008BD0044025800000258D841000889 +:1039A00000ED00E01F00080308B500F0D7FAFEF7AD +:1039B00013FB0F4BD3F8DC2042F04002C3F8DC20AD +:1039C000D3F8042122F04002C3F80421D3F80431D3 +:1039D000084B1A6842F008021A601A6842F00402A2 +:1039E0001A60FEF755FFBDE80840FEF703BD00BFB3 +:1039F000004402580018024870470000114BD3F8E9 +:103A0000E82042F00802C3F8E820D3F8102142F081 +:103A10000802C3F810210C4AD3F81031D36B43F0DD +:103A20000803D363C722094B9A624FF0FF32DA6270 +:103A300000229A615A63DA605A6001225A611A6060 +:103A4000704700BF004402580010005C000C0040AA +:103A5000094A08B51169D3680B40D9B29B076FEAD0 +:103A60000101116107D5202383F31188FEF7D4FAF1 +:103A7000002383F3118808BD000C0040384B4FF041 +:103A8000FF31D3F88020C3F88010D3F880200022C3 +:103A9000C3F88020D3F88000D3F88400C3F88410E2 +:103AA000D3F88400C3F88420D3F88400D86F40F0A2 +:103AB000FF4040F4FF0040F43F5040F03F00D86723 +:103AC000D86F20F0FF4020F4FF0020F43F5020F09A +:103AD0003F00D867D86FD3F888006FEA40506FEA8C +:103AE0005050C3F88800D3F88800C0F30A00C3F828 +:103AF0008800D3F88800D3F89000C3F89010D3F86A +:103B00009000C3F89020D3F89000D3F89400C3F845 +:103B10009410D3F89400C3F89420D3F89400D3F809 +:103B20009800C3F89810D3F89800C3F89820D3F8F9 +:103B30009800D3F88C00C3F88C10D3F88C00C3F82D +:103B40008C20D3F88C00D3F89C00C3F89C10D3F8D9 +:103B50009C10C3F89C20D3F89C3000F0D3B900BF70 +:103B600000440258614B0122C3F80821604BD3F88E +:103B7000F42042F00202C3F8F420D3F81C2142F0F2 +:103B80000202C3F81C210222D3F81C31594BDA601F +:103B90005A689104FCD5584A1A6001229A60574A23 +:103BA000DA6000221A614FF440429A61514B9A69DF +:103BB0009204FCD51A6842F480721A604C4B1A6F5A +:103BC00012F4407F04D04FF480321A6700221A6743 +:103BD0001A6842F001021A60454B1A685007FCD57A +:103BE00000221A611A6912F03802FBD10121196012 +:103BF0004FF0804159605A67414ADA62414A1A617E +:103C00001A6842F480321A60394B1A689103FCD565 +:103C10001A6842F480521A601A689204FCD53A4A33 +:103C20003A499A6200225A6319633949DA639963FF +:103C30005A64384A1A64384ADA621A6842F0A8525A +:103C40001A602B4B1A6802F02852B2F1285FF9D1A2 +:103C500048229A614FF48862DA6140221A622F4A40 +:103C6000DA644FF000521A652D4A5A652D4A9A655A +:103C700032232D4A1360136803F00F03022BFAD18D +:103C80001B4B1A6942F003021A611A6902F03802EA +:103C9000182AFAD1D3F8DC2042F00052C3F8DC2015 +:103CA000D3F8042142F00052C3F80421D3F80421D0 +:103CB000D3F8DC2042F08042C3F8DC20D3F80421A2 +:103CC00042F08042C3F80421D3F80421D3F8DC2069 +:103CD00042F00042C3F8DC20D3F8042142F0004255 +:103CE000C3F80421D3F80431704700BF00800051AD +:103CF000004402580048025800C000F002000001D1 +:103D00000000FF01008890081210200063020701E4 +:103D10002C02040047040508FD0BFF0120000020D1 +:103D20000010E00000010100002000524FF0B042FE +:103D300008B5D2F8883003F00103C2F8883023B107 +:103D4000044A13680BB150689847BDE80840FEF775 +:103D5000EDBD00BF7C4600204FF0B04208B5D2F860 +:103D6000883003F00203C2F8883023B1044A936814 +:103D70000BB1D0689847BDE80840FEF7D7BD00BF3B +:103D80007C4600204FF0B04208B5D2F8883003F0EE +:103D90000403C2F8883023B1044A13690BB1506997 +:103DA0009847BDE80840FEF7C1BD00BF7C46002033 +:103DB0004FF0B04208B5D2F8883003F00803C2F8DB +:103DC000883023B1044A93690BB1D0699847BDE8A4 +:103DD0000840FEF7ABBD00BF7C4600204FF0B0426C +:103DE00008B5D2F8883003F01003C2F8883023B148 +:103DF000044A136A0BB1506A9847BDE80840FEF7C1 +:103E000095BD00BF7C4600204FF0B04310B5D3F8FD +:103E1000884004F47872C3F88820A30604D5124AB7 +:103E2000936A0BB1D06A9847600604D50E4A136BAB +:103E30000BB1506B9847210604D50B4A936B0BB11D +:103E4000D06B9847E20504D5074A136C0BB1506C50 +:103E50009847A30504D5044A936C0BB1D06C9847DE +:103E6000BDE81040FEF762BD7C4600204FF0B04335 +:103E700010B5D3F8884004F47C42C3F8882062056A +:103E800004D5164A136D0BB1506D9847230504D520 +:103E9000124A936D0BB1D06D9847E00404D50F4AD8 +:103EA000136E0BB1506E9847A10404D50B4A936E64 +:103EB0000BB1D06E9847620404D5084A136F0BB15A +:103EC000506F9847230404D5044A936F0BB1D06F09 +:103ED0009847BDE81040FEF729BD00BF7C46002092 +:103EE00008B50348FDF746F9BDE80840FEF71EBDDA +:103EF000CC23002008B5FFF7ABFDBDE80840FEF776 +:103F000015BD0000062108B50846FFF7C7FA0621CF +:103F10000720FFF7C3FA06210820FFF7BFFA0621A2 +:103F20000920FFF7BBFA06210A20FFF7B7FA06219E +:103F30001720FFF7B3FA06212820FFF7AFFA09216F +:103F40007A20FFF7ABFA07213220FFF7A7FA0C21FE +:103F50002520BDE80840FFF7A1BA000008B5FFF72B +:103F60008DFD00F00DF8FDF717FBFDF7EFFCFDF7F9 +:103F7000C1FBFFF741FDBDE80840FFF76FBC000043 +:103F80000023054A19460133102BC2E9001102F142 +:103F90000802F8D1704700BF7C46002010B50139F7 +:103FA0000244904201D1002005E0037811F8014F4E +:103FB000A34201D0181B10BD0130F2E7034611F8EF +:103FC000012B03F8012B002AF9D1704753544D33CC +:103FD0003248373F3F3F0053544D33324837343334 +:103FE0002F37353300000000F0290020CC230020BB +:103FF000009600000000000000000000000000002B +:104000000000000000000000000000006115000832 +:104010004D15000889150008751500088115000860 +:104020006D1500085915000845150008951500087C +:1040300000000000711600085D16000899160008BF +:1040400085160008911600087D16000869160008FC +:1040500055160008A5160008000000000100000029 +:10406000000000006D61696E0000000069646C650D +:10407000000000006C40000878280020F029002093 +:1040800001000000D51F00080000000041726475A7 +:1040900050696C6F740025424F415244252D424CAB +:1040A000002553455249414C250000000200000004 +:1040B00000000000911800080119000840004000AD +:1040C00098420020A84200200200000000000000EA +:1040D0000300000000000000491900080000000073 +:1040E00010000000B84200200000000001000000A5 +:1040F000000000002C45002001010200912300086F +:10410000A12200083D2300082123000843000000ED +:104110001441000809024300020100C032090400F2 +:104120000001020201000524001001052401000124 +:10413000042402020524060001070582030800FF8B +:1041400009040100020A0000000705010240000006 +:1041500007058102400000001200000060410008D5 +:104160001201100102000040091241570002010231 +:10417000030100000403090425424F415244250075 +:1041800042656173744837763200303132333435EA +:1041900036373839414243444546000000000000AC +:1041A0009D1A0008551D0008011E0008400040002F +:1041B0006446002064460020010000007446002090 +:1041C0008000000040010000080000000001000025 +:1041D00000040000080000000001A86A00000000C0 +:1041E000AAAAAAAA00011464FFFF000000000000B0 +:1041F00070A70A000000000000000000AAAAAAAAF6 +:1042000000000000FFFF00000000000000000000B0 +:104210000000000400000000AAAAAAAA00000000F2 +:10422000FFDF0000000000000000000000000000B0 +:1042300000000000AAAAAAAA00000000FFFF0000D8 +:10424000000000000000000000010000000000006D +:10425000AAAAAAAA00010000FFFF000000000000B7 +:10426000000000000000000000000000AAAAAAAAA6 +:1042700000000000FFFF0000000000000000000040 +:104280000000000000000000AAAAAAAA0000000086 +:10429000FFFF000000000000000000000000000020 +:1042A00000000000AAAAAAAA00000000FFFF000068 +:1042B00000000000000000000000000000000000FE +:1042C000AAAAAAAA00000000FFFF00000000000048 +:1042D000000000000000000000000000AAAAAAAA36 +:1042E00000000000FFFF00000000000000000000D0 +:1042F0000000000000000000AAAAAAAA0000000016 +:10430000FFFF0000000000000000000000000000AF +:10431000200400000000000000001E00000000005B +:10432000FF00000000000000CC3F00083F0000003C +:1043300050040000D73F00083F0000000096000036 +:1043400000000800960000000008000004000000C3 +:1043500074410008000000000000000000000000A0 +:0C43600000000000000000000000000051 :00000001FF From 9c4188d7301b877e87555970ccc1795cf9ee74ca Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 1 Mar 2022 17:38:29 +0000 Subject: [PATCH 0061/3101] AP_Airspeed: Health: use reading from correct airspeed sensor --- libraries/AP_Airspeed/AP_Airspeed_Health.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index 4ee9a90e287..432cc801ac7 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -28,8 +28,7 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i) return; } - const float aspeed = get_airspeed(); - if (aspeed <= 0) { + if (state[i].airspeed <= 0) { // invalid estimate return; } @@ -44,7 +43,7 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i) } return; } - const float speed_diff = fabsf(aspeed-gps.ground_speed()); + const float speed_diff = fabsf(state[i].airspeed-gps.ground_speed()); // update health_probability with LowPassFilter if (speed_diff > _wind_max) { From 1435fdcf97a9194abca4d31b27b73990900677f9 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 1 Mar 2022 17:44:13 +0000 Subject: [PATCH 0062/3101] AP_Logger: log structure: update airspeed heath probability feild name --- libraries/AP_Logger/LogStructure.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index aacdb5e8ef8..810e2469714 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -769,7 +769,7 @@ struct PACKED log_VER { // @Field: Offset: Offset from parameter // @Field: U: True if sensor is being used // @Field: H: True if sensor is healthy -// @Field: Hfp: Probability sensor has failed +// @Field: Hp: Probability sensor is healthy // @Field: Pri: True if sensor is the primary sensor // @LoggerMessage: BCN @@ -1303,7 +1303,7 @@ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \ LOG_STRUCTURE_FROM_CAMERA \ - { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBfB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hfp,Pri", "s#nPOPP----", "F-00B00----", true }, \ + { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBfB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,Pri", "s#nPOPP----", "F-00B00----", true }, \ LOG_STRUCTURE_FROM_BATTMONITOR \ { LOG_MAG_MSG, sizeof(log_MAG), \ "MAG", "QBhhhhhhhhhBI", "TimeUS,I,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOX,MOY,MOZ,Health,S", "s#GGGGGGGGG-s", "F-CCCCCCCCC-F", true }, \ From 72a65cadb866f52d56f1b76a5075faab5f287b29 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 1 Mar 2022 17:44:43 +0000 Subject: [PATCH 0063/3101] AP_Airspeed: rename get_health_failure_probability to get_health_probability --- libraries/AP_Airspeed/AP_Airspeed.cpp | 2 +- libraries/AP_Airspeed/AP_Airspeed.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 11a5c1528a1..12a03c47203 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -689,7 +689,7 @@ void AP_Airspeed::Log_Airspeed() offset : get_offset(i), use : use(i), healthy : healthy(i), - health_prob : get_health_failure_probability(i), + health_prob : get_health_probability(i), primary : get_primary() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index a4790d484b1..6bdd86f65cb 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -266,12 +266,12 @@ class AP_Airspeed // returns 0 if the sensor is not enabled float get_pressure(uint8_t i); - // get the failure health probability - float get_health_failure_probability(uint8_t i) const { + // get the health probability + float get_health_probability(uint8_t i) const { return state[i].failures.health_probability; } - float get_health_failure_probability(void) const { - return get_health_failure_probability(primary); + float get_health_probability(void) const { + return get_health_probability(primary); } void update_calibration(uint8_t i, float raw_pressure); From 1cc2e082e00d38bf12f7c5fa1969cf4257738109 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:08:26 +0000 Subject: [PATCH 0064/3101] AP_AHRS: remove custom rotations --- libraries/AP_AHRS/AP_AHRS.cpp | 8 ++++---- libraries/AP_AHRS/AP_AHRS.h | 8 -------- libraries/AP_AHRS/AP_AHRS_Backend.cpp | 10 ++-------- 3 files changed, 6 insertions(+), 20 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 39932b3bcde..1cba5089a34 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -115,7 +115,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Param: ORIENTATION // @DisplayName: Board Orientation // @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. The label for each option is specified in the order of rotations for that orientation. This option takes affect on next boot. After changing you will need to re-level your vehicle. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0), @@ -154,7 +154,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced - AP_GROUPINFO("CUSTOM_ROLL", 15, AP_AHRS, _custom_roll, 0), + // index 15 // @Param: CUSTOM_PIT // @DisplayName: Board orientation pitch offset @@ -163,7 +163,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced - AP_GROUPINFO("CUSTOM_PIT", 16, AP_AHRS, _custom_pitch, 0), + // index 16 // @Param: CUSTOM_YAW // @DisplayName: Board orientation yaw offset @@ -172,7 +172,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced - AP_GROUPINFO("CUSTOM_YAW", 17, AP_AHRS, _custom_yaw, 0), + // index 17 AP_GROUPEND }; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f62b2c2cf99..c2da9f2185b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -637,14 +637,6 @@ class AP_AHRS { AP_Int8 _wind_max; AP_Int8 _board_orientation; AP_Int8 _ekf_type; - AP_Float _custom_roll; - AP_Float _custom_pitch; - AP_Float _custom_yaw; - - /* - * support for custom AHRS orientation, replacing _board_orientation - */ - Matrix3f _custom_rotation; /* * DCM-backend parameters; it takes references to these diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 202605c955c..42d7e2562f3 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -70,14 +70,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ void AP_AHRS::update_orientation() { const enum Rotation orientation = (enum Rotation)_board_orientation.get(); - if (orientation != ROTATION_CUSTOM) { - AP::ins().set_board_orientation(orientation); - AP::compass().set_board_orientation(orientation); - } else { - _custom_rotation.from_euler(radians(_custom_roll), radians(_custom_pitch), radians(_custom_yaw)); - AP::ins().set_board_orientation(orientation, &_custom_rotation); - AP::compass().set_board_orientation(orientation, &_custom_rotation); - } + AP::ins().set_board_orientation(orientation); + AP::compass().set_board_orientation(orientation); } // return a ground speed estimate in m/s From 209ad965be6d75981058d0879bd9d7f51c365ecf Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:11:09 +0000 Subject: [PATCH 0065/3101] AP_Compass: remove custom rotations --- libraries/AP_Compass/AP_Compass.cpp | 14 +++++------ libraries/AP_Compass/AP_Compass.h | 14 +---------- libraries/AP_Compass/AP_Compass_Backend.cpp | 25 +------------------ .../AP_Compass/AP_Compass_Calibration.cpp | 2 +- libraries/AP_Compass/AP_Compass_SITL.cpp | 7 +----- 5 files changed, 10 insertions(+), 52 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index f7461775080..9a087c70a7d 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -161,7 +161,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT // @DisplayName: Compass orientation // @Description: The orientation of the first external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. The label for each option is specified in the order of rotations for that orientation. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE), @@ -321,7 +321,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT2 // @DisplayName: Compass2 orientation // @Description: The orientation of a second external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE), @@ -344,7 +344,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: ORIENT3 // @DisplayName: Compass3 orientation // @Description: The orientation of a third external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. - // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,42:Roll45,43:Roll315,100:Custom + // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 // @User: Advanced AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE), @@ -632,7 +632,6 @@ const AP_Param::GroupInfo Compass::var_info[] = { AP_GROUPINFO("DEV_ID8", 48, Compass, extra_dev_id[4], 0), #endif // COMPASS_MAX_UNREG_DEV -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) // @Param: CUS_ROLL // @DisplayName: Custom orientation roll offset // @Description: Compass mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM. @@ -641,7 +640,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_ROLL", 49, Compass, _custom_roll, 0), + // index 49 // @Param: CUS_PIT // @DisplayName: Custom orientation pitch offset @@ -651,7 +650,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_PIT", 50, Compass, _custom_pitch, 0), + // index 50 // @Param: CUS_YAW // @DisplayName: Custom orientation yaw offset @@ -661,8 +660,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced - AP_GROUPINFO("CUS_YAW", 51, Compass, _custom_yaw, 0), -#endif + // index 51 AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 994ea718324..0acfc7e9e35 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -237,9 +237,8 @@ friend class AP_Compass_Backend; bool auto_declination_enabled() const { return _auto_declination != 0; } // set overall board orientation - void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) { + void set_board_orientation(enum Rotation orientation) { _board_orientation = orientation; - _custom_rotation = custom_rotation; } /// Set the motor compensation type @@ -456,12 +455,6 @@ friend class AP_Compass_Backend; // board orientation from AHRS enum Rotation _board_orientation = ROTATION_NONE; - // custom board rotation matrix - Matrix3f* _custom_rotation; - - // custom external compass rotation matrix - Matrix3f* _custom_external_rotation; - // declination in radians AP_Float _declination; @@ -478,11 +471,6 @@ friend class AP_Compass_Backend; // automatic compass orientation on calibration AP_Int8 _rotate_auto; - // custom compass rotation - AP_Float _custom_roll; - AP_Float _custom_pitch; - AP_Float _custom_yaw; - // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation float _thr; diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index ae411082216..71bb464bba8 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -22,24 +22,10 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) mag.rotate(state.rotation); if (!state.external) { - // and add in AHRS_ORIENTATION setting if not an external compass - if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) { - mag = *_compass._custom_rotation * mag; - } else { - mag.rotate(_compass._board_orientation); - } + mag.rotate(_compass._board_orientation); } else { // add user selectable orientation -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) - Rotation rotation = Rotation(state.orientation.get()); - if (rotation == ROTATION_CUSTOM && _compass._custom_external_rotation) { - mag = *_compass._custom_external_rotation * mag; - } else { - mag.rotate(rotation); - } -#else mag.rotate((enum Rotation)state.orientation.get()); -#endif } } @@ -233,15 +219,6 @@ bool AP_Compass_Backend::is_external(uint8_t instance) void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation) { _compass._state[Compass::StateIndex(instance)].rotation = rotation; -#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) - // lazily create the custom rotation matrix - if (!_compass._custom_external_rotation && Rotation(_compass._state[Compass::StateIndex(instance)].orientation.get()) == ROTATION_CUSTOM) { - _compass._custom_external_rotation = new Matrix3f(); - if (_compass._custom_external_rotation) { - _compass._custom_external_rotation->from_euler(radians(_compass._custom_roll), radians(_compass._custom_pitch), radians(_compass._custom_yaw)); - } - } -#endif } static constexpr float FILTER_KOEF = 0.1f; diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index eb7e2909ca3..95d432896c6 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -82,7 +82,7 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) if (_rotate_auto) { enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE; - if (r != ROTATION_CUSTOM) { + if (r < ROTATION_MAX) { _calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2, _rotate_auto>=3); } } diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index 27401637203..7f04ab534ee 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -128,12 +128,7 @@ void AP_Compass_SITL::_timer() Vector3f f = (_eliptical_corr * new_mag_data) - _sitl->mag_ofs[i].get(); // rotate compass f.rotate_inverse((enum Rotation)_sitl->mag_orient[i].get()); - // and add in AHRS_ORIENTATION setting if not an external compass - if (get_board_orientation() == ROTATION_CUSTOM) { - f = _sitl->ahrs_rotation * f; - } else { - f.rotate(get_board_orientation()); - } + f.rotate(get_board_orientation()); // scale the compass to simulate sensor scale factor errors f *= _sitl->mag_scaling[i]; From b77476caa12d1d5ac03fdf6c6ebbf9b516887d11 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:11:46 +0000 Subject: [PATCH 0066/3101] AP_InertialSensor: remove custom orentations --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 12 ++---------- libraries/AP_InertialSensor/AP_InertialSensor.h | 4 +--- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 12 ++---------- 3 files changed, 5 insertions(+), 23 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e9834911348..b19b2614ad0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -2133,11 +2133,7 @@ bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vec return false; } _accel_calibrator[_acc_body_aligned-1].get_sample_corrected(sample_num, ret); - if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) { - ret = *_custom_rotation * ret; - } else { - ret.rotate(_board_orientation); - } + ret.rotate(_board_orientation); return true; } @@ -2162,11 +2158,7 @@ bool AP_InertialSensor::get_primary_accel_cal_sample_avg(uint8_t sample_num, Vec } avg /= count; ret = avg; - if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) { - ret = *_custom_rotation * ret; - } else { - ret.rotate(_board_orientation); - } + ret.rotate(_board_orientation); return true; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index f4618664fc4..fbbceff9dad 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -224,9 +224,8 @@ class AP_InertialSensor : AP_AccelCal_Client static const struct AP_Param::GroupInfo var_info[]; // set overall board orientation - void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) { + void set_board_orientation(enum Rotation orientation) { _board_orientation = orientation; - _custom_rotation = custom_rotation; } // return the selected loop rate at which samples are made avilable @@ -595,7 +594,6 @@ class AP_InertialSensor : AP_AccelCal_Client // board orientation from AHRS enum Rotation _board_orientation; - Matrix3f* _custom_rotation; // per-sensor orientation to allow for board type defaults at runtime enum Rotation _gyro_orientation[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index e0c8dc21ad0..4177f9d4e90 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -126,11 +126,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect } // rotate to body frame - if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) { - accel = *_imu._custom_rotation * accel; - } else { - accel.rotate(_imu._board_orientation); - } + accel.rotate(_imu._board_orientation); } void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) @@ -155,11 +151,7 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto gyro -= _imu._gyro_offset[instance]; } - if (_imu._board_orientation == ROTATION_CUSTOM && _imu._custom_rotation) { - gyro = *_imu._custom_rotation * gyro; - } else { - gyro.rotate(_imu._board_orientation); - } + gyro.rotate(_imu._board_orientation); } /* From 7a6f57ccf1c8d593c346e214829b1e1bc417f96b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:12:24 +0000 Subject: [PATCH 0067/3101] AP_Math: add support for AP_CustomRotations --- libraries/AP_Math/quaternion.cpp | 15 +++++++++++---- libraries/AP_Math/quaternion.h | 2 ++ libraries/AP_Math/rotations.h | 7 +++++-- libraries/AP_Math/tests/test_math.cpp | 4 ++-- libraries/AP_Math/vector3.cpp | 13 ++++++++++--- 5 files changed, 30 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 93918818b5d..99d9c0824e2 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -18,8 +18,11 @@ #pragma GCC optimize("O2") +#include "quaternion.h" #include "AP_Math.h" #include +#include +#include #define HALF_SQRT_2_PlUS_SQRT_2 0.92387953251128673848313610506011 // sqrt(2 + sqrt(2)) / 2 #define HALF_SQRT_2_MINUS_SQTR_2 0.38268343236508972626808144923416 // sqrt(2 - sqrt(2)) / 2 @@ -376,12 +379,16 @@ void QuaternionT::from_rotation(enum Rotation rotation) q3 = q4 = 0.0; return; - case ROTATION_CUSTOM: - // Error; custom rotations not supported - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + case ROTATION_CUSTOM_1: + case ROTATION_CUSTOM_2: +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // Do not support custom rotations on Periph + AP::custom_rotations().from_rotation(rotation, *this); return; - +#endif case ROTATION_MAX: + case ROTATION_CUSTOM_OLD: + case ROTATION_CUSTOM_END: break; } // rotation invalid diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index e73eeaa2976..ad09ef583b5 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -17,6 +17,8 @@ // Refactored by Jonathan Challinger #pragma once +#include "definitions.h" +#include "matrix3.h" #include #if MATH_CHECK_INDEXES #include diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index d7b4dabbbaa..042ac090337 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -76,7 +76,10 @@ enum Rotation : uint8_t { // to the MAVLink messages as well. /////////////////////////////////////////////////////////////////////// ROTATION_MAX, - ROTATION_CUSTOM = 100, + ROTATION_CUSTOM_OLD = 100, + ROTATION_CUSTOM_1 = 101, + ROTATION_CUSTOM_2 = 102, + ROTATION_CUSTOM_END, }; @@ -87,5 +90,5 @@ enum Rotation : uint8_t { Here are the same values in a form suitable for a @Values attribute in auto documentation: -@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,42:Roll45,43:Roll315,100:Custom +@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2 */ diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index bdd4e2ec4f6..775f21b7ae7 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -83,11 +83,11 @@ TEST(VectorTest, Rotations) EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested"; #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX - TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1); + TEST_ROTATION(ROTATION_CUSTOM_OLD, 1, 1, 1); TEST_ROTATION(ROTATION_MAX, 1, 1, 1); #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL Vector3F v {1, 1, 1}; - EXPECT_EXIT(v.rotate(ROTATION_CUSTOM), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::flow_of_ctrl"); + EXPECT_EXIT(v.rotate(ROTATION_CUSTOM_OLD), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation"); EXPECT_EXIT(v.rotate(ROTATION_MAX), testing::KilledBySignal(SIGABRT), "AP_InternalError::error_t::bad_rotation"); #endif } diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 95864cca7fc..e13ab69a27f 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -20,6 +20,8 @@ #include "AP_Math.h" #include +#include +#include // rotate a vector by a standard rotation, attempting // to use the minimum number of floating point operations @@ -255,11 +257,16 @@ void Vector3::rotate(enum Rotation rotation) y = tmp; return; } - case ROTATION_CUSTOM: - // Error: caller must perform custom rotations via matrix multiplication - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + case ROTATION_CUSTOM_1: + case ROTATION_CUSTOM_2: +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // Do not support custom rotations on Periph + AP::custom_rotations().rotate(rotation, *this); return; +#endif case ROTATION_MAX: + case ROTATION_CUSTOM_OLD: + case ROTATION_CUSTOM_END: break; } // rotation invalid From f15c84bdacf1a298f1e947aa07509fa2c4250e9d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:13:03 +0000 Subject: [PATCH 0068/3101] AP_Vehicle: add custom rotations lib --- libraries/AP_Vehicle/AP_Vehicle.cpp | 6 ++++++ libraries/AP_Vehicle/AP_Vehicle.h | 3 +++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 338081b6c9b..d0b37269b9d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -73,6 +73,10 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(airspeed, "ARSPD", 10, AP_Vehicle, AP_Airspeed), #endif + // @Group: CUST_ROT + // @Path: ../AP_CustomRotations/AP_CustomRotations.cpp + AP_SUBGROUPINFO(custom_rotations, "CUST_ROT", 11, AP_Vehicle, AP_CustomRotations), + AP_GROUPEND }; @@ -204,6 +208,8 @@ void AP_Vehicle::setup() efi.init(); #endif + custom_rotations.init(); + gcs().send_text(MAV_SEVERITY_INFO, "ArduPilot Ready"); } diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 2d0b0bf7861..4df022eb8b3 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -51,6 +51,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif +#include class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -413,6 +414,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { bool done_safety_init; uint32_t _last_internal_errors; // backup of AP_InternalError::internal_errors bitmask + + AP_CustomRotations custom_rotations; }; namespace AP { From b188d8fc5ea1d7c9898b708f5f22f6d4df57b111 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:13:20 +0000 Subject: [PATCH 0069/3101] SITL: remove custom rotations --- libraries/SITL/SIM_Aircraft.cpp | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 2f8464448a5..b1bdf65988c 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -418,26 +418,9 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) if (ahrs_orientation != nullptr) { enum Rotation imu_rotation = (enum Rotation)ahrs_orientation->get(); if (imu_rotation != last_imu_rotation) { - // Surprisingly, Matrix3::from_rotation(ROTATION_CUSTOM) is the identity matrix - // so we must deal with that here - if (imu_rotation == ROTATION_CUSTOM) { - if ((custom_roll == nullptr) || (custom_pitch == nullptr) || (custom_yaw == nullptr)) { - enum ap_var_type ptype; - custom_roll = (AP_Float *)AP_Param::find("AHRS_CUSTOM_ROLL", &ptype); - custom_pitch = (AP_Float *)AP_Param::find("AHRS_CUSTOM_PIT", &ptype); - custom_yaw = (AP_Float *)AP_Param::find("AHRS_CUSTOM_YAW", &ptype); - } - if ((custom_roll != nullptr) && (custom_pitch != nullptr) && (custom_yaw != nullptr)) { - sitl->ahrs_rotation.from_euler(radians(*custom_roll), radians(*custom_pitch), radians(*custom_yaw)); - sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed(); - } else { - AP_HAL::panic("could not find one or more of parameters AHRS_CUSTOM_ROLL/PITCH/YAW"); - } - } else { - sitl->ahrs_rotation.from_rotation(imu_rotation); - sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed(); - last_imu_rotation = imu_rotation; - } + sitl->ahrs_rotation.from_rotation(imu_rotation); + sitl->ahrs_rotation_inv = sitl->ahrs_rotation.transposed(); + last_imu_rotation = imu_rotation; } if (imu_rotation != ROTATION_NONE) { Matrix3f m = dcm; From 480cc3fa46b256973055a3f3edbdfe1ff27e0d04 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:13:45 +0000 Subject: [PATCH 0070/3101] AP_AHRS: convert param to new custom rotation --- libraries/AP_AHRS/AP_AHRS.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 1cba5089a34..1409af24deb 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10) #define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20) @@ -154,6 +155,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced + // index 15 // @Param: CUSTOM_PIT @@ -163,6 +165,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced + // index 16 // @Param: CUSTOM_YAW @@ -172,6 +175,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // @Units: deg // @Increment: 1 // @User: Advanced + // index 17 AP_GROUPEND @@ -227,6 +231,26 @@ void AP_AHRS::init() #if HAL_NMEA_OUTPUT_ENABLED _nmea_out = AP_NMEA_Output::probe(); #endif + +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + // convert to new custom rotaton + // PARAMETER_CONVERSION - Added: Nov-2021 + if (_board_orientation == ROTATION_CUSTOM_OLD) { + _board_orientation.set_and_save(ROTATION_CUSTOM_1); + AP_Param::ConversionInfo info; + if (AP_Param::find_top_level_key_by_pointer(this, info.old_key)) { + info.type = AP_PARAM_FLOAT; + float rpy[3] = {}; + AP_Float rpy_param; + for (info.old_group_element=15; info.old_group_element<=17; info.old_group_element++) { + if (AP_Param::find_old_parameter(&info, &rpy_param)) { + rpy[info.old_group_element-15] = rpy_param.get(); + } + } + AP::custom_rotations().convert(ROTATION_CUSTOM_1, rpy[0], rpy[1], rpy[2]); + } + } +#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) } // updates matrices responsible for rotating vectors from vehicle body From 6c48c346c1481815708ce63ea8eec18bcf29a634 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 Nov 2021 16:13:55 +0000 Subject: [PATCH 0071/3101] AP_Compass: convert param to new custom rotation --- libraries/AP_Compass/AP_Compass.cpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 9a087c70a7d..e527286cfcc 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "AP_Compass_SITL.h" #include "AP_Compass_AK8963.h" @@ -640,6 +641,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced + // index 49 // @Param: CUS_PIT @@ -650,6 +652,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced + // index 50 // @Param: CUS_YAW @@ -660,7 +663,9 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Increment: 1 // @RebootRequired: True // @User: Advanced + // index 51 + AP_GROUPEND }; @@ -688,6 +693,30 @@ void Compass::init() return; } + // convert to new custom rotation method + // PARAMETER_CONVERSION - Added: Nov-2021 +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + for (StateIndex i(0); i 1 // Look if there was a primary compass setup in previous version // if so and the the primary compass is not set in current setup From 2b9421c4db67774b682f0fc7d3645e4e8252dd77 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 12 Nov 2021 21:33:18 +0000 Subject: [PATCH 0072/3101] add AP_CustomRotations --- .../AP_CustomRotations/AP_CustomRotations.cpp | 159 ++++++++++++++++++ .../AP_CustomRotations/AP_CustomRotations.h | 82 +++++++++ .../AP_CustomRotations_params.cpp | 36 ++++ 3 files changed, 277 insertions(+) create mode 100644 libraries/AP_CustomRotations/AP_CustomRotations.cpp create mode 100644 libraries/AP_CustomRotations/AP_CustomRotations.h create mode 100644 libraries/AP_CustomRotations/AP_CustomRotations_params.cpp diff --git a/libraries/AP_CustomRotations/AP_CustomRotations.cpp b/libraries/AP_CustomRotations/AP_CustomRotations.cpp new file mode 100644 index 00000000000..5123ab0b6ab --- /dev/null +++ b/libraries/AP_CustomRotations/AP_CustomRotations.cpp @@ -0,0 +1,159 @@ +#include "AP_CustomRotations.h" + +#include +#include +#include +#include + +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + +const AP_Param::GroupInfo AP_CustomRotations::var_info[] = { + + // @Param: _ENABLE + // @DisplayName: Enable Custom rotations + // @Values: 0:Disable, 1:Enable + // @Description: This enables custom rotations + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_CustomRotations, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Group: 1_ + // @Path: AP_CustomRotations_params.cpp + AP_SUBGROUPINFO(params[0], "1_", 2, AP_CustomRotations, AP_CustomRotation_params), + + // @Group: 2_ + // @Path: AP_CustomRotations_params.cpp + AP_SUBGROUPINFO(params[1], "2_", 3, AP_CustomRotations, AP_CustomRotation_params), + + AP_GROUPEND +}; + +AP_CustomRotation::AP_CustomRotation(AP_CustomRotation_params _params): + params(_params) +{ + init(); +} + +void AP_CustomRotation::init() +{ + m.from_euler(radians(params.roll),radians(params.pitch),radians(params.yaw)); + q.from_rotation_matrix(m); +} + +AP_CustomRotations::AP_CustomRotations() +{ + AP_Param::setup_object_defaults(this, var_info); + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (singleton != nullptr) { + AP_HAL::panic("AP_CustomRotations must be singleton"); + } +#endif + singleton = this; +} + +void AP_CustomRotations::init() +{ + if (enable == 0) { + return; + } + + // make sure all custom rotations are allocated + for (uint8_t i = 0; i < NUM_CUST_ROT; i++) { + AP_CustomRotation* rot = get_rotation(Rotation(i + ROTATION_CUSTOM_1)); + if (rot == nullptr) { + AP_BoardConfig::allocation_error("Custom Rotations"); + } + } +} + +void AP_CustomRotations::convert(Rotation r, float roll, float pitch, float yaw) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + if (!rot->params.roll.configured() && !rot->params.pitch.configured() && !rot->params.yaw.configured()) { + rot->params.roll.set_and_save(roll); + rot->params.pitch.set_and_save(pitch); + rot->params.yaw.set_and_save(yaw); + rot->init(); + } +} + +void AP_CustomRotations::set(Rotation r, float roll, float pitch, float yaw) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + rot->params.roll.set(roll); + rot->params.pitch.set(pitch); + rot->params.yaw.set(yaw); + rot->init(); +} + +void AP_CustomRotations::from_rotation(Rotation r, QuaternionD& q) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + q = rot->q.todouble(); +} + +void AP_CustomRotations::from_rotation(Rotation r, Quaternion& q) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + q = rot->q; +} + +void AP_CustomRotations::rotate(Rotation r, Vector3d& v) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + v = (rot->m * v.tofloat()).todouble(); +} + +void AP_CustomRotations::rotate(Rotation r, Vector3f& v) +{ + AP_CustomRotation* rot = get_rotation(r); + if (rot == nullptr) { + return; + } + v = rot->m * v; +} + +AP_CustomRotation* AP_CustomRotations::get_rotation(Rotation r) +{ + if (r < ROTATION_CUSTOM_1 || r >= ROTATION_CUSTOM_END) { + INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation); + return nullptr; + } + const uint8_t index = r - ROTATION_CUSTOM_1; + if (rotations[index] == nullptr) { + rotations[index] = new AP_CustomRotation(params[index]); + // make sure param is enabled if custom rotation is used + enable.set_and_save_ifchanged(1); + } + return rotations[index]; +} + +// singleton instance +AP_CustomRotations *AP_CustomRotations::singleton; + +namespace AP { + +AP_CustomRotations &custom_rotations() +{ + return *AP_CustomRotations::get_singleton(); +} + +} + +#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) diff --git a/libraries/AP_CustomRotations/AP_CustomRotations.h b/libraries/AP_CustomRotations/AP_CustomRotations.h new file mode 100644 index 00000000000..9f25d14a19f --- /dev/null +++ b/libraries/AP_CustomRotations/AP_CustomRotations.h @@ -0,0 +1,82 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include +#include + +#define NUM_CUST_ROT ROTATION_CUSTOM_END - ROTATION_CUSTOM_1 + +struct AP_CustomRotation_params { +public: + AP_CustomRotation_params(); + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Float roll; + AP_Float pitch; + AP_Float yaw; +}; + +class AP_CustomRotation { +public: + AP_CustomRotation(AP_CustomRotation_params _params); + + void init(); + + Quaternion q; + Matrix3f m; + + AP_CustomRotation_params ¶ms; +}; + +class AP_CustomRotations { +public: + AP_CustomRotations(); + + CLASS_NO_COPY(AP_CustomRotations); + + static AP_CustomRotations *get_singleton(void) { return singleton; } + + void init(); + + void from_rotation(enum Rotation r, QuaternionD &q); + void from_rotation(enum Rotation r, Quaternion &q); + + void rotate(enum Rotation r, Vector3d& v); + void rotate(enum Rotation r, Vector3f& v); + + void convert(Rotation r, float roll, float pitch, float yaw); + void set(Rotation r, float roll, float pitch, float yaw); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + AP_Int8 enable; + + AP_CustomRotation* get_rotation(Rotation r); + + AP_CustomRotation* rotations[NUM_CUST_ROT]; + + AP_CustomRotation_params params[NUM_CUST_ROT]; + + static AP_CustomRotations *singleton; +}; + +namespace AP { + AP_CustomRotations &custom_rotations(); +}; + diff --git a/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp b/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp new file mode 100644 index 00000000000..07d260d2cbc --- /dev/null +++ b/libraries/AP_CustomRotations/AP_CustomRotations_params.cpp @@ -0,0 +1,36 @@ +#include "AP_CustomRotations.h" +#include + +#if !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + +const AP_Param::GroupInfo AP_CustomRotation_params::var_info[] = { + + // @Param: ROLL + // @DisplayName: Custom roll + // @Description: Custom euler roll, euler 321 (yaw, pitch, roll) ordering + // @Units: deg + // @RebootRequired: True + AP_GROUPINFO("ROLL", 1, AP_CustomRotation_params, roll, 0), + + // @Param: PITCH + // @DisplayName: Custom pitch + // @Description: Custom euler pitch, euler 321 (yaw, pitch, roll) ordering + // @Units: deg + // @RebootRequired: True + AP_GROUPINFO("PITCH", 2, AP_CustomRotation_params, pitch, 0), + + // @Param: YAW + // @DisplayName: Custom yaw + // @Description: Custom euler yaw, euler 321 (yaw, pitch, roll) ordering + // @Units: deg + // @RebootRequired: True + AP_GROUPINFO("YAW", 3, AP_CustomRotation_params, yaw, 0), + + AP_GROUPEND +}; + +AP_CustomRotation_params::AP_CustomRotation_params() { + AP_Param::setup_object_defaults(this, var_info); +} + +#endif // !APM_BUILD_TYPE(APM_BUILD_AP_Periph) From 15326a8d902bf8f3bb66e7dbb9c51c78de37b775 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 20 Nov 2021 23:54:22 +0000 Subject: [PATCH 0073/3101] AP_Math: tests: define hal where requirerd for custom rotaitons --- libraries/AP_Math/tests/test_geodesic_grid.cpp | 2 ++ libraries/AP_Math/tests/test_matrix3.cpp | 2 ++ libraries/AP_Math/tests/test_perpendicular.cpp | 2 ++ libraries/AP_Math/tests/test_quaternion.cpp | 2 ++ libraries/AP_Math/tests/test_segment_intersection.cpp | 2 ++ libraries/AP_Math/tests/test_vector2.cpp | 2 ++ libraries/AP_Math/tests/test_vector3.cpp | 2 ++ 7 files changed, 14 insertions(+) diff --git a/libraries/AP_Math/tests/test_geodesic_grid.cpp b/libraries/AP_Math/tests/test_geodesic_grid.cpp index 8cf897d1315..e0e8b6e6235 100644 --- a/libraries/AP_Math/tests/test_geodesic_grid.cpp +++ b/libraries/AP_Math/tests/test_geodesic_grid.cpp @@ -20,6 +20,8 @@ #include "math_test.h" #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + class TestParam { public: /** diff --git a/libraries/AP_Math/tests/test_matrix3.cpp b/libraries/AP_Math/tests/test_matrix3.cpp index 3372134f60e..099368e68d1 100644 --- a/libraries/AP_Math/tests/test_matrix3.cpp +++ b/libraries/AP_Math/tests/test_matrix3.cpp @@ -17,6 +17,8 @@ #include "math_test.h" +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + // given we are in the Math library, you're epected to know what // you're doing when directly comparing floats: #pragma GCC diagnostic push diff --git a/libraries/AP_Math/tests/test_perpendicular.cpp b/libraries/AP_Math/tests/test_perpendicular.cpp index 8e9befc871a..8abfe6e7191 100644 --- a/libraries/AP_Math/tests/test_perpendicular.cpp +++ b/libraries/AP_Math/tests/test_perpendicular.cpp @@ -3,6 +3,8 @@ #include #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + #define EXPECT_VECTOR2F_EQ(v1, v2) \ do { \ EXPECT_FLOAT_EQ(v1[0], v2[0]); \ diff --git a/libraries/AP_Math/tests/test_quaternion.cpp b/libraries/AP_Math/tests/test_quaternion.cpp index 664307d9c70..3e878d3d63b 100644 --- a/libraries/AP_Math/tests/test_quaternion.cpp +++ b/libraries/AP_Math/tests/test_quaternion.cpp @@ -2,6 +2,8 @@ #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + // Tests that quaternion multiplication obeys Hamilton's quaternion multiplication convention // i*i == j*j == k*k == i*j*k == -1 TEST(QuaternionTest, QuaternionMultiplicationOfBases) { diff --git a/libraries/AP_Math/tests/test_segment_intersection.cpp b/libraries/AP_Math/tests/test_segment_intersection.cpp index 0be05162769..b1c741741eb 100644 --- a/libraries/AP_Math/tests/test_segment_intersection.cpp +++ b/libraries/AP_Math/tests/test_segment_intersection.cpp @@ -2,6 +2,8 @@ #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + #define EXPECT_VECTOR2F_EQ(v1, v2) \ do { \ EXPECT_FLOAT_EQ(v1[0], v2[0]); \ diff --git a/libraries/AP_Math/tests/test_vector2.cpp b/libraries/AP_Math/tests/test_vector2.cpp index e0b460a9cef..2125aabaca1 100644 --- a/libraries/AP_Math/tests/test_vector2.cpp +++ b/libraries/AP_Math/tests/test_vector2.cpp @@ -2,6 +2,8 @@ #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + TEST(Vector2Test, Operator) { Vector2f v_float0{1.0f, 1.0f}; diff --git a/libraries/AP_Math/tests/test_vector3.cpp b/libraries/AP_Math/tests/test_vector3.cpp index 2fffa64ddce..dac9662f4a4 100644 --- a/libraries/AP_Math/tests/test_vector3.cpp +++ b/libraries/AP_Math/tests/test_vector3.cpp @@ -2,6 +2,8 @@ #define ALLOW_DOUBLE_MATH_FUNCTIONS #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + TEST(Vector3Test, Operator) { Vector3f v_float0{1.0f, 1.0f,1.0f}; From 97602aadf59f63a1c459fc3b1c632aaa226eccc1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 20 Nov 2021 23:55:37 +0000 Subject: [PATCH 0074/3101] AP_Math: examples: test custom rotations --- .../AP_Math/examples/rotations/rotations.cpp | 39 +++++++++++++++++-- 1 file changed, 36 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Math/examples/rotations/rotations.cpp b/libraries/AP_Math/examples/rotations/rotations.cpp index fb3095890fc..e181597f04c 100644 --- a/libraries/AP_Math/examples/rotations/rotations.cpp +++ b/libraries/AP_Math/examples/rotations/rotations.cpp @@ -4,6 +4,9 @@ #include #include +#include + +AP_CustomRotations cust_rot; void setup(); void loop(); @@ -165,9 +168,9 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya float q_roll, q_pitch, q_yaw, qe_roll, qe_pitch, qe_yaw; q.to_euler(q_roll, q_pitch, q_yaw); qe.to_euler(qe_roll, qe_pitch, qe_yaw); - const float roll_diff = fabsf(wrap_PI(q_roll - qe_roll)); - const float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch)); - const float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw)); + float roll_diff = fabsf(wrap_PI(q_roll - qe_roll)); + float pitch_diff = fabsf(wrap_PI(q_pitch - qe_pitch)); + float yaw_diff = fabsf(wrap_PI(q_yaw - qe_yaw)); if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) { hal.console->printf("quaternion test %u failed : yaw:%f/%f roll:%f/%f pitch:%f/%f\n", (unsigned)rotation, @@ -175,6 +178,36 @@ static void test_euler(enum Rotation rotation, float roll, float pitch, float ya (double)q_roll,(double)qe_roll, (double)q_pitch,(double)qe_pitch); } + + // test custom rotations + AP::custom_rotations().set(ROTATION_CUSTOM_1, roll, pitch, yaw); + v1 = v; + v1.rotate(ROTATION_CUSTOM_1); + + diff = (v2 - v1); + if (diff.length() > accuracy) { + hal.console->printf("euler test %u failed : yaw:%d roll:%d pitch:%d\n", + (unsigned)rotation, + (int)yaw, + (int)roll, + (int)pitch); + hal.console->printf("custom rotated: "); + print_vector(v1); + hal.console->printf("correct rotated: "); + print_vector(v2); + hal.console->printf("\n"); + } + + Quaternion qc; + qc.from_rotation(ROTATION_CUSTOM_1); + float qc_roll, qc_pitch, qc_yaw; + qc.to_euler(qc_roll, qc_pitch, qc_yaw); + roll_diff = fabsf(wrap_PI(qc_roll - qe_roll)); + pitch_diff = fabsf(wrap_PI(qc_pitch - qe_pitch)); + yaw_diff = fabsf(wrap_PI(qc_yaw - qe_yaw)); + if ((roll_diff > q_accuracy) || (pitch_diff > q_accuracy) || (yaw_diff > q_accuracy)) { + hal.console->printf("custom quaternion test %u failed\n", (unsigned)rotation); + } } static void test_rotate_inverse(void) From 9ec8bb17b29f2aa3318762632ea54726005b756b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 21 Nov 2021 01:53:19 +0000 Subject: [PATCH 0075/3101] AP_Math: benchmarks: define hal --- libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp | 2 ++ libraries/AP_Math/benchmarks/benchmark_matrix.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp b/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp index 357998eb0b8..71da640b0a7 100644 --- a/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp +++ b/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp @@ -18,6 +18,8 @@ #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + static const Vector3f triangles[20][3] = { {{-M_GOLDEN, 1, 0}, {-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}}, {{-1, 0,-M_GOLDEN}, {-M_GOLDEN,-1, 0}, { 0,-M_GOLDEN,-1}}, diff --git a/libraries/AP_Math/benchmarks/benchmark_matrix.cpp b/libraries/AP_Math/benchmarks/benchmark_matrix.cpp index 01c04dd783c..d14b3cbbe0a 100644 --- a/libraries/AP_Math/benchmarks/benchmark_matrix.cpp +++ b/libraries/AP_Math/benchmarks/benchmark_matrix.cpp @@ -2,6 +2,8 @@ #include +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + static void BM_MatrixMultiplication(benchmark::State& state) { Matrix3f m1(Vector3f(1.0f, 2.0f, 3.0f), From 3e75283717245f62859c79f226117d6e6f27a6ae Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 22 Feb 2022 00:09:14 +0000 Subject: [PATCH 0076/3101] AP_Math: ftype: include float.h --- libraries/AP_Math/ftype.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Math/ftype.h b/libraries/AP_Math/ftype.h index 04cf0b50279..d2095df1047 100644 --- a/libraries/AP_Math/ftype.h +++ b/libraries/AP_Math/ftype.h @@ -5,6 +5,7 @@ */ #include +#include /* capital F is used to denote the chosen float type (float or double) From 6ac661c6cea2ef98fc15f8001f0039032bdd20c2 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 1 Mar 2022 12:39:40 +0000 Subject: [PATCH 0077/3101] Tools: ardupilotwaf: add AP_CustomRotations --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index d0513a28e5f..eb7452f1c92 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -109,6 +109,7 @@ 'AP_VideoTX', 'AP_FETtecOneWire', 'AP_Torqeedo', + 'AP_CustomRotations' ] def get_legacy_defines(sketch_name, bld): From d8fb6cc522120bd1b738efb9aef3ee0c4e33b541 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Feb 2022 11:27:47 +1100 Subject: [PATCH 0078/3101] AP_Scripting: added example for orbit follow allows a following vehicle to orbit the lead vehicle --- .../AP_Scripting/examples/orbit_follow.lua | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 libraries/AP_Scripting/examples/orbit_follow.lua diff --git a/libraries/AP_Scripting/examples/orbit_follow.lua b/libraries/AP_Scripting/examples/orbit_follow.lua new file mode 100644 index 00000000000..5629bbbe1a7 --- /dev/null +++ b/libraries/AP_Scripting/examples/orbit_follow.lua @@ -0,0 +1,44 @@ +-- example that does an orbit of a vehicle that is being followed +-- by adjusting the follow offset X and Y parameters +-- user settable FOLL_ORB_TIME and FOLL_ORB_RADIUS parameters +-- are provided for the time for one orbit (in seconds) and the radius +-- of the orbit + +-- setup param block for FOLL_ extensions +local PARAM_TABLE_KEY = 83 +local PARAM_TABLE_PREFIX = "FOLL_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table') + +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +function bind_add_param(name, index, default_value) + assert(param:add_param(PARAM_TABLE_KEY, index, name, default_value), string.format('could not add %s', PARAM_TABLE_PREFIX .. name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +local FOLL_OFS_X = bind_param("FOLL_OFS_X") +local FOLL_OFS_Y = bind_param("FOLL_OFS_Y") +local FOLL_ORB_RADIUS = bind_add_param("ORB_RADIUS", 1, 5) +local FOLL_ORB_TIME = bind_add_param("ORB_TIME", 2, 10) + +local t = 0 + +function update() + t = t + 0.1 + --gcs:send_text(0, string.format("t=%.2f X=%.2f Y=%.2f", t, FOLL_OFS_X:get(), FOLL_OFS_Y:get())) + if t > FOLL_ORB_TIME:get() then + t = t - FOLL_ORB_TIME:get() + end + FOLL_OFS_X:set(math.cos((t/FOLL_ORB_TIME:get())*2*math.pi)*FOLL_ORB_RADIUS:get()) + FOLL_OFS_Y:set(math.sin((t/FOLL_ORB_TIME:get())*2*math.pi)*FOLL_ORB_RADIUS:get()) + return update, 100 +end + +gcs:send_text(0, string.format("orbit started")) +return update, 100 + + From 54208574f0ee96b406a854069d787d34cfa6d5f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 22 Feb 2022 16:23:32 +1100 Subject: [PATCH 0079/3101] CI: use common ccache environment and enable CCACHE_SLOPPINESS=file_stat_matches --- .github/workflows/ccache.env | 10 ++++++++ .github/workflows/macos_build.yml | 8 +------ .github/workflows/test_chibios.yml | 8 +------ .github/workflows/test_coverage.yml | 8 +------ .github/workflows/test_linux_sbc.yml | 8 +------ .github/workflows/test_replay.yml | 8 +------ .github/workflows/test_sitl_copter.yml | 32 ++++--------------------- .github/workflows/test_sitl_periph.yml | 16 ++----------- .github/workflows/test_sitl_plane.yml | 16 ++----------- .github/workflows/test_sitl_rover.yml | 16 ++----------- .github/workflows/test_sitl_sub.yml | 16 ++----------- .github/workflows/test_sitl_tracker.yml | 16 ++----------- .github/workflows/test_size.yml | 10 -------- .github/workflows/test_unit_tests.yml | 8 +------ 14 files changed, 30 insertions(+), 150 deletions(-) create mode 100644 .github/workflows/ccache.env diff --git a/.github/workflows/ccache.env b/.github/workflows/ccache.env new file mode 100644 index 00000000000..2cae97776a7 --- /dev/null +++ b/.github/workflows/ccache.env @@ -0,0 +1,10 @@ +# common ccache env vars for CI +export CCACHE_SLOPPINESS=file_stat_matches + +mkdir -p ~/.ccache +echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf +echo "compression = true" >> ~/.ccache/ccache.conf +echo "compression_level = 6" >> ~/.ccache/ccache.conf +echo "max_size = 400M" >> ~/.ccache/ccache.conf +ccache -s +ccache -z diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index 5c5c891fde5..e142a796ace 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -42,13 +42,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{matrix.config}} # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test build ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index 76db9c91d9a..8f2d141781d 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -67,13 +67,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }} # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} ${{ matrix.toolchain }} gcc-${{matrix.gcc}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index e191f6d67e3..45749b21e8c 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -46,13 +46,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: Configure CAN if: ${{ matrix.config == 'sitltest-can'}} run: | diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index 7c1ef116888..e36a28fc3db 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -60,13 +60,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index 69ca68c5875..7ecae879f49 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -41,13 +41,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index bb99604bba6..237af064482 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -38,13 +38,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build copter ${{ matrix.toolchain }} shell: bash run: | @@ -96,13 +90,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} @@ -157,13 +145,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build heli shell: bash run: | @@ -203,13 +185,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 1a81791b70c..8a517adf873 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -31,13 +31,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: install 32-bit libraries run: | dpkg --add-architecture i386 @@ -90,13 +84,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: install 32-bit libraries run: | sudo dpkg --add-architecture i386 diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 9d4f7df05f0..3640bc4e902 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -38,13 +38,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build plane ${{ matrix.toolchain }} shell: bash run: | @@ -91,13 +85,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 1b0ee631534..b87437a3308 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -38,13 +38,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build rover ${{ matrix.toolchain }} shell: bash run: | @@ -91,13 +85,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 8d43b72d188..39c25c28b59 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -38,13 +38,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build sub ${{ matrix.toolchain }} shell: bash run: | @@ -90,13 +84,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index db9fc0586ac..8c6abec961c 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -38,13 +38,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: build tracker ${{ matrix.toolchain }} shell: bash run: | @@ -90,13 +84,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-base- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index 09db01cfd1c..a1ed220ddb6 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -43,16 +43,6 @@ jobs: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - name: Build master ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index db361317e6a..e745cae37be 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -46,13 +46,7 @@ jobs: restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}- # restore ccache from either previous build on this branch or on master - name: setup ccache run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z + . .github/workflows/ccache.env - name: test ${{matrix.config}} ${{ matrix.toolchain }} env: CI_BUILD_TARGET: ${{matrix.config}} From 11f7cce97893c1a75bb0e6b222591fe604427607 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 24 Feb 2022 16:34:07 +1100 Subject: [PATCH 0080/3101] AP_HAL_ChibiOS: allow specification of AUTOBUILD_TARGETS in hwdef files --- libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat | 2 ++ 5 files changed, 10 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat index de4e9e68c24..f7310ab52dc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat @@ -9,3 +9,5 @@ include ../CubeBlack/hwdef.dat # pull Solo's default parameters from /Tools/Frame_params # these are parameters the Solo requires for proper operation that are different from the 4 standard defaults. env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param' + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat index a1f9b08279d..b5bd97b349b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat @@ -51,3 +51,5 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES define HAL_OREO_LED_ENABLED 1 define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5 + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/hwdef.dat index b5214322ff4..c00287cb2b3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/hwdef.dat @@ -36,3 +36,5 @@ define HAL_WITH_RAMTRON 1 # use a longer frame gap detector for more robust SBUS with H12 controller define HAL_SBUS_FRAME_GAP 5000U + +AUTOBUILD_TARGETS Plane diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat index e30d74f8f54..cdd20aff026 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat @@ -128,3 +128,5 @@ define AP_PARAM_MAX_EMBEDDED_PARAM 8192 # Disable un-needed hardware drivers define HAL_WITH_ESC_TELEM 0 define AP_FETTEC_ONEWIRE_ENABLED 0 + +AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index ce2aa4eeb29..15430fe5cfc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -113,3 +113,5 @@ define AP_MOTORS_FRAME_QUAD_ENABLED 1 define COMPASS_MAX_SENSORS 1 define BARO_MAX_INSTANCES 1 define INS_MAX_INSTANCES 1 + +AUTOBUILD_TARGETS Copter From cb6f9ea6a1456510629c0b8b2f04532ee0587d19 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 24 Feb 2022 16:34:08 +1100 Subject: [PATCH 0081/3101] Tools: allow specification of AUTOBUILD_TARGETS in hwdef files --- Tools/scripts/board_list.py | 34 ++++++++++++++++++++++++++++++--- Tools/scripts/build_binaries.py | 26 +++++++++++-------------- 2 files changed, 42 insertions(+), 18 deletions(-) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index d823dffacd4..7306a0cdee5 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -14,6 +14,14 @@ class Board(object): def __init__(self, name): self.name = name self.is_ap_periph = False + self.autobuild_targets = [ + 'AntennaTracker', + 'Blimp', + 'Copter', + 'Heli', + 'Plane', + 'Rover', + ] class BoardList(object): @@ -77,6 +85,13 @@ def __init__(self): if re.match(r"^\s*env AP_PERIPH_HEAVY 1", line): board.is_ap_periph = 1 + # a hwdef can specify which vehicles this target is valid for: + match = re.match(r"AUTOBUILD_TARGETS\s*(.*)", line) + if match is not None: + board.autobuild_targets = [ + x.rstrip().lstrip().lower() for x in match.group(1).split(",") + ] + def read_hwdef(self, filepath): fh = open(filepath) ret = [] @@ -89,7 +104,7 @@ def read_hwdef(self, filepath): ret += [line] return ret - def find_autobuild_boards(self): + def find_autobuild_boards(self, build_target=None): ret = [] for board in self.boards: if board.is_ap_periph: @@ -136,7 +151,20 @@ def find_autobuild_boards(self): ret = filter(lambda x : x not in blacklist, ret) - return list(ret) + # if the caller has supplied a vehicle to limit to then we do that here: + if build_target is not None: + # Slow down: n^2 algorithm ahead + newret = [] + for x in ret: + for b in self.boards: + if b.name.lower() != x.lower(): + continue + if build_target.lower() not in [y.lower() for y in b.autobuild_targets]: + continue + newret.append(x) + ret = newret + + return sorted(list(ret)) def find_ap_periph_boards(self): blacklist = [ @@ -159,7 +187,7 @@ def find_ap_periph_boards(self): if x.name in blacklist: continue ret.append(x.name) - return list(ret) + return sorted(list(ret)) AUTOBUILD_BOARDS = BoardList().find_autobuild_boards() diff --git a/Tools/scripts/build_binaries.py b/Tools/scripts/build_binaries.py index d13e4fcd071..4af52ba655c 100755 --- a/Tools/scripts/build_binaries.py +++ b/Tools/scripts/build_binaries.py @@ -27,7 +27,8 @@ import gen_stable import build_binaries_history -from board_list import AUTOBUILD_BOARDS, AP_PERIPH_BOARDS +import board_list +from board_list import AP_PERIPH_BOARDS if sys.version_info[0] < 3: running_python3 = False @@ -67,6 +68,7 @@ class build_binaries(object): def __init__(self, tags): self.tags = tags self.dirty = False + self.board_list = board_list.BoardList() def progress(self, string): '''pretty-print progress''' @@ -535,18 +537,15 @@ def print_exception_caught(self, e, send_statustext=True): self.progress("Exception caught: %s" % self.get_exception_stacktrace(e)) - def common_boards(self): - '''returns list of boards common to all vehicles''' - return AUTOBUILD_BOARDS - def AP_Periph_boards(self): return AP_PERIPH_BOARDS def build_arducopter(self, tag): '''build Copter binaries''' + boards = [] - boards.extend(["skyviper-v2450", "aerofc-v1", "bebop", "CubeSolo", "CubeGreen-solo", "skyviper-journey"]) - boards.extend(self.common_boards()[:]) + boards.extend(["aerofc-v1", "bebop"]) + boards.extend(self.board_list.find_autobuild_boards('Copter')) self.build_vehicle(tag, "ArduCopter", boards, @@ -556,7 +555,7 @@ def build_arducopter(self, tag): def build_arduplane(self, tag): '''build Plane binaries''' - boards = self.common_boards()[:] + boards = self.board_list.find_autobuild_boards('Plane')[:] boards.append("disco") self.build_vehicle(tag, "ArduPlane", @@ -566,19 +565,17 @@ def build_arduplane(self, tag): def build_antennatracker(self, tag): '''build Tracker binaries''' - boards = self.common_boards()[:] self.build_vehicle(tag, "AntennaTracker", - boards, + self.board_list.find_autobuild_boards('Tracker')[:], "AntennaTracker", "antennatracker") def build_rover(self, tag): '''build Rover binaries''' - boards = self.common_boards() self.build_vehicle(tag, "Rover", - boards, + self.board_list.find_autobuild_boards('Rover')[:], "Rover", "ardurover") @@ -586,7 +583,7 @@ def build_ardusub(self, tag): '''build Sub binaries''' self.build_vehicle(tag, "ArduSub", - self.common_boards(), + self.board_list.find_autobuild_boards('Sub')[:], "Sub", "ardusub") @@ -601,10 +598,9 @@ def build_AP_Periph(self, tag): def build_blimp(self, tag): '''build Blimp binaries''' - boards = self.common_boards() self.build_vehicle(tag, "Blimp", - boards, + self.board_list.find_autobuild_boards('Blimp')[:], "Blimp", "blimp") From da4602b5d2a523245458c3dd75231242dd00ece1 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Wed, 26 Jan 2022 13:50:59 +1100 Subject: [PATCH 0082/3101] AP_SerialManager: Add support for High Latency MAVLink protocol --- libraries/AP_SerialManager/AP_SerialManager.cpp | 10 ++++++---- libraries/AP_SerialManager/AP_SerialManager.h | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 3aa3407b7c6..63a12b3eb47 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -136,7 +136,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 1_PROTOCOL // @DisplayName: Telem1 protocol selection // @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:MegaSquirt EFI, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency // @User: Standard // @RebootRequired: True AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, SerialProtocol_MAVLink2), @@ -461,6 +461,7 @@ void AP_SerialManager::init() case SerialProtocol_Console: case SerialProtocol_MAVLink: case SerialProtocol_MAVLink2: + case SerialProtocol_MAVLinkHL: uart->begin(map_baudrate(state[i].baud), AP_SERIALMANAGER_MAVLINK_BUFSIZE_RX, AP_SERIALMANAGER_MAVLINK_BUFSIZE_TX); @@ -676,7 +677,8 @@ AP_SerialManager::SerialProtocol AP_SerialManager::get_mavlink_protocol(mavlink_ uint8_t chan_idx = (uint8_t)(mav_chan - MAVLINK_COMM_0); for (uint8_t i=0; i Date: Wed, 26 Jan 2022 13:51:54 +1100 Subject: [PATCH 0083/3101] GCS_MAVLink: Add support for High Latency MAVLink protocol --- libraries/GCS_MAVLink/GCS.cpp | 11 +++++ libraries/GCS_MAVLink/GCS.h | 20 ++++++++- libraries/GCS_MAVLink/GCS_Common.cpp | 65 ++++++++++++++++++++++++--- libraries/GCS_MAVLink/GCS_MAVLink.cpp | 7 +++ 4 files changed, 95 insertions(+), 8 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index f25883b5f2c..e1971c52f22 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -119,6 +119,17 @@ void GCS::send_named_float(const char *name, float value) const (const char *)&packet); } +#if HAL_HIGH_LATENCY2_ENABLED +void GCS::enable_high_latency_connections(bool enabled) +{ + for (uint8_t i=0; iflags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; } - + +#if HAL_HIGH_LATENCY2_ENABLED + if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) { + is_high_latency_link = true; + } +#endif return true; } @@ -938,8 +944,9 @@ bool GCS_MAVLINK::should_send_message_in_delay_callback(const ap_message id) con // microseconds to return! switch (id) { - case MSG_HEARTBEAT: case MSG_NEXT_PARAM: + case MSG_HEARTBEAT: + case MSG_HIGH_LATENCY2: case MSG_AUTOPILOT_VERSION: return true; default: @@ -1405,7 +1412,7 @@ bool GCS_MAVLINK::set_ap_message_interval(enum ap_message id, uint16_t interval_ // mavlink work!) void GCS_MAVLINK::send_message(enum ap_message id) { - if (id == MSG_HEARTBEAT) { + if (id == MSG_HEARTBEAT || id == MSG_HIGH_LATENCY2) { save_signing_timestamp(false); // update the mask of all streaming channels if (is_streaming()) { @@ -1428,7 +1435,8 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, } if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && - AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) { + (AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2 || + AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLinkHL)) { // if we receive any MAVLink2 packets on a connection // currently sending MAVLink1 then switch to sending // MAVLink2 @@ -4354,6 +4362,12 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t result = handle_preflight_reboot(packet); break; +#if HAL_HIGH_LATENCY2_ENABLED + case MAV_CMD_CONTROL_HIGH_LATENCY: + result = handle_control_high_latency(packet); + break; +#endif // HAL_HIGH_LATENCY2_ENABLED + case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: { @@ -5695,7 +5709,15 @@ void GCS_MAVLINK::initialise_message_intervals_from_streamrates() for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) { initialise_message_intervals_for_stream(all_stream_entries[i].stream_id); } +#if HAL_HIGH_LATENCY2_ENABLED + if (!is_high_latency_link) { + set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000); + } else { + set_mavlink_message_id_interval(MAVLINK_MSG_ID_HIGH_LATENCY2, 5000); + } +#else set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000); +#endif } bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const @@ -5706,6 +5728,12 @@ bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint1 return true; } + if (id == MSG_HIGH_LATENCY2) { + // handle HL2 requests as a special case because HL2 is not "streamed" + interval = 5000; + return true; + } + #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED // a user can specify default rates in files, which are read close // to vehicle startup @@ -5941,7 +5969,7 @@ uint64_t GCS_MAVLINK::capabilities() const MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION; AP_SerialManager::SerialProtocol mavlink_protocol = AP::serialmanager().get_mavlink_protocol(chan); - if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) { + if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) { ret |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; } @@ -6103,4 +6131,29 @@ int8_t GCS_MAVLINK::high_latency_air_temperature() const return INT8_MIN; } + +/* + handle a MAV_CMD_CONTROL_HIGH_LATENCY command + + Enable or disable any marked (via SERIALn_PROTOCOL) high latency connections + */ +MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_long_t &packet) +{ + // high latency mode is enabled if param1=1 or disabled if param1=0 + if (is_equal(packet.param1, 0.0f)) { + gcs().enable_high_latency_connections(false); + } else if (is_equal(packet.param1, 1.0f)) { + gcs().enable_high_latency_connections(true); + } else { + return MAV_RESULT_FAILED; + } + + // send to all other mavlink components with same sysid + mavlink_command_long_t hl_msg{}; + hl_msg.command = MAV_CMD_CONTROL_HIGH_LATENCY; + hl_msg.param1 = packet.param1; + GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&hl_msg, sizeof(hl_msg)); + + return MAV_RESULT_ACCEPTED; +} #endif // HAL_HIGH_LATENCY2_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 0005207df09..ff141739852 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -94,6 +94,13 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) { return; } +#if HAL_HIGH_LATENCY2_ENABLED + // if it's a disabled high latency channel, don't send + GCS_MAVLINK *link = gcs().chan(chan); + if (!link->should_send()) { + return; + } +#endif if (gcs_alternative_active[chan]) { // an alternative protocol is active return; From 976e0ecc29c443b4a1d07136bbec4738ba3f9972 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 2 Mar 2022 12:35:37 -0700 Subject: [PATCH 0084/3101] SRV_Channel: Changing servo functions are now reboot required Changing from a PWM function to GPIO now requires a reboot. --- libraries/SRV_Channel/SRV_Channel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 0e5cf88f27c..10b7f4f50c3 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -69,6 +69,7 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Values{Copter}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,88:Winch,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted // @Values{Rover}: -1:GPIO,0:Disabled,1:RCPassThru,6:MountPan,7:MountTilt,8:MountRoll,9:MountOpen,10:CameraTrigger,12:Mount2Pan,13:Mount2Tilt,14:Mount2Roll,15:Mount2Open,22:SprayerPump,23:SprayerSpinner,26:GroundSteering,28:Gripper,33:Motor1,34:Motor2,35:Motor3,36:Motor4,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,70:Throttle,73:ThrottleLeft,74:ThrottleRight,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted // @User: Standard + // @RebootRequired: True AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), AP_GROUPEND From 799933572716e41cdc354e5fea099b5b7f6d2a52 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Mar 2022 12:38:24 +1100 Subject: [PATCH 0085/3101] hwdef: fixed buzzer on CUAV_GPS peripheral --- libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat index 4d59ab434e0..acb771bb7ca 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat @@ -152,3 +152,6 @@ define HAL_PERIPH_ENABLE_BARO # startup define HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY 0 define HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY 8 + +# also enable buzzer +define HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY 1 From 8b563836acd4110cc005a0eaee00524a42f3c1c5 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:51:00 +0000 Subject: [PATCH 0086/3101] AC_AutoTune: nuke clang warnings --- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 5e574638fa9..cf0fa5f3bee 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -201,15 +201,9 @@ class AC_AutoTune_Heli : public AC_AutoTune // flag for finding the peak of the gain response bool find_peak; - // updating angle P up yaw - // counter value of previous good frequency - uint8_t sp_prev_good_frq_cnt; - // updating rate P up // counter value of previous good frequency uint8_t rp_prev_good_frq_cnt; - // previous gain - float rp_prev_gain; // updating rate D up // counter value of previous good frequency From 578ca147b7556fd1e00c413cfdf15223e20dd944 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:51:26 +0000 Subject: [PATCH 0087/3101] AP_Common: add UNUSED_PRIVATE_MEMBER --- libraries/AP_Common/AP_Common.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index aad3adc3036..76db086c21b 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -33,6 +33,13 @@ // used to mark a function that may be unused in some builds #define UNUSED_FUNCTION __attribute__((unused)) +// used to mark an attribute that may be unused in some builds +#ifdef __clang__ +#define UNUSED_PRIVATE_MEMBER __attribute__((unused)) +#else +#define UNUSED_PRIVATE_MEMBER +#endif + // this can be used to optimize individual functions #define OPTIMIZE(level) __attribute__((optimize(level))) From 4ac73e4ffded7a710412fec1960e6c4cc198cf4a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:51:47 +0000 Subject: [PATCH 0088/3101] AP_ExternalAHRS: nuke clang warnings --- libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 2ca99717716..f788bba8016 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -49,7 +49,6 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { int8_t port_num; bool port_opened; uint32_t baudrate; - uint16_t rate; void update_thread(); bool check_uart(); From ac7ee37b207f1d788d5d907ac91908746e83dd29 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:52:22 +0000 Subject: [PATCH 0089/3101] AP_FETtecOneWire: nuke clang warnings --- libraries/AP_FETtecOneWire/AP_FETtecOneWire.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h index 56d30994225..031c9c3a6fb 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h @@ -193,7 +193,6 @@ class AP_FETtecOneWire : public AP_ESC_Telem_Backend }; uint32_t _min_fast_throttle_period_us; ///< minimum allowed fast-throttle command transmit period - uint32_t _last_not_running_warning_ms; ///< last time we warned the user their ESCs are stuffed int32_t _motor_mask; ///< an un-mutable copy of the _motor_mask_parameter taken before _init_done goes true int32_t _reverse_mask; ///< a copy of the _reverse_mask_parameter taken while not armed int32_t _running_mask; ///< a bitmask of the actively running ESCs From 6a31e4d8167eb8fc8ac4e401c7183dd83e119c52 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:53:21 +0000 Subject: [PATCH 0090/3101] AP_Frsky_Telem: nuke clang warnings --- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 6 +++--- libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 8f69544c607..a16b5c08e54 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -246,7 +246,7 @@ extern const AP_HAL::HAL& hal; bool AP_Frsky_SPortParser::should_process_packet(const uint8_t *packet, bool discard_duplicates) { // check for duplicate packets - if (discard_duplicates && _parse_state.last_packet != nullptr) { + if (discard_duplicates) { /* Note: the polling byte packet[0] should be ignored in the comparison because we might get the same packet with different polling bytes @@ -323,10 +323,10 @@ bool AP_Frsky_SPortParser::get_packet(AP_Frsky_SPort::sport_packet_t &sport_pack } const AP_Frsky_SPort::sport_packet_t sp { - _parse_state.rx_buffer[0], + { _parse_state.rx_buffer[0], _parse_state.rx_buffer[1], le16toh_ptr(&_parse_state.rx_buffer[2]), - le32toh_ptr(&_parse_state.rx_buffer[4]) + le32toh_ptr(&_parse_state.rx_buffer[4]) }, }; sport_packet = sp; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index cc82caa8709..3f7457f2954 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -857,10 +857,10 @@ bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint1 // queue only Uplink packets if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) { const AP_Frsky_SPort::sport_packet_t sp { - 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID + { 0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID frame, appid, - data + data } }; _SPort_bidir.rx_packet_queue.push_force(sp); From eb259971e03e7a147d4165e0875fb22bc650ad33 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:53:46 +0000 Subject: [PATCH 0091/3101] AP_Generator: nuke clang warnings --- libraries/AP_Generator/AP_Generator_RichenPower.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.h b/libraries/AP_Generator/AP_Generator_RichenPower.h index 3acf33566ca..d861ea19beb 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.h +++ b/libraries/AP_Generator/AP_Generator_RichenPower.h @@ -151,7 +151,7 @@ class AP_Generator_RichenPower : public AP_Generator_Backend uint8_t footermagic1; uint8_t footermagic2; }; - assert_storage_size _assert_storage_size_RichenPacket; + assert_storage_size _assert_storage_size_RichenPacket UNUSED_PRIVATE_MEMBER; union RichenUnion { uint8_t parse_buffer[70]; From 5a7f5844d4c902913235a54f9ac0bf80a57b78be Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:54:07 +0000 Subject: [PATCH 0092/3101] AP_HAL: nuke clang warnings --- libraries/AP_HAL/HAL.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index ae9a0861b7d..b6e5146c359 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -109,15 +109,15 @@ class AP_HAL::HAL { private: // the uartX ports must be contiguous in ram for the serial() method to work AP_HAL::UARTDriver* uartA; - AP_HAL::UARTDriver* uartB; - AP_HAL::UARTDriver* uartC; - AP_HAL::UARTDriver* uartD; - AP_HAL::UARTDriver* uartE; - AP_HAL::UARTDriver* uartF; - AP_HAL::UARTDriver* uartG; - AP_HAL::UARTDriver* uartH; - AP_HAL::UARTDriver* uartI; - AP_HAL::UARTDriver* uartJ; + AP_HAL::UARTDriver* uartB UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartC UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartD UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartE UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartF UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartG UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartH UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartI UNUSED_PRIVATE_MEMBER; + AP_HAL::UARTDriver* uartJ UNUSED_PRIVATE_MEMBER; public: AP_HAL::I2CDeviceManager* i2c_mgr; From 173a43e361bd59e3ec673c2fc9c28d75435e2b8f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:54:31 +0000 Subject: [PATCH 0093/3101] AP_Proximity: nuke clang warnings --- libraries/AP_Proximity/AP_Proximity_SITL.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index e0620169126..c80c976c3b8 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -31,9 +31,6 @@ class AP_Proximity_SITL : public AP_Proximity_Backend AP_Float *fence_alt_max; Location current_loc; - // latest sector updated - uint8_t last_sector; - // get distance in meters to fence in a particular direction in degrees (0 is forward, angles increase in the clockwise direction) bool get_distance_to_fence(float angle_deg, float &distance) const; From 72d01aa8da280d606b9aa1ee922b65c377c9109c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:54:49 +0000 Subject: [PATCH 0094/3101] SITL: nuke clang warnings --- libraries/SITL/SIM_Buzzer.h | 5 ----- libraries/SITL/SIM_EFI_MegaSquirt.h | 2 -- libraries/SITL/SIM_Helicopter.h | 3 --- libraries/SITL/SIM_MS5XXX.h | 4 ---- libraries/SITL/SIM_RichenPower.h | 4 +--- libraries/SITL/SIM_Scrimmage.cpp | 3 +-- libraries/SITL/SIM_Scrimmage.h | 2 -- libraries/SITL/SIM_Temperature_MCP9600.h | 1 - libraries/SITL/SIM_VectorNav.h | 4 ---- 9 files changed, 2 insertions(+), 26 deletions(-) diff --git a/libraries/SITL/SIM_Buzzer.h b/libraries/SITL/SIM_Buzzer.h index 74c23094720..1af1635b771 100644 --- a/libraries/SITL/SIM_Buzzer.h +++ b/libraries/SITL/SIM_Buzzer.h @@ -40,11 +40,6 @@ class Buzzer { AP_Int8 _enable; // enable buzzer sim AP_Int8 _pin; - bool was_on; - - uint32_t on_time; - - bool prep_done; }; } diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.h b/libraries/SITL/SIM_EFI_MegaSquirt.h index d32707874df..f957a296c75 100644 --- a/libraries/SITL/SIM_EFI_MegaSquirt.h +++ b/libraries/SITL/SIM_EFI_MegaSquirt.h @@ -44,8 +44,6 @@ class EFI_MegaSquirt : public SerialDevice { private: void send_table(); - uint32_t time_send_ms; - struct PACKED { uint16_t size; uint8_t command; diff --git a/libraries/SITL/SIM_Helicopter.h b/libraries/SITL/SIM_Helicopter.h index a2d757385ea..5845ae27709 100644 --- a/libraries/SITL/SIM_Helicopter.h +++ b/libraries/SITL/SIM_Helicopter.h @@ -62,7 +62,6 @@ class Helicopter : public Aircraft { float hover_throttle = 0.5f; float terminal_velocity = 80; float hover_lean = 3.2f; - float yaw_zero = 0.1f; float rotor_rot_accel = radians(20); float roll_rate_max = radians(1400); float pitch_rate_max = radians(1400); @@ -70,10 +69,8 @@ class Helicopter : public Aircraft { float rsc_setpoint = 0.8f; float izz = 0.2f; float tr_dist = 0.85f; - float tr_accel_max = 50.0f; //rad/s/s float cyclic_scalar = 7.2; // converts swashplate servo ouputs to cyclic blade pitch float thrust_scale; - float tail_thrust_scale; Vector2f _tpp_angle; float torque_scale; float torque_mpog; diff --git a/libraries/SITL/SIM_MS5XXX.h b/libraries/SITL/SIM_MS5XXX.h index 345722b2ba1..532fcdc1190 100644 --- a/libraries/SITL/SIM_MS5XXX.h +++ b/libraries/SITL/SIM_MS5XXX.h @@ -23,10 +23,6 @@ class MS5XXX : public I2CDevice // float pressure; // float temperature; - // time we last updated the measurements (simulated internal - // workings of sensor) - uint32_t last_update_ms; - void reset(); enum class Command : uint8_t { diff --git a/libraries/SITL/SIM_RichenPower.h b/libraries/SITL/SIM_RichenPower.h index 962eb3a6090..8b5eba90a46 100644 --- a/libraries/SITL/SIM_RichenPower.h +++ b/libraries/SITL/SIM_RichenPower.h @@ -71,8 +71,6 @@ class RichenPower : public SerialDevice { // So we set batt fs high 46s // Gennie keeps batts charged to 49v + typically - class SIM *_sitl; - uint32_t last_sent_ms; void update_control_pin(const struct sitl_input &input); @@ -122,7 +120,7 @@ class RichenPower : public SerialDevice { uint8_t footermagic1; uint8_t footermagic2; }; - assert_storage_size _assert_storage_size_RichenPacket; + assert_storage_size _assert_storage_size_RichenPacket UNUSED_PRIVATE_MEMBER; union RichenUnion { uint8_t parse_buffer[70]; diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp index 64b623094f2..e4d91548ac3 100644 --- a/libraries/SITL/SIM_Scrimmage.cpp +++ b/libraries/SITL/SIM_Scrimmage.cpp @@ -35,8 +35,7 @@ Scrimmage::Scrimmage(const char *_frame_str) : Aircraft(_frame_str), prev_timestamp_us(0), recv_sock(true), - send_sock(true), - frame_str(_frame_str) + send_sock(true) { } diff --git a/libraries/SITL/SIM_Scrimmage.h b/libraries/SITL/SIM_Scrimmage.h index 5887d3368f8..e6228cb281a 100644 --- a/libraries/SITL/SIM_Scrimmage.h +++ b/libraries/SITL/SIM_Scrimmage.h @@ -86,8 +86,6 @@ class Scrimmage : public Aircraft { uint64_t prev_timestamp_us; SocketAPM recv_sock; SocketAPM send_sock; - - const char *frame_str; }; } // namespace SITL diff --git a/libraries/SITL/SIM_Temperature_MCP9600.h b/libraries/SITL/SIM_Temperature_MCP9600.h index 29b60a3d6ea..d8109cbaed5 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.h +++ b/libraries/SITL/SIM_Temperature_MCP9600.h @@ -30,7 +30,6 @@ class MCP9600 : public I2CDevice, private I2CRegisters_ConfigurableLength // should be a call on aircraft: float some_temperature = 26.5; - float last_temperature = -1000.0f; uint32_t last_temperature_update_ms; }; diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index c072807da58..4e73f69478f 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -41,10 +41,6 @@ class VectorNav : public SerialDevice { void update(void); private: - // TODO: make these parameters: - const uint8_t system_id = 17; - const uint8_t component_id = 18; - uint32_t last_pkt1_us; uint32_t last_pkt2_us; From 11a5c8d2a442c5f7627871569d904d428cc57edf Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 26 Feb 2022 13:50:22 +0000 Subject: [PATCH 0095/3101] Copter: nuke clang warnings --- ArduCopter/Attitude.cpp | 2 +- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_guided.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index c823849d868..5d4660c51d4 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -28,7 +28,7 @@ void Copter::update_throttle_hover() // calc average throttle if we are in a level hover. accounts for heli hover roll trim if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z_up_cms()) < 60 && - labs(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) { + fabsf(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) { // Can we set the time constant automatically motors->update_throttle_hover(0.01f); #if HAL_GYROFFT_ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9d2d1a590ab..8e3a56c9f2a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1980,7 +1980,7 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) } // check if we have completed circling - return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); + return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1); } // verify_spline_wp - check if we have reached the next way point using spline diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 0a26f0aa14b..8ed8c8eacb0 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -684,7 +684,7 @@ void ModeGuided::pos_control_run() float pos_offset_z_buffer = 0.0; // Vertical buffer size in m if (guided_pos_terrain_alt) { - pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsf(guided_pos_target_cm.z)); + pos_offset_z_buffer = MIN(copter.wp_nav->get_terrain_margin() * 100.0, 0.5 * fabsF(guided_pos_target_cm.z)); } pos_control->input_pos_xyz(guided_pos_target_cm, terr_offset, pos_offset_z_buffer); From 99cf10ce1208fa12aa2528f788a4da66ff16eba8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Feb 2022 13:04:46 +1100 Subject: [PATCH 0096/3101] AP_Follow: support the FOLLOW_TARGET mavlink message this is used by qgroundcontrol, allowing follow on a mobile device note that you must set the qgc option to "always send follow" and also must set FOLL_ALT_TYPE=2 in ArduPilot --- libraries/AP_Follow/AP_Follow.cpp | 79 ++++++++++++++++++++++++++----- 1 file changed, 68 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index f1def162d25..4e996c49aa8 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -275,13 +275,10 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) } // decode global-position-int message - if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { - - // get estimated location and velocity (for logging) - Location loc_estimate{}; - Vector3f vel_estimate; - UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate)); + bool updated = false; + switch (msg.msgid) { + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode message mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(&msg, &packet); @@ -296,13 +293,11 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) // select altitude source based on FOLL_ALT_TYPE param if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) { - // relative altitude - _target_location.alt = packet.relative_alt / 10; // convert millimeters to cm - _target_location.relative_alt = 1; // set relative_alt flag + // above home alt + _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME); } else { // absolute altitude - _target_location.alt = packet.alt / 10; // convert millimeters to cm - _target_location.relative_alt = 0; // reset relative_alt flag + _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE); } _target_velocity_ned.x = packet.vx * 0.01f; // velocity north @@ -320,6 +315,68 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) _sysid.set(msg.sysid); _automatic_sysid = true; } + updated = true; + break; + } + case MAVLINK_MSG_ID_FOLLOW_TARGET: { + // decode message + mavlink_follow_target_t packet; + mavlink_msg_follow_target_decode(&msg, &packet); + + // ignore message if lat and lon are (exactly) zero + if ((packet.lat == 0 && packet.lon == 0)) { + return; + } + // require at least position + if ((packet.est_capabilities & (1<<0)) == 0) { + return; + } + + Location new_loc = _target_location; + new_loc.lat = packet.lat; + new_loc.lng = packet.lon; + new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE); + + // FOLLOW_TARGET is always AMSL, change the provided alt to + // above home if we are configured for relative alt + if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE && + !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) { + return; + } + _target_location = new_loc; + + if (packet.est_capabilities & (1<<1)) { + _target_velocity_ned.x = packet.vel[0]; // velocity north + _target_velocity_ned.y = packet.vel[1]; // velocity east + _target_velocity_ned.z = packet.vel[2]; // velocity down + } + + // get a local timestamp with correction for transport jitter + _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis()); + + if (packet.est_capabilities & (1<<3)) { + Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]}; + float r, p, y; + q.to_euler(r,p,y); + _target_heading = degrees(y); + _last_heading_update_ms = _last_location_update_ms; + } + + // initialise _sysid if zero to sender's id + if (_sysid == 0) { + _sysid.set(msg.sysid); + _automatic_sysid = true; + } + updated = true; + break; + } + } + + if (updated) { + // get estimated location and velocity + Location loc_estimate{}; + Vector3f vel_estimate; + UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate)); // log lead's estimated vs reported position // @LoggerMessage: FOLL From e6f488ccc11e2eb753c85e494304f29e09331754 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Mar 2022 08:53:38 +0900 Subject: [PATCH 0097/3101] AP_Follow: zreo velocities if not provided --- libraries/AP_Follow/AP_Follow.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 4e996c49aa8..a71d8eefa35 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -349,6 +349,8 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) _target_velocity_ned.x = packet.vel[0]; // velocity north _target_velocity_ned.y = packet.vel[1]; // velocity east _target_velocity_ned.z = packet.vel[2]; // velocity down + } else { + _target_velocity_ned.zero(); } // get a local timestamp with correction for transport jitter From 2e5c2012227d075deeb988edf2ff8d392a0f5251 Mon Sep 17 00:00:00 2001 From: xianglunkai <1322099375@qq.com> Date: Fri, 4 Mar 2022 10:45:34 +0800 Subject: [PATCH 0098/3101] AR_AttitudeControl: use _desired_speed instead of desired_speed for throttle-speed controller --- libraries/APM_Control/AR_AttitudeControl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 5de227c70c9..ca8aa2593db 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -635,11 +635,11 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor // calculate base throttle (protect against divide by zero) float throttle_base = 0.0f; if (is_positive(cruise_speed) && is_positive(cruise_throttle)) { - throttle_base = desired_speed * (cruise_throttle / cruise_speed); + throttle_base = _desired_speed * (cruise_throttle / cruise_speed); } // calculate final output - float throttle_out = _throttle_speed_pid.update_all(desired_speed, speed, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high)); + float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high)); throttle_out += _throttle_speed_pid.get_ff(); throttle_out += throttle_base; @@ -654,10 +654,10 @@ float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor // protect against reverse output being sent to the motors unless braking has been enabled if (!_brake_enable) { // if both desired speed and actual speed are positive, do not allow negative values - if ((desired_speed >= 0.0f) && (throttle_out <= 0.0f)) { + if ((_desired_speed >= 0.0f) && (throttle_out <= 0.0f)) { throttle_out = 0.0f; _throttle_limit_low = true; - } else if ((desired_speed <= 0.0f) && (throttle_out >= 0.0f)) { + } else if ((_desired_speed <= 0.0f) && (throttle_out >= 0.0f)) { throttle_out = 0.0f; _throttle_limit_high = true; } From 3fa12152f6f0cdb68172d45607483fb23420333a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 3 Mar 2022 14:54:55 +1100 Subject: [PATCH 0099/3101] autotest: add tests for high latency control protocol --- Tools/autotest/common.py | 264 ++++++++++++++++++++++++++++++--------- 1 file changed, 208 insertions(+), 56 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 0e86a51f454..fd60578c0e0 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1829,6 +1829,22 @@ def run_cmd_reboot(self): 0, 0) + def run_cmd_enable_high_latency(self, new_state): + p1 = 0 + if new_state: + p1 = 1 + + self.run_cmd( + mavutil.mavlink.MAV_CMD_CONTROL_HIGH_LATENCY, + p1, # p1 - enable/disable + 0, # p2 + 0, # p3 + 0, # p4 + 0, # p5 + 0, # p6 + 0, # p7 + ) + def reboot_sitl_mav(self, required_bootcount=None): """Reboot SITL instance using mavlink and wait for it to reconnect.""" # we must make sure that stats have been reset - otherwise @@ -2662,7 +2678,7 @@ def drain_mav_unparsed(self, mav=None, quiet=True, freshen_sim_time=False): count = 0 tstart = time.time() while True: - this = self.mav.recv(1000000) + this = mav.recv(1000000) if len(this) == 0: break count += len(this) @@ -2798,6 +2814,87 @@ def HIGH_LATENCY2(self): if m.temperature_air == -128: # High_Latency2 defaults to INT8_MIN for no temperature available raise NotAchievedException("Air Temperature not received from HIGH_LATENCY2") + self.HIGH_LATENCY2_links() + + def HIGH_LATENCY2_links(self): + + self.start_subtest("SerialProtocol_MAVLinkHL links") + + ex = None + self.context_push() + mav2 = None + try: + + self.set_parameter("SERIAL2_PROTOCOL", 43) # HL) + + self.reboot_sitl() + + mav2 = mavutil.mavlink_connection( + "tcp:localhost:5763", + robust_parsing=True, + source_system=7, + source_component=7, + ) + + self.start_subsubtest("Don't get HIGH_LATENCY2 by default") + for mav in self.mav, mav2: + self.assert_not_receive_message('HIGH_LATENCY2', mav=mav, timeout=10) + + self.start_subsubtest("Get HIGH_LATENCY2 upon link enabled only on HL link") + self.run_cmd_enable_high_latency(True) + self.assert_receive_message("HIGH_LATENCY2", mav=mav2, timeout=10) + self.assert_not_receive_message("HIGH_LATENCY2", mav=self.mav, timeout=10) + + self.start_subsubtest("Not get HIGH_LATENCY2 upon HL disable") + self.run_cmd_enable_high_latency(False) + self.delay_sim_time(10) + self.assert_not_receive_message('HIGH_LATENCY2', mav=self.mav, timeout=10) + self.drain_mav(mav2) + self.assert_not_receive_message('HIGH_LATENCY2', mav=mav2, timeout=10) + + self.start_subsubtest("Stream rate adjustments") + self.run_cmd_enable_high_latency(True) + self.assert_message_rate_hz("HIGH_LATENCY2", 0.2, ndigits=1, mav=mav2) + for test_rate in (1, 0.1, 2): + self.test_rate( + "HIGH_LATENCY2 on enabled link", + test_rate, + test_rate, + mav=mav2, + ndigits=1, + victim_message="HIGH_LATENCY2", + message_rate_sample_period=60, + ) + self.assert_not_receive_message("HIGH_LATENCY2", mav=self.mav, timeout=10) + self.run_cmd_enable_high_latency(False) + + self.start_subsubtest("Not get HIGH_LATENCY2 after disabling after playing with rates") + self.assert_not_receive_message('HIGH_LATENCY2', mav=self.mav, timeout=10) + self.delay_sim_time(1) + self.drain_mav(mav2) + self.assert_not_receive_message('HIGH_LATENCY2', mav=mav2, timeout=10) + + self.start_subsubtest("Enable and disable should not affect non-HL links getting HIGH_LATENCY2") + self.set_message_rate_hz("HIGH_LATENCY2", 5, mav=self.mav) + self.assert_message_rate_hz("HIGH_LATENCY2", 5, mav=self.mav) + + self.run_cmd_enable_high_latency(True) + self.assert_message_rate_hz("HIGH_LATENCY2", 5, mav=self.mav), + + self.run_cmd_enable_high_latency(False) + self.assert_message_rate_hz("HIGH_LATENCY2", 5, mav=self.mav) + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.context_pop() + + self.reboot_sitl() + + self.set_message_rate_hz("HIGH_LATENCY2", 0) + + if ex is not None: + raise ex def test_log_download(self): if self.is_tracker(): @@ -3057,7 +3154,7 @@ def get_sim_time_cached(self): timeout = 30 if self.valgrind: timeout *= 10 - if time.time() - self.last_sim_time_cached_wallclock > timeout: + if time.time() - self.last_sim_time_cached_wallclock > timeout and not self.gdb: raise AutoTestTimeoutException("sim_time_cached is not updating!") return ret @@ -3543,8 +3640,29 @@ def assert_mission_files_same(self, file1, file2, match_comments=False): raise ValueError("count %u not handled" % count) self.progress("Files same") - def assert_receive_message(self, type, timeout=1, verbose=False, very_verbose=False): - m = self.mav.recv_match(type=type, blocking=True, timeout=timeout) + def assert_not_receive_message(self, message, timeout=1, mav=None): + '''this is like assert_not_receiving_message but uses sim time not + wallclock time''' + self.progress("making sure we're not getting %s messages" % message) + if mav is None: + mav = self.mav + + tstart = self.get_sim_time_cached() + while True: + m = mav.recv_match(type=message, blocking=True, timeout=0.1) + if m is not None: + self.progress("Received: %s" % self.dump_message_verbose(m)) + raise PreconditionFailedException("Receiving %s messages" % message) + if mav != self.mav: + # update timestamp.... + self.drain_mav(self.mav) + if self.get_sim_time_cached() - tstart > timeout: + return + + def assert_receive_message(self, type, timeout=1, verbose=False, very_verbose=False, mav=None): + if mav is None: + mav = self.mav + m = mav.recv_match(type=type, blocking=True, timeout=timeout) if verbose: self.progress("Received (%s)" % str(m)) if very_verbose: @@ -4796,8 +4914,11 @@ def send_cmd(self, p7, target_sysid=None, target_compid=None, + mav=None, ): """Send a MAVLink command long.""" + if mav is None: + mav = self.mav if target_sysid is None: target_sysid = self.sysid_thismav() if target_compid is None: @@ -4818,17 +4939,17 @@ def send_cmd(self, p5, p6, p7)) - self.mav.mav.command_long_send(target_sysid, - target_compid, - command, - 1, # confirmation - p1, - p2, - p3, - p4, - p5, - p6, - p7) + mav.mav.command_long_send(target_sysid, + target_compid, + command, + 1, # confirmation + p1, + p2, + p3, + p4, + p5, + p6, + p7) def run_cmd(self, command, @@ -4843,8 +4964,9 @@ def run_cmd(self, target_sysid=None, target_compid=None, timeout=10, - quiet=False): - self.drain_mav() + quiet=False, + mav=None): + self.drain_mav(mav=mav) self.get_sim_time() # required for timeout in run_cmd_get_ack to work self.send_cmd( command, @@ -4857,20 +4979,25 @@ def run_cmd(self, p7, target_sysid=target_sysid, target_compid=target_compid, + mav=mav, ) - self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet) + self.run_cmd_get_ack(command, want_result, timeout, quiet=quiet, mav=mav) - def run_cmd_get_ack(self, command, want_result, timeout, quiet=False): + def run_cmd_get_ack(self, command, want_result, timeout, quiet=False, mav=None): # note that the caller should ensure that this cached # timestamp is reasonably up-to-date! + if mav is None: + mav = self.mav tstart = self.get_sim_time_cached() while True: + if mav != self.mav: + self.drain_mav() delta_time = self.get_sim_time_cached() - tstart if delta_time > timeout: raise AutoTestTimeoutException("Did not get good COMMAND_ACK within %fs" % timeout) - m = self.mav.recv_match(type='COMMAND_ACK', - blocking=True, - timeout=0.1) + m = mav.recv_match(type='COMMAND_ACK', + blocking=True, + timeout=0.1) if m is None: continue if not quiet: @@ -8328,16 +8455,22 @@ def test_arm_feature(self): self.progress("ALL PASS") # TODO : Test arming magic; - def get_message_rate(self, victim_message, timeout): + def get_message_rate(self, victim_message, timeout=10, mav=None): + if mav is None: + mav = self.mav tstart = self.get_sim_time() count = 0 while self.get_sim_time_cached() < tstart + timeout: - m = self.mav.recv_match(type=victim_message, - blocking=True, - timeout=0.1 - ) + m = mav.recv_match( + type=victim_message, + blocking=True, + timeout=0.1 + ) if m is not None: count += 1 + if mav != self.mav: + self.drain_mav(self.mav) + time_delta = self.get_sim_time_cached() - tstart self.progress("%s count after %f seconds: %u" % (victim_message, time_delta, count)) @@ -8346,7 +8479,7 @@ def get_message_rate(self, victim_message, timeout): def rate_to_interval_us(self, rate): return 1/float(rate)*1000000.0 - def set_message_rate_hz(self, id, rate_hz): + def set_message_rate_hz(self, id, rate_hz, mav=None): '''set a message rate in Hz; 0 for original, -1 to disable''' if type(id) == str: id = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % id) @@ -8361,15 +8494,20 @@ def set_message_rate_hz(self, id, rate_hz): 0, 0, 0, - 0) + 0, + mav=mav) - def send_get_message_interval(self, victim_message_id): - self.mav.mav.command_long_send( + def send_get_message_interval(self, victim_message, mav=None): + if mav is None: + mav = self.mav + if type(victim_message) == str: + victim_message = eval("mavutil.mavlink.MAVLINK_MSG_ID_%s" % victim_message) + mav.mav.command_long_send( 1, 1, mavutil.mavlink.MAV_CMD_GET_MESSAGE_INTERVAL, 1, # confirmation - float(victim_message_id), + float(victim_message), 0, 0, 0, @@ -8377,23 +8515,36 @@ def send_get_message_interval(self, victim_message_id): 0, 0) - def test_rate(self, desc, in_rate, expected_rate): + def test_rate(self, + desc, + in_rate, + expected_rate + , mav=None, + victim_message="VFR_HUD", + ndigits=0, + message_rate_sample_period=10): + if mav is None: + mav = self.mav + self.progress("###### %s" % desc) - self.progress("Setting rate to %u" % in_rate) + self.progress("Setting rate to %f" % round(in_rate, ndigits=ndigits)) - self.set_message_rate_hz(self.victim_message_id, in_rate) + self.set_message_rate_hz(victim_message, in_rate, mav=mav) - new_measured_rate = self.get_message_rate(self.victim_message, 10) - self.progress("Measured rate: %f (want %u)" % - (new_measured_rate, expected_rate)) - if round(new_measured_rate) != expected_rate: - raise NotAchievedException("Rate not achieved (got %f want %u)" % - (new_measured_rate, expected_rate)) + new_measured_rate = self.get_message_rate(victim_message, timeout=message_rate_sample_period, mav=mav) + self.progress( + "Measured rate: %f (want %f)" % + (round(new_measured_rate, ndigits=ndigits), + round(expected_rate, ndigits=ndigits)) + ) + if round(new_measured_rate, ndigits=ndigits) != round(expected_rate, ndigits=ndigits): + raise NotAchievedException("Rate not achieved (got %f want %f)" % + (round(new_measured_rate, ndigits), round(expected_rate, ndigits))) # make sure get_message_interval works: - self.send_get_message_interval(self.victim_message_id) + self.send_get_message_interval(victim_message, mav=mav) - m = self.mav.recv_match(type='MESSAGE_INTERVAL', blocking=True) + m = mav.recv_match(type='MESSAGE_INTERVAL', blocking=True) if in_rate == 0: want = self.rate_to_interval_us(expected_rate) @@ -8405,7 +8556,7 @@ def test_rate(self, desc, in_rate, expected_rate): if m.interval_us != want: raise NotAchievedException("Did not read same interval back from autopilot: want=%d got=%d)" % (want, m.interval_us)) - m = self.mav.recv_match(type='COMMAND_ACK', blocking=True) + m = mav.recv_match(type='COMMAND_ACK', blocking=True) if m.result != mavutil.mavlink.MAV_RESULT_ACCEPTED: raise NotAchievedException("Expected ACCEPTED for reading message interval") @@ -8439,22 +8590,22 @@ def test_set_message_interval_many(self): if ex is not None: raise ex - def assert_message_rate_hz(self, message, want_rate, sample_period=20): - self.drain_mav() - rate = round(self.get_message_rate(message, sample_period)) - self.progress("%s: Want=%u got=%u" % (message, want_rate, rate)) + def assert_message_rate_hz(self, message, want_rate, sample_period=20, ndigits=0, mav=None): + if mav is None: + mav = self.mav + self.drain_mav(mav) + rate = round(self.get_message_rate(message, sample_period, mav=mav), ndigits=ndigits) + self.progress("%s: Want=%f got=%f" % (message, round(want_rate, ndigits=ndigits), round(rate, ndigits=ndigits))) if rate != want_rate: - raise NotAchievedException("Did not get expected rate (want=%u got=%u" % (want_rate, rate)) + raise NotAchievedException("Did not get expected rate (want=%f got=%f)" % (want_rate, rate)) def test_set_message_interval_basic(self): - self.victim_message = 'VFR_HUD' - self.victim_message_id = mavutil.mavlink.MAVLINK_MSG_ID_VFR_HUD ex = None try: - rate = round(self.get_message_rate(self.victim_message, 20)) + rate = round(self.get_message_rate("VFR_HUD", 20)) self.progress("Initial rate: %u" % rate) - self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2) + self.test_rate("Test set to %u" % (rate/2,), rate/2, rate/2, victim_message="VFR_HUD") # this assumes the streamrates have not been played with: self.test_rate("Resetting original rate using 0-value", 0, rate) self.test_rate("Disabling using -1-value", -1, 0) @@ -9768,9 +9919,11 @@ def test_parameter_checks_poscontrol(self, param_prefix): want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED) self.disarm_vehicle() - def assert_not_receiving_message(self, message, timeout=1): + def assert_not_receiving_message(self, message, timeout=1, mav=None): self.progress("making sure we're not getting %s messages" % message) - m = self.mav.recv_match(type=message, blocking=True, timeout=timeout) + if mav is None: + mav = self.mav + m = mav.recv_match(type=message, blocking=True, timeout=timeout) if m is not None: raise PreconditionFailedException("Receiving %s messags" % message) @@ -10055,7 +10208,6 @@ def ahrstrim_attitude_correctness(self): self.wait_attitude(desroll=0, despitch=0, timeout=120, tolerance=1.5) if ahrs_type != 0: # we don't get secondary msgs while DCM is primary self.wait_attitude(desroll=0, despitch=0, message_type='AHRS2', tolerance=1, timeout=120) - # self.send_debug_trap() self.wait_attitude_quaternion(desroll=0, despitch=0, tolerance=1, timeout=120) self.context_pop() From a8b86e9c45bf04709e5d22bd641955cbb36b2ec6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 5 Mar 2022 09:31:45 +1030 Subject: [PATCH 0100/3101] AP_Math: Convert S-Curves to use maximum Snap to remove minimum time between waypoints --- libraries/AP_Math/SCurve.cpp | 160 ++++++++++++++++-------- libraries/AP_Math/SCurve.h | 21 ++-- libraries/AP_Math/tests/test_scurve.cpp | 15 +-- 3 files changed, 128 insertions(+), 68 deletions(-) diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index a09a7514287..115687f6168 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -40,7 +40,7 @@ SCurve::SCurve() // initialise and clear the path void SCurve::init() { - jerk_time = 0.0f; + snap_max = 0.0f; jerk_max = 0.0f; accel_max = 0.0f; vel_max = 0.0f; @@ -57,7 +57,7 @@ void SCurve::init() void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination, float speed_xy, float speed_up, float speed_down, float accel_xy, float accel_z, - float jerk_time_sec, float jerk_maximum) + float snap_maximum, float jerk_maximum) { init(); @@ -67,8 +67,8 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination return; } - // set jerk time and jerk max - jerk_time = jerk_time_sec; + // set snap_max and jerk max + snap_max = snap_maximum; jerk_max = jerk_maximum; // update speed and acceleration limits along path @@ -77,7 +77,7 @@ void SCurve::calculate_track(const Vector3f &origin, const Vector3f &destination accel_xy, accel_z); // avoid divide-by zeros. Path will be left as a zero length path - if (!is_positive(jerk_time) || !is_positive(jerk_max) || !is_positive(accel_max) || !is_positive(vel_max)) { + if (!is_positive(snap_max) || !is_positive(jerk_max) || !is_positive(accel_max) || !is_positive(vel_max)) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL ::printf("SCurve::calculate_track created zero length path\n"); #endif @@ -210,13 +210,13 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) // minimum velocity that can be obtained by shortening SEG_ACCEL_MAX const float Vmin = segment[SEG_ACCEL_END].end_vel - segment[SEG_ACCEL_MAX].end_accel * (segment[SEG_ACCEL_MAX].end_time - MAX(time, segment[SEG_ACCEL_MAX-1].end_time)); - float Jm, t2, t4, t6; - calculate_path(jerk_time, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, Vstart, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6); uint8_t seg = SEG_INIT+1; - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -231,12 +231,12 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) segment[i].end_pos = segment[SEG_ACCEL_END].end_pos; } - calculate_path(jerk_time, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, t2, t4, t6); + calculate_path(snap_max, jerk_max, 0.0f, accel_max, MAX(Vmin, vel_max), Pend * 0.5f, Jm, tj, t2, t4, t6); seg = SEG_CONST + 1; - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -264,27 +264,30 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) { // add velocity adjustment // check there is enough time to make velocity change + // we use the approximation that the time will be distance/max_vel and 8 jerk segments const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos; float Jm = 0; + float tj = 0; float t2 = 0; float t4 = 0; float t6 = 0; + float jerk_time = MIN(powf((fabsf(vel_max - segment[SEG_ACCEL_END].end_vel) * M_PI) / (4 * snap_max), 1/3), jerk_max * M_PI / (2 * snap_max)); if ((vel_max < segment[SEG_ACCEL_END].end_vel) && (jerk_time*12.0f < L/segment[SEG_ACCEL_END].end_vel)) { // we have a problem here with small segments. - calculate_path(jerk_time, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, t6, t4, t2); + calculate_path(snap_max, jerk_max, vel_max, accel_max, segment[SEG_ACCEL_END].end_vel, L * 0.5f, Jm, tj, t6, t4, t2); Jm = -Jm; } else if ((vel_max > segment[SEG_ACCEL_END].end_vel) && (L/(jerk_time*12.0f) > segment[SEG_ACCEL_END].end_vel)) { float Vm = MIN(vel_max, L/(jerk_time*12.0f)); - calculate_path(jerk_time, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, t2, t4, t6); + calculate_path(snap_max, jerk_max, segment[SEG_ACCEL_END].end_vel, accel_max, Vm, L * 0.5f, Jm, tj, t2, t4, t6); } uint8_t seg = SEG_ACCEL_END + 1; if (!is_zero(Jm) && !is_negative(t2) && !is_negative(t4) && !is_negative(t6)) { - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_SPEED_CHANGE_END].end_accel = 0.0f; @@ -297,11 +300,11 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) Vend = MIN(Vend, segment[SEG_SPEED_CHANGE_END].end_vel); add_segment_const_jerk(seg, 0.0f, 0.0f); if (Vend < segment[SEG_SPEED_CHANGE_END].end_vel) { - float Jm, t2, t4, t6; - calculate_path(jerk_time, jerk_max, Vend, accel_max, segment[SEG_CONST].end_vel, Pend - segment[SEG_CONST].end_pos, Jm, t2, t4, t6); - add_segments_jerk(seg, jerk_time, -Jm, t6); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, Vend, accel_max, segment[SEG_CONST].end_vel, Pend - segment[SEG_CONST].end_pos, Jm, tj, t2, t4, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); } else { // No deceleration is required for (uint8_t i = SEG_CONST+1; i <= SEG_DECEL_END; i++) { @@ -355,14 +358,14 @@ float SCurve::set_origin_speed_max(float speed) const float track_length = track.length(); speed = MIN(speed, Vm); - float Jm, t2, t4, t6; - calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, tj, t2, t4, t6); uint8_t seg = SEG_INIT; add_segment(seg, 0.0f, SegmentType::CONSTANT_JERK, 0.0f, 0.0f, speed, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -388,11 +391,11 @@ float SCurve::set_origin_speed_max(float speed) seg = SEG_CONST; add_segment_const_jerk(seg, 0.0f, 0.0f); - calculate_path(jerk_time, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, track_length * 0.5f, Jm, t2, t4, t6); + calculate_path(snap_max, jerk_max, 0.0f, accel_max, segment[SEG_CONST].end_vel, track_length * 0.5f, Jm, tj, t2, t4, t6); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -437,15 +440,15 @@ void SCurve::set_destination_speed_max(float speed) const float track_length = track.length(); speed = MIN(speed, Vm); - float Jm, t2, t4, t6; - calculate_path(jerk_time, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, t2, t4, t6); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, speed, accel_max, Vm, track_length * 0.5f, Jm, tj, t2, t4, t6); uint8_t seg = SEG_CONST; add_segment_const_jerk(seg, 0.0f, 0.0f); - add_segments_jerk(seg, jerk_time, -Jm, t6); + add_segments_jerk(seg, tj, -Jm, t6); add_segment_const_jerk(seg, t4, 0.0f); - add_segments_jerk(seg, jerk_time, Jm, t2); + add_segments_jerk(seg, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -597,7 +600,7 @@ void SCurve::advance_time(float dt) // calculate the jerk, acceleration, velocity and position at the provided time void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const { - if ((num_segs != segments_max) || !is_positive(jerk_time)) { + if (num_segs != segments_max) { Jt_out = 0; At_out = 0; Vt_out = 0; @@ -607,7 +610,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float SegmentType Jtype; uint8_t pnt = num_segs; - float Jm, T0, A0, V0, P0; + float Jm, tj, T0, A0, V0, P0; // find active segment at time_now for (uint8_t i = 0; i < num_segs; i++) { @@ -618,6 +621,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float if (pnt == 0) { Jtype = SegmentType::CONSTANT_JERK; Jm = 0.0f; + tj = 0.0f; T0 = segment[pnt].end_time; A0 = segment[pnt].end_accel; V0 = segment[pnt].end_vel; @@ -625,6 +629,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float } else if (pnt == num_segs) { Jtype = SegmentType::CONSTANT_JERK; Jm = 0.0f; + tj = 0.0f; T0 = segment[pnt - 1].end_time; A0 = segment[pnt - 1].end_accel; V0 = segment[pnt - 1].end_vel; @@ -632,6 +637,7 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float } else { Jtype = segment[pnt].seg_type; Jm = segment[pnt].jerk_ref; + tj = segment[pnt].end_time - segment[pnt - 1].end_time; T0 = segment[pnt - 1].end_time; A0 = segment[pnt - 1].end_accel; V0 = segment[pnt - 1].end_vel; @@ -643,10 +649,10 @@ void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float calc_javp_for_segment_const_jerk(time_now - T0, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); break; case SegmentType::POSITIVE_JERK: - calc_javp_for_segment_incr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); + calc_javp_for_segment_incr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); break; case SegmentType::NEGATIVE_JERK: - calc_javp_for_segment_decr_jerk(time_now - T0, jerk_time, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); + calc_javp_for_segment_decr_jerk(time_now - T0, tj, Jm, A0, V0, P0, Jt_out, At_out, Vt_out, Pt_out); break; } Pt_out = MAX(0.0f, Pt_out); @@ -664,6 +670,13 @@ void SCurve::calc_javp_for_segment_const_jerk(float time_now, float J0, float A0 // Calculate the jerk, acceleration, velocity and position at time time_now when running the increasing jerk magnitude time segment based on a raised cosine profile void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const { + if (!is_positive(tj)) { + Jt = 0.0; + At = A0; + Vt = V0; + Pt = P0; + return; + } const float Alpha = Jm * 0.5f; const float Beta = M_PI / tj; Jt = Alpha * (1.0f - cosf(Beta * time_now)); @@ -675,6 +688,13 @@ void SCurve::calc_javp_for_segment_incr_jerk(float time_now, float tj, float Jm, // Calculate the jerk, acceleration, velocity and position at time time_now when running the decreasing jerk magnitude time segment based on a raised cosine profile void SCurve::calc_javp_for_segment_decr_jerk(float time_now, float tj, float Jm, float A0, float V0, float P0, float &Jt, float &At, float &Vt, float &Pt) const { + if (!is_positive(tj)) { + Jt = 0.0; + At = A0; + Vt = V0; + Pt = P0; + return; + } const float Alpha = Jm * 0.5f; const float Beta = M_PI / tj; const float AT = Alpha * tj; @@ -699,12 +719,12 @@ void SCurve::add_segments(float L) return; } - float Jm, t2, t4, t6; - calculate_path(jerk_time, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, t2, t4, t6); + float Jm, tj, t2, t4, t6; + calculate_path(snap_max, jerk_max, 0.0f, accel_max, vel_max, L * 0.5f, Jm, tj, t2, t4, t6); - add_segments_jerk(num_segs, jerk_time, Jm, t2); + add_segments_jerk(num_segs, tj, Jm, t2); add_segment_const_jerk(num_segs, t4, 0.0f); - add_segments_jerk(num_segs, jerk_time, -Jm, t6); + add_segments_jerk(num_segs, tj, -Jm, t6); // remove numerical errors segment[SEG_ACCEL_END].end_accel = 0.0f; @@ -721,9 +741,9 @@ void SCurve::add_segments(float L) const float t15 = MAX(0.0f, (L - 2.0f * segment[SEG_SPEED_CHANGE_END].end_pos) / segment[SEG_SPEED_CHANGE_END].end_vel); add_segment_const_jerk(num_segs, t15, 0.0f); - add_segments_jerk(num_segs, jerk_time, -Jm, t6); + add_segments_jerk(num_segs, tj, -Jm, t6); add_segment_const_jerk(num_segs, t4, 0.0f); - add_segments_jerk(num_segs, jerk_time, Jm, t2); + add_segments_jerk(num_segs, tj, Jm, t2); // remove numerical errors segment[SEG_DECEL_END].end_accel = 0.0f; @@ -731,23 +751,24 @@ void SCurve::add_segments(float L) } // calculate the segment times for the trigonometric S-Curve path defined by: -// tj - duration of the raised cosine jerk profile +// Sm - duration of the raised cosine jerk profile // Jm - maximum value of the raised cosine jerk profile // V0 - initial velocity magnitude // Am - maximum constant acceleration // Vm - maximum constant velocity // L - Length of the path -// t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables -void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out) +// tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables +void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L,float &Jm_out, float &tj_out, float &t2_out, float &t4_out, float &t6_out) { // init outputs Jm_out = 0.0f; + tj_out = 0.0f; t2_out = 0.0f; t4_out = 0.0f; t6_out = 0.0f; // check for invalid arguments - if (!is_positive(tj) || !is_positive(Jm) || !is_positive(Am) || !is_positive(Vm) || !is_positive(L)) { + if (!is_positive(Sm) || !is_positive(Jm) || !is_positive(Am) || !is_positive(Vm) || !is_positive(L)) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL ::printf("SCurve::calculate_path invalid inputs\n"); #endif @@ -760,9 +781,24 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl return; } - Am = MIN(MIN(Am, (Vm - V0) / (2.0f * tj)), (L + 4.0f * V0 * tj) / (4.0f * sq(tj))); - if (fabsf(Am) < Jm * tj) { - Jm = Am / tj; + float tj = Jm * M_PI / (2 * Sm); + float At = MIN(MIN(Am, + (Vm - V0) / (2.0f * tj) ), + (L + 4.0f * V0 * tj) / (4.0f * sq(tj)) ); + if (fabsf(At) < Jm * tj) { + if (is_zero(V0)) { + // we do not have a solution for non-zero initial velocity + tj = MIN( MIN( MIN( tj, + powf((L * M_PI) / (8.0 * Sm), 1.0/4.0) ), + powf((Vm * M_PI) / (4.0 * Sm), 1.0/3.0) ), + safe_sqrt((Am * M_PI) / (2.0 * Sm)) ); + Jm = 2.0 * Sm * tj / M_PI; + Am = Jm * tj; + } else { + // When doing speed change we use fixed tj and adjust Jm for small changes + Am = At; + Jm = Am / tj; + } if ((Vm <= V0 + 2.0f * Am * tj) || (L <= 4.0f * V0 * tj + 4.0f * Am * sq(tj))) { // solution = 0 - t6 t4 t2 = 0 0 0 t2_out = 0.0f; @@ -790,10 +826,12 @@ void SCurve::calculate_path(float tj, float Jm, float V0, float Am, float Vm, fl t6_out = t2_out; } } + tj_out = tj; Jm_out = Jm; // check outputs and reset back to zero if necessary if (!isfinite(Jm_out) || is_negative(Jm_out) || + !isfinite(tj_out) || is_negative(tj_out) || !isfinite(t2_out) || is_negative(t2_out) || !isfinite(t4_out) || is_negative(t4_out) || !isfinite(t6_out) || is_negative(t6_out)) { @@ -824,7 +862,7 @@ void SCurve::add_segments_jerk(uint8_t &index, float tj, float Jm, float Tcj) void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0) { // if no time increase copy previous segment - if (is_zero(tj)) { + if (!is_positive(tj)) { add_segment(index, segment[index - 1].end_time, SegmentType::CONSTANT_JERK, J0, @@ -847,6 +885,16 @@ void SCurve::add_segment_const_jerk(uint8_t &index, float tj, float J0) // the index variable is the position of this segment in the path array and is incremented to reference the next segment in the array void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm) { + // if no time increase copy previous segment + if (!is_positive(tj)) { + add_segment(index, segment[index - 1].end_time, + SegmentType::CONSTANT_JERK, + 0.0, + segment[index - 1].end_accel, + segment[index - 1].end_vel, + segment[index - 1].end_pos); + return; + } const float Beta = M_PI / tj; const float Alpha = Jm * 0.5f; const float AT = Alpha * tj; @@ -866,6 +914,16 @@ void SCurve::add_segment_incr_jerk(uint8_t &index, float tj, float Jm) // the index variable is the position of this segment in the path and is incremented to reference the next segment in the array void SCurve::add_segment_decr_jerk(uint8_t &index, float tj, float Jm) { + // if no time increase copy previous segment + if (!is_positive(tj)) { + add_segment(index, segment[index - 1].end_time, + SegmentType::CONSTANT_JERK, + 0.0, + segment[index - 1].end_accel, + segment[index - 1].end_vel, + segment[index - 1].end_pos); + return; + } const float Beta = M_PI / tj; const float Alpha = Jm * 0.5f; const float AT = Alpha * tj; @@ -960,8 +1018,8 @@ bool SCurve::valid() const // debugging messages void SCurve::debug() const { - ::printf("num_segs:%u, time:%4.2f, jerk_time:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n", - (unsigned)num_segs, (double)time, (double)jerk_time, (double)jerk_max, (double)accel_max, (double)vel_max); + ::printf("num_segs:%u, time:%4.2f, snap_max:%4.2f, jerk_max:%4.2f, accel_max:%4.2f, vel_max:%4.2f\n", + (unsigned)num_segs, (double)time, (double)snap_max, (double)jerk_max, (double)accel_max, (double)vel_max); ::printf("T, Jt, J, A, V, P \n"); for (uint8_t i = 0; i < num_segs; i++) { ::printf("i:%u, T:%4.2f, Jtype:%4.2f, J:%4.2f, A:%4.2f, V: %4.2f, P: %4.2f\n", diff --git a/libraries/AP_Math/SCurve.h b/libraries/AP_Math/SCurve.h index 7696c334509..701e08446bc 100644 --- a/libraries/AP_Math/SCurve.h +++ b/libraries/AP_Math/SCurve.h @@ -36,8 +36,8 @@ * velocity: rate of change of position. aka speed * acceleration: rate of change of speed * jerk: rate of change of acceleration + * snap: rate of change of jerk * jerk time: the time (in seconds) for jerk to increase from zero to its maximum value - * jounce: rate of change of jerk * track: 3D path that the vehicle will follow * path: position, velocity, accel and jerk kinematic profile that this library generates */ @@ -53,21 +53,22 @@ class SCurve { void init(); // calculate the segment times for the trigonometric S-Curve path defined by: - // tj - duration of the raised cosine jerk profile (aka jerk time) - // Jm - maximum value of the raised cosine jerk profile (aka jerk max) + // Sm - maximum value of the snap profile + // Jm - maximum value of the raised cosine jerk profile // V0 - initial velocity magnitude // Am - maximum constant acceleration // Vm - maximum constant velocity // L - Length of the path + // tj_out, t2_out, t4_out, t6_out are the segment durations needed to achieve the kinematic path specified by the input variables // this is an internal function, static for test suite - static void calculate_path(float tj, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &t2_out, float &t4_out, float &t6_out); + static void calculate_path(float Sm, float Jm, float V0, float Am, float Vm, float L, float &Jm_out, float &tj_out, float &t2_out, float &t4_out, float &t6_out); // generate a trigonometric track in 3D space that moves over a straight line // between two points defined by the origin and destination void calculate_track(const Vector3f &origin, const Vector3f &destination, float speed_xy, float speed_up, float speed_down, float accel_xy, float accel_z, - float jerk_time_sec, float jerk_maximum); + float snap_maximum, float jerk_maximum); // set maximum velocity and re-calculate the path using these limits void set_speed_max(float speed_xy, float speed_up, float speed_down); @@ -146,16 +147,16 @@ class SCurve { void add_segments(float L); // generate three time segments forming the jerk profile - void add_segments_jerk(uint8_t &seg_pnt, float tj, float Jm, float Tcj); + void add_segments_jerk(uint8_t &seg_pnt, float Jm, float tj, float Tcj); // generate constant jerk time segment - void add_segment_const_jerk(uint8_t &seg_pnt, float tin, float J0); + void add_segment_const_jerk(uint8_t &seg_pnt, float J0, float tin); // generate increasing jerk magnitude time segment based on a raised cosine profile - void add_segment_incr_jerk(uint8_t &seg_pnt, float tj, float Jm); + void add_segment_incr_jerk(uint8_t &seg_pnt, float Jm, float tj); // generate decreasing jerk magnitude time segment based on a raised cosine profile - void add_segment_decr_jerk(uint8_t &seg_pnt, float tj, float Jm); + void add_segment_decr_jerk(uint8_t &seg_pnt, float Jm, float tj); // set speed and acceleration limits for the path // origin and destination are offsets from EKF origin @@ -183,7 +184,7 @@ class SCurve { void add_segment(uint8_t &seg_pnt, float end_time, SegmentType seg_type, float jerk_ref, float end_accel, float end_vel, float end_pos); // members - float jerk_time; // duration of jerk raised cosine time segment + float snap_max; // maximum snap magnitude float jerk_max; // maximum jerk magnitude float accel_max; // maximum acceleration magnitude float vel_max; // maximum velocity magnitude diff --git a/libraries/AP_Math/tests/test_scurve.cpp b/libraries/AP_Math/tests/test_scurve.cpp index 6620d790a06..22b1a0beda5 100644 --- a/libraries/AP_Math/tests/test_scurve.cpp +++ b/libraries/AP_Math/tests/test_scurve.cpp @@ -7,13 +7,14 @@ TEST(LinesScurve, test_calculate_path) { - float Jm_out, t2_out, t4_out, t6_out; - SCurve::calculate_path(0.300000012, 19.4233513, 0, 5.82700586, 188.354691, 2.09772229, - Jm_out, t2_out, t4_out, t6_out); - EXPECT_FLOAT_EQ(Jm_out, 19.423351); - EXPECT_FLOAT_EQ(t2_out, 0.0); - EXPECT_FLOAT_EQ(t4_out, 0.0); - EXPECT_FLOAT_EQ(t6_out, 0.0); + // this test doesn't do much... + float Jm_out, tj_out, t2_out, t4_out, t6_out; + SCurve::calculate_path(62.8319, 10, 0, 5, 10, 100, + Jm_out, tj_out, t2_out, t4_out, t6_out); + EXPECT_FLOAT_EQ(Jm_out, 10); + EXPECT_FLOAT_EQ(t2_out, 0.25000018); + EXPECT_FLOAT_EQ(t4_out, 1.2500002); + EXPECT_FLOAT_EQ(t6_out, 0.25000018); } From 5fa37e253e13090926878b8ccecc766cfa36193a Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 3 Mar 2022 09:23:11 +1030 Subject: [PATCH 0101/3101] AC_WP_Nav: Convert S-Curves to use maximum Snap to remove minimum time between waypoints --- libraries/AC_WPNav/AC_WPNav.cpp | 26 +++++++++++++------------- libraries/AC_WPNav/AC_WPNav.h | 6 +++--- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 2fb251efd27..2854d38be53 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -177,7 +177,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) if (!is_positive(_wp_jerk)) { _wp_jerk = _wp_accel_cmss; } - calc_scurve_jerk_and_jerk_time(); + calc_scurve_jerk_and_snap(); _scurve_prev_leg.init(); _scurve_this_leg.init(); @@ -334,7 +334,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) _scurve_this_leg.calculate_track(_origin, _destination, _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), _wp_accel_cmss, _wp_accel_z_cmss, - _scurve_jerk_time, _scurve_jerk * 100.0f); + _scurve_snap * 100.0f, _scurve_jerk * 100.0f); if (!is_zero(origin_speed)) { // rebuild start of scurve if we have a non-zero origin speed _scurve_this_leg.set_origin_speed_max(origin_speed); @@ -362,7 +362,7 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain _scurve_next_leg.calculate_track(_destination, destination, _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), _wp_accel_cmss, _wp_accel_z_cmss, - _scurve_jerk_time, _scurve_jerk * 100.0f); + _scurve_snap * 100.0f, _scurve_jerk * 100.0); if (_this_leg_is_spline) { const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max); @@ -857,8 +857,8 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_ } // helper function to calculate scurve jerk and jerk_time values -// updates _scurve_jerk and _scurve_jerk_time -void AC_WPNav::calc_scurve_jerk_and_jerk_time() +// updates _scurve_jerk and _scurve_snap +void AC_WPNav::calc_scurve_jerk_and_snap() { // calculate jerk _scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS); @@ -868,14 +868,14 @@ void AC_WPNav::calc_scurve_jerk_and_jerk_time() _scurve_jerk = MIN(_scurve_jerk, _wp_jerk); } - // calculate jerk time - // Jounce (the rate of change of jerk) uses the attitude control input time constant because multicopters + // calculate maximum snap + // Snap (the rate of change of jerk) uses the attitude control input time constant because multicopters // lean to accelerate. This means the change in angle is equivalent to the change in acceleration - const float jounce = MIN(_attitude_control.get_accel_roll_max_radss() * GRAVITY_MSS, _attitude_control.get_accel_pitch_max_radss() * GRAVITY_MSS); - if (is_positive(jounce)) { - _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.5f * _scurve_jerk * M_PI / jounce); - } else { - _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.1f); + _scurve_snap = (_scurve_jerk * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f)); + const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS; + if (is_positive(snap)) { + _scurve_snap = MIN(_scurve_snap, snap); } - _scurve_jerk_time *= 2.0f; + // reduce maximum snap by a factor of two from what the aircraft is capable of + _scurve_snap *= 0.5; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 4a4432aff83..f2c050566b0 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -213,8 +213,8 @@ class AC_WPNav } _flags; // helper function to calculate scurve jerk and jerk_time values - // updates _scurve_jerk and _scurve_jerk_time - void calc_scurve_jerk_and_jerk_time(); + // updates _scurve_jerk and _scurve_snap + void calc_scurve_jerk_and_snap(); // references and pointers to external libraries const AP_InertialNav& _inav; @@ -241,7 +241,7 @@ class AC_WPNav SCurve _scurve_this_leg; // current scurve trajectory SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory float _scurve_jerk; // scurve jerk max in m/s/s/s - float _scurve_jerk_time; // scurve jerk time (time in seconds for jerk to increase from zero _scurve_jerk) + float _scurve_snap; // scurve snap in m/s/s/s/s // spline curves SplineCurve _spline_this_leg; // spline curve for current segment From 7673948d2aa6af70e80a83696e0497afc34e2f46 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 5 Mar 2022 09:31:58 +1030 Subject: [PATCH 0102/3101] AP_Math: SCurve: Increase corner speeds --- libraries/AP_Math/SCurve.cpp | 38 ++++++++++++++++++++++++++++-------- libraries/AP_Math/SCurve.h | 23 ++++++++++++++-------- 2 files changed, 45 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index 115687f6168..42a14ca797b 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -26,9 +26,11 @@ extern const AP_HAL::HAL &hal; #define SEG_INIT 0 #define SEG_ACCEL_MAX 4 +#define SEG_TURN_IN 4 #define SEG_ACCEL_END 7 #define SEG_SPEED_CHANGE_END 14 #define SEG_CONST 15 +#define SEG_TURN_OUT 15 #define SEG_DECEL_END 22 // constructor @@ -264,7 +266,6 @@ void SCurve::set_speed_max(float speed_xy, float speed_up, float speed_down) if (!is_equal(vel_max, segment[SEG_ACCEL_END].end_vel)) { // add velocity adjustment // check there is enough time to make velocity change - // we use the approximation that the time will be distance/max_vel and 8 jerk segments const float L = segment[SEG_CONST].end_pos - segment[SEG_ACCEL_END].end_pos; float Jm = 0; @@ -481,7 +482,7 @@ void SCurve::set_destination_speed_max(float speed) // target_pos should be set to this segment's origin and it will be updated to the current position target // target_vel and target_accel are updated with new targets // returns true if vehicle has passed the apex of the corner -bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) +bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) { prev_leg.move_to_pos_vel_accel(dt, target_pos, target_vel, target_accel); move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); @@ -489,16 +490,19 @@ bool SCurve::advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, floa // check for change of leg on fast waypoint const float time_to_destination = get_time_remaining(); - if (fast_waypoint && braking() && is_zero(next_leg.get_time_elapsed()) && (time_to_destination <= next_leg.get_accel_finished_time())) { + if (fast_waypoint + && is_zero(next_leg.get_time_elapsed()) + && (get_time_elapsed() >= time_turn_out() - next_leg.time_turn_in()) + && (position_sq >= 0.25 * track.length_squared())) { + Vector3f turn_pos = -get_track(); Vector3f turn_vel, turn_accel; move_from_time_pos_vel_accel(get_time_elapsed() + time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); next_leg.move_from_time_pos_vel_accel(time_to_destination * 0.5f, turn_pos, turn_vel, turn_accel); const float speed_min = MIN(get_speed_along_track(), next_leg.get_speed_along_track()); - const float accel_min = MIN(get_accel_along_track(), next_leg.get_accel_along_track()); if ((get_time_remaining() < next_leg.time_end() * 0.5f) && (turn_pos.length() < wp_radius) && (Vector2f{turn_vel.x, turn_vel.y}.length() < speed_min) && - (Vector2f{turn_accel.x, turn_accel.y}.length() < 2.0f*accel_min)) { + (Vector2f{turn_accel.x, turn_accel.y}.length() < accel_corner)) { next_leg.move_from_pos_vel_accel(dt, target_pos, target_vel, target_accel); } } else if (!is_zero(next_leg.get_time_elapsed())) { @@ -559,7 +563,7 @@ void SCurve::move_from_time_pos_vel_accel(float time_now, Vector3f &pos, Vector3 float SCurve::time_end() const { if (num_segs != segments_max) { - return 0.0f; + return 0.0; } return segment[SEG_DECEL_END].end_time; } @@ -568,7 +572,7 @@ float SCurve::time_end() const float SCurve::get_time_remaining() const { if (num_segs != segments_max) { - return 0.0f; + return 0.0; } return segment[SEG_DECEL_END].end_time - time; } @@ -577,7 +581,7 @@ float SCurve::get_time_remaining() const float SCurve::get_accel_finished_time() const { if (num_segs != segments_max) { - return 0.0f; + return 0.0; } return segment[SEG_ACCEL_END].end_time; } @@ -591,6 +595,24 @@ bool SCurve::braking() const return time >= segment[SEG_CONST].end_time; } +// return time offset used to initiate the turn onto leg +float SCurve::time_turn_in() const +{ + if (num_segs != segments_max) { + return 0.0; + } + return segment[SEG_TURN_IN].end_time; +} + +// return time offset used to initiate the turn from leg +float SCurve::time_turn_out() const +{ + if (num_segs != segments_max) { + return 0.0; + } + return segment[SEG_TURN_OUT].end_time; +} + // increment the internal time void SCurve::advance_time(float dt) { diff --git a/libraries/AP_Math/SCurve.h b/libraries/AP_Math/SCurve.h index 701e08446bc..c801e1a003e 100644 --- a/libraries/AP_Math/SCurve.h +++ b/libraries/AP_Math/SCurve.h @@ -81,14 +81,15 @@ class SCurve { void set_destination_speed_max(float speed); // move target location along path from origin to destination - // prev_leg and next_leg are the paths before and after this path - // wp_radius is max distance from the waypoint at the apex of the turn - // fast_waypoint should be true if vehicle will not stop at end of this leg - // dt is the time increment the vehicle will move along the path - // target_pos should be set to this segment's origin and it will be updated to the current position target - // target_vel and target_accel are updated with new targets - // returns true if vehicle has passed the apex of the corner - bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED; + // prev_leg and next_leg - the paths before and after this path + // wp_radius - max distance from the waypoint at the apex of the turn + // accel_corner - max acceleration that the aircraft may use during the corner + // fast_waypoint - true if vehicle will not stop at end of this leg + // dt - the time increment the vehicle will move along the path + // target_pos - set to this segment's origin and it will be updated to the current position target + // target_vel and target_accel - updated with new targets + // advance_target_along_track returns true if vehicle has passed the apex of the corner + bool advance_target_along_track(SCurve &prev_leg, SCurve &next_leg, float wp_radius, float accel_corner, bool fast_waypoint, float dt, Vector3f &target_pos, Vector3f &target_vel, Vector3f &target_accel) WARN_IF_UNUSED; // time has reached the end of the sequence bool finished() const WARN_IF_UNUSED; @@ -128,6 +129,12 @@ class SCurve { // return true if the sequence is braking to a stop bool braking() const WARN_IF_UNUSED; + // return time offset used to initiate the turn onto leg + float time_turn_in() const WARN_IF_UNUSED; + + // return time offset used to initiate the turn from leg + float time_turn_out() const WARN_IF_UNUSED; + // increment the internal time void advance_time(float dt); From 93aea6781008a7f882d14d8c875ac4c764a53568 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 2 Mar 2022 13:59:00 +1030 Subject: [PATCH 0103/3101] AC_WPNav: Increase corner speeds --- libraries/AC_WPNav/AC_WPNav.cpp | 5 +++-- libraries/AC_WPNav/AC_WPNav.h | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 2854d38be53..05a0bf125df 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -150,7 +150,8 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) // sanity check parameters // check _wp_accel_cmss is reasonable - const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f))); + _scurve_accel_corner = GRAVITY_MSS * 100.0f * tanf(ToRad(_pos_control.get_lean_angle_max_cd() * 0.01f)); + const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner); _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss); // check _wp_radius_cm is reasonable @@ -497,7 +498,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) if (!_this_leg_is_spline) { // update target position, velocity and acceleration target_pos = _origin; - s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel); + s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel); } else { // splinetarget_vel target_vel = curr_target_vel; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index f2c050566b0..b5e1805d718 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -240,6 +240,7 @@ class AC_WPNav SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory SCurve _scurve_this_leg; // current scurve trajectory SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory + float _scurve_accel_corner; // scurve maximum corner acceleration in m/s/s float _scurve_jerk; // scurve jerk max in m/s/s/s float _scurve_snap; // scurve snap in m/s/s/s/s From 3ad10d7077f645c50642402c600b0ced75a780e9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 17:33:10 +1100 Subject: [PATCH 0104/3101] Tools: convert test_build_options to an object --- Tools/autotest/test_build_options.py | 155 +++++++++++++-------------- 1 file changed, 75 insertions(+), 80 deletions(-) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index b659e3ef770..562f466263d 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -14,87 +14,82 @@ from pysim import util -# swiped from app.py: -def get_build_options_from_ardupilot_tree(): - '''return a list of build options''' - import importlib.util - spec = importlib.util.spec_from_file_location( - "build_options.py", - os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', 'scripts', 'build_options.py')) - mod = importlib.util.module_from_spec(spec) - spec.loader.exec_module(mod) - return mod.BUILD_OPTIONS - - -def write_defines_to_file(defines, filepath): - content = "\n".join(["define %s %s" % (a, b) for (a, b) in defines.items()]) - with open(filepath, "w") as f: - f.write(content) - - -def get_defines(feature, options): - '''returns a hash of (name, value) defines to turn feature off - - recursively gets dependencies''' - ret = { - feature.define: 0, - } - if feature.dependency is None: +class TestBuildOptions(object): + # swiped from app.py: + def get_build_options_from_ardupilot_tree(self): + '''return a list of build options''' + import importlib.util + spec = importlib.util.spec_from_file_location( + "build_options.py", + os.path.join(os.path.dirname(os.path.realpath(__file__)), + '..', 'scripts', 'build_options.py')) + mod = importlib.util.module_from_spec(spec) + spec.loader.exec_module(mod) + return mod.BUILD_OPTIONS + + def write_defines_to_file(self, defines, filepath): + content = "\n".join(["define %s %s" % (a, b) for (a, b) in defines.items()]) + with open(filepath, "w") as f: + f.write(content) + + def get_defines(self, feature, options): + '''returns a hash of (name, value) defines to turn feature off - + recursively gets dependencies''' + ret = { + feature.define: 0, + } + if feature.dependency is None: + return ret + for depname in feature.dependency.split(','): + dep = None + for f in options: + if f.label == depname: + dep = f + if dep is None: + raise ValueError("Invalid dep (%s)" % dep) + ret.update(self.get_defines(dep, options)) return ret - for depname in feature.dependency.split(','): - dep = None - for f in options: - if f.label == depname: - dep = f - if dep is None: - raise ValueError("Invalid dep (%s)" % dep) - ret.update(get_defines(dep, options)) - return ret - - -def test_feature(feature, options): - defines = get_defines(feature, options) - test_compile_with_defines(defines) - - -def test_compile_with_defines(defines): - extra_hwdef_filepath = "/tmp/extra.hwdef" - write_defines_to_file(defines, extra_hwdef_filepath) - util.waf_configure( - "CubeOrange", - extra_hwdef=extra_hwdef_filepath, - ) - for t in 'copter', 'plane', 'rover', 'antennatracker', 'sub', 'blimp': - try: - util.waf_build(t) - except Exception: - print("Failed to build (%s) with everything disabled" % - (t,)) - raise - - -def run_disable_in_turn(): - options = get_build_options_from_ardupilot_tree() - count = 1 - for feature in options: - print("##### Disabling feature %s(%s) (%u/%u)" % - (feature.label, feature.define, count, len(options))) - test_feature(feature, options) - count += 1 - - -def run_disable_all(): - options = get_build_options_from_ardupilot_tree() - defines = {} - for feature in options: - defines[feature.define] = 0 - test_compile_with_defines(defines) - - -def run(): - run_disable_all() - run_disable_in_turn() + + def test_feature(self, feature, options): + defines = self.get_defines(feature, options) + self.test_compile_with_defines(defines) + + def test_compile_with_defines(self, defines): + extra_hwdef_filepath = "/tmp/extra.hwdef" + self.write_defines_to_file(defines, extra_hwdef_filepath) + util.waf_configure( + "CubeOrange", + extra_hwdef=extra_hwdef_filepath, + ) + for t in 'copter', 'plane', 'rover', 'antennatracker', 'sub', 'blimp': + try: + util.waf_build(t) + except Exception: + print("Failed to build (%s) with everything disabled" % + (t,)) + raise + + def run_disable_in_turn(self): + options = self.get_build_options_from_ardupilot_tree() + count = 1 + for feature in options: + print("##### Disabling feature %s(%s) (%u/%u)" % + (feature.label, feature.define, count, len(options))) + self.test_feature(feature, options) + count += 1 + + def run_disable_all(self): + options = self.get_build_options_from_ardupilot_tree() + defines = {} + for feature in options: + defines[feature.define] = 0 + self.test_compile_with_defines(defines) + + def run(self): + self.run_disable_all() + self.run_disable_in_turn() if __name__ == '__main__': - run() + tbo = TestBuildOptions() + tbo.run() From c27e3f0c353940291829d90b512b8a64788ce1cf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 23:06:54 +1100 Subject: [PATCH 0105/3101] test_build_options.py: emit size savings from disabling features --- Tools/autotest/test_build_options.py | 48 +++++++++++++++++++++++++--- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 562f466263d..0bd08fa7787 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -15,6 +15,9 @@ class TestBuildOptions(object): + def __init__(self): + self.sizes_nothing_disabled = None + # swiped from app.py: def get_build_options_from_ardupilot_tree(self): '''return a list of build options''' @@ -51,24 +54,53 @@ def get_defines(self, feature, options): return ret def test_feature(self, feature, options): - defines = self.get_defines(feature, options) + # defines = self.get_defines(feature, options) + defines = { + feature.define: 0, + } self.test_compile_with_defines(defines) + def build_targets(self): + '''return a list of build targets''' + return ['copter', 'plane', 'rover', 'antennatracker', 'sub', 'blimp'] + + def board(self): + '''returns board to build for''' + return "CubeOrange" + def test_compile_with_defines(self, defines): extra_hwdef_filepath = "/tmp/extra.hwdef" self.write_defines_to_file(defines, extra_hwdef_filepath) util.waf_configure( - "CubeOrange", + self.board(), extra_hwdef=extra_hwdef_filepath, ) - for t in 'copter', 'plane', 'rover', 'antennatracker', 'sub', 'blimp': + for t in self.build_targets(): try: - util.waf_build(t) + util.run_cmd([util.relwaf(), t]) except Exception: - print("Failed to build (%s) with everything disabled" % + print("Failed to build (%s) with things disabled" % (t,)) raise + def find_build_sizes(self): + '''returns a hash with size of all build targets''' + ret = {} + target_to_binpath = { + "copter": "arducopter", + } + for target in self.build_targets(): + path = os.path.join("build", self.board(), "bin", "%s.bin" % target_to_binpath[target]) + ret[target] = os.path.getsize(path) + return ret + + def disable_in_turn_check_sizes(self, feature, sizes_nothing_disabled): + current_sizes = self.find_build_sizes() + for (build, new_size) in current_sizes.items(): + old_size = sizes_nothing_disabled[build] + print("Disabling %s(%s) on %s saves %u bytes" % + (feature.label, feature.define, build, old_size - new_size)) + def run_disable_in_turn(self): options = self.get_build_options_from_ardupilot_tree() count = 1 @@ -77,6 +109,7 @@ def run_disable_in_turn(self): (feature.label, feature.define, count, len(options))) self.test_feature(feature, options) count += 1 + self.disable_in_turn_check_sizes(feature, self.sizes_nothing_disabled) def run_disable_all(self): options = self.get_build_options_from_ardupilot_tree() @@ -85,8 +118,13 @@ def run_disable_all(self): defines[feature.define] = 0 self.test_compile_with_defines(defines) + def run_disable_none(self): + self.test_compile_with_defines({}) + self.sizes_nothing_disabled = self.find_build_sizes() + def run(self): self.run_disable_all() + self.run_disable_none() self.run_disable_in_turn() From 33c7bb21cf11c5a15d6e5c8194be60149e5a9dc7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 9 Feb 2022 10:31:51 +1100 Subject: [PATCH 0106/3101] AC_AutoTune: createstructure to hold specific test's sweep results --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 225 +++++++++++---------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 25 +-- 2 files changed, 126 insertions(+), 124 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index eb860971fdf..06ba328cd2b 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -124,26 +124,26 @@ void AC_AutoTune_Heli::test_init() if (test_phase[12] > 0.0f && test_phase[12] < 180.0f) { freq_cnt = 12; // start with freq found for sweep where phase was 180 deg - } else if (!is_zero(sweep.ph180_freq)) { + } else if (!is_zero(sweep.ph180.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.ph180_freq - 0.25f * 3.14159f * 2.0f; + test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f; // otherwise start at min freq to step up in dwell frequency until phase > 160 deg } else { freq_cnt = 0; test_freq[freq_cnt] = min_sweep_freq; } - curr_test_freq = test_freq[freq_cnt]; - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + curr_test.freq = test_freq[freq_cnt]; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; // MAX_GAINS and RD_UP both start with a sweep initially but if it has been completed then start dwells at the freq for 180 deg phase } else { - if (!is_zero(sweep.ph180_freq)) { + if (!is_zero(sweep.ph180.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.ph180_freq - 0.25f * 3.14159f * 2.0f; - curr_test_freq = test_freq[freq_cnt]; - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + test_freq[freq_cnt] = sweep.ph180.freq - 0.25f * 3.14159f * 2.0f; + curr_test.freq = test_freq[freq_cnt]; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; if (tune_type == MAX_GAINS) { reset_maxgains_update_gain_variables(); } @@ -170,12 +170,12 @@ void AC_AutoTune_Heli::test_init() case SP_UP: // initialize start frequency if (is_zero(start_freq)) { - if (!is_zero(sweep.maxgain_freq)) { + if (!is_zero(sweep.maxgain.freq)) { freq_cnt = 12; - test_freq[freq_cnt] = sweep.maxgain_freq - 0.25f * 3.14159f * 2.0f; - curr_test_freq = test_freq[freq_cnt]; - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + test_freq[freq_cnt] = sweep.maxgain.freq - 0.25f * 3.14159f * 2.0f; + curr_test.freq = test_freq[freq_cnt]; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; test_accel_max = 0.0f; } else { start_freq = min_sweep_freq; @@ -280,7 +280,7 @@ void AC_AutoTune_Heli::do_gcs_announcements() } else { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Sweep"); if (settle_time == 0) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test_freq), (double)(curr_test_gain), (double)(curr_test_phase)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test.freq), (double)(curr_test.gain), (double)(curr_test.phase)); } } break; @@ -335,8 +335,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_rp)); } else { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); } break; case RD_UP: @@ -351,8 +351,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f angle_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_sp)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tune_accel=%f max_accel=%f", (double)(tune_accel), (double)(test_accel_max)); } else { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); } break; case MAX_GAINS: @@ -360,8 +360,8 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { // announce results of dwell and update gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt])); } else { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain_freq), (double)(sweep.maxgain_gain)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180_freq), (double)(sweep.ph180_gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); } break; default: @@ -938,8 +938,8 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq) settle_time = 200; if (!is_equal(start_freq, stop_freq)) { reset_sweep_variables(); - curr_test_gain = 0.0f; - curr_test_phase = 0.0f; + curr_test.gain = 0.0f; + curr_test.phase = 0.0f; } // filter at lower frequency to remove steady state @@ -1129,9 +1129,9 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, } if (freqresp_rate.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { - curr_test_freq = freqresp_rate.get_freq(); - curr_test_gain = freqresp_rate.get_gain(); - curr_test_phase = freqresp_rate.get_phase(); + curr_test.freq = freqresp_rate.get_freq(); + curr_test.gain = freqresp_rate.get_gain(); + curr_test.phase = freqresp_rate.get_phase(); // reset cycle_complete to allow indication of next cycle freqresp_rate.reset_cycle_complete(); // log sweep data @@ -1147,25 +1147,25 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, // set sweep data if a frequency sweep is being conducted if (!is_equal(start_frq,stop_frq) && (float)(now - dwell_start_time_ms) > 2.5f * cycle_time_ms) { // track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped. - if (curr_test_phase > 180.0f && sweep.progress == 0) { + if (curr_test.phase > 180.0f && sweep.progress == 0) { sweep.progress = 1; - } else if (curr_test_phase > 270.0f && sweep.progress == 1) { + } else if (curr_test.phase > 270.0f && sweep.progress == 1) { sweep.progress = 2; } - if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f && sweep.progress == 0) { - sweep.ph180_freq = curr_test_freq; - sweep.ph180_gain = curr_test_gain; - sweep.ph180_phase = curr_test_phase; + if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f && sweep.progress == 0) { + sweep.ph180.freq = curr_test.freq; + sweep.ph180.gain = curr_test.gain; + sweep.ph180.phase = curr_test.phase; } - if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f && sweep.progress == 1) { - sweep.ph270_freq = curr_test_freq; - sweep.ph270_gain = curr_test_gain; - sweep.ph270_phase = curr_test_phase; + if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) { + sweep.ph270.freq = curr_test.freq; + sweep.ph270.gain = curr_test.gain; + sweep.ph270.phase = curr_test.phase; } - if (curr_test_gain > sweep.maxgain_gain) { - sweep.maxgain_gain = curr_test_gain; - sweep.maxgain_freq = curr_test_freq; - sweep.maxgain_phase = curr_test_phase; + if (curr_test.gain > sweep.maxgain.gain) { + sweep.maxgain.gain = curr_test.gain; + sweep.maxgain.freq = curr_test.freq; + sweep.maxgain.phase = curr_test.phase; } if (now - step_start_time_ms >= sweep_time_ms + 200) { // we have passed the maximum stop time @@ -1223,8 +1223,8 @@ void AC_AutoTune_Heli::angle_dwell_test_init(float start_frq, float filt_freq) if (!is_equal(start_freq, stop_freq)) { reset_sweep_variables(); - curr_test_gain = 0.0f; - curr_test_phase = 0.0f; + curr_test.gain = 0.0f; + curr_test.phase = 0.0f; } } @@ -1326,9 +1326,9 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq); if (freqresp_angle.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { - curr_test_freq = freqresp_angle.get_freq(); - curr_test_gain = freqresp_angle.get_gain(); - curr_test_phase = freqresp_angle.get_phase(); + curr_test.freq = freqresp_angle.get_freq(); + curr_test.gain = freqresp_angle.get_gain(); + curr_test.phase = freqresp_angle.get_phase(); test_accel_max = freqresp_angle.get_accel_max(); // reset cycle_complete to allow indication of next cycle freqresp_angle.reset_cycle_complete(); @@ -1344,20 +1344,20 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo // set sweep data if a frequency sweep is being conducted if (!is_equal(start_frq,stop_frq)) { - if (curr_test_phase <= 160.0f && curr_test_phase >= 150.0f) { - sweep.ph180_freq = curr_test_freq; - sweep.ph180_gain = curr_test_gain; - sweep.ph180_phase = curr_test_phase; + if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f) { + sweep.ph180.freq = curr_test.freq; + sweep.ph180.gain = curr_test.gain; + sweep.ph180.phase = curr_test.phase; } - if (curr_test_phase <= 250.0f && curr_test_phase >= 240.0f) { - sweep.ph270_freq = curr_test_freq; - sweep.ph270_gain = curr_test_gain; - sweep.ph270_phase = curr_test_phase; + if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f) { + sweep.ph270.freq = curr_test.freq; + sweep.ph270.gain = curr_test.gain; + sweep.ph270.phase = curr_test.phase; } - if (curr_test_gain > sweep.maxgain_gain) { - sweep.maxgain_gain = curr_test_gain; - sweep.maxgain_freq = curr_test_freq; - sweep.maxgain_phase = curr_test_phase; + if (curr_test.gain > sweep.maxgain.gain) { + sweep.maxgain.gain = curr_test.gain; + sweep.maxgain.freq = curr_test.freq; + sweep.maxgain.phase = curr_test.phase; } if (now - step_start_time_ms >= sweep_time_ms + 200) { // we have passed the maximum stop time @@ -1614,25 +1614,25 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai frq_cnt++; if (frq_cnt == 12) { freq[frq_cnt] = freq[rp_prev_good_frq_cnt]; - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } else { freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } } else if (is_equal(start_freq,stop_freq)) { if (phase[frq_cnt] > 180.0f) { - curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; + curr_test.freq = curr_test.freq - 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test.freq; } else if (phase[frq_cnt] < 160.0f) { - curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; + curr_test.freq = curr_test.freq + 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test.freq; } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { if (gain[frq_cnt] < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) { tune_p += 0.05f * max_gain_p.max_allowed; } else { counter = AUTOTUNE_SUCCESS_COUNT; - // reset curr_test_freq and frq_cnt for next test - curr_test_freq = freq[0]; + // reset curr_test.freq and frq_cnt for next test + curr_test.freq = freq[0]; frq_cnt = 0; tune_p -= 0.05f * max_gain_p.max_allowed; tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed); @@ -1643,8 +1643,8 @@ void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, float *freq, float *gai if (counter == AUTOTUNE_SUCCESS_COUNT) { start_freq = 0.0f; //initializes next test that uses dwell test } else { - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; } } @@ -1655,15 +1655,15 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai // frequency sweep was conducted. check to see if freq for 180 deg phase was determined and start there if it was if (!is_equal(start_freq,stop_freq)) { - if (!is_zero(sweep.ph180_freq)) { - freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + if (!is_zero(sweep.ph180.freq)) { + freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr; frq_cnt = 12; freq_cnt_max = frq_cnt; } else { frq_cnt = 1; freq[frq_cnt] = min_sweep_freq; } - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } // if sweep failed to find frequency for 180 phase then use dwell to find frequency if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { @@ -1679,26 +1679,26 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai frq_cnt++; if (frq_cnt == 12) { freq[frq_cnt] = freq[rd_prev_good_frq_cnt]; - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } else { freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } } else if (is_equal(start_freq,stop_freq)) { if (phase[frq_cnt] > 180.0f) { - curr_test_freq = curr_test_freq - 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; + curr_test.freq = curr_test.freq - 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test.freq; } else if (phase[frq_cnt] < 160.0f) { - curr_test_freq = curr_test_freq + 0.5 * test_freq_incr; - freq[frq_cnt] = curr_test_freq; + curr_test.freq = curr_test.freq + 0.5 * test_freq_incr; + freq[frq_cnt] = curr_test.freq; } else if (phase[frq_cnt] <= 180.0f && phase[frq_cnt] >= 160.0f) { if ((gain[frq_cnt] < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) { tune_d += 0.05f * max_gain_d.max_allowed; rd_prev_gain = gain[frq_cnt]; } else { counter = AUTOTUNE_SUCCESS_COUNT; - // reset curr_test_freq and frq_cnt for next test - curr_test_freq = freq[0]; + // reset curr_test.freq and frq_cnt for next test + curr_test.freq = freq[0]; frq_cnt = 0; rd_prev_gain = 0.0f; tune_d -= 0.05f * max_gain_d.max_allowed; @@ -1710,8 +1710,8 @@ void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, float *freq, float *gai start_freq = 0.0f; //initializes next test that uses dwell test reset_sweep_variables(); } else { - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; } } @@ -1722,16 +1722,16 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga float gain_incr = 0.5f; if (!is_equal(start_freq,stop_freq)) { - if (!is_zero(sweep.maxgain_freq)) { + if (!is_zero(sweep.maxgain.freq)) { frq_cnt = 12; - freq[frq_cnt] = sweep.maxgain_freq - 0.5f * test_freq_incr; + freq[frq_cnt] = sweep.maxgain.freq - 0.5f * test_freq_incr; freq_cnt_max = frq_cnt; } else { frq_cnt = 1; freq[frq_cnt] = min_sweep_freq; freq_cnt_max = 0; } - curr_test_freq = freq[frq_cnt]; + curr_test.freq = freq[frq_cnt]; } if (freq_cnt < 12 && is_equal(start_freq,stop_freq)) { if (gain[freq_cnt] > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) { @@ -1755,10 +1755,10 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga freq_cnt++; if (freq_cnt == 12) { freq[freq_cnt] = freq[freq_cnt_max]; - curr_test_freq = freq[freq_cnt]; + curr_test.freq = freq[freq_cnt]; } else { freq[freq_cnt] = freq[freq_cnt-1] + test_freq_incr; - curr_test_freq = freq[freq_cnt]; + curr_test.freq = freq[freq_cnt]; } } @@ -1778,7 +1778,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); } } - curr_test_freq = freq[freq_cnt]; + curr_test.freq = freq[freq_cnt]; sp_prev_gain = gain[freq_cnt]; } else if (gain[freq_cnt] > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN && !find_peak) { tune_p -= gain_incr; @@ -1790,7 +1790,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga find_peak = false; phase_max = phase[freq_cnt]; } - curr_test_freq = freq[freq_cnt]; + curr_test.freq = freq[freq_cnt]; sp_prev_gain = gain[freq_cnt]; } else { // adjust tuning gain so max response gain is not exceeded @@ -1804,11 +1804,11 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga if (counter == AUTOTUNE_SUCCESS_COUNT) { start_freq = 0.0f; //initializes next test that uses dwell test reset_sweep_variables(); - curr_test_freq = freq[0]; + curr_test.freq = freq[0]; freq_cnt = 0; } else { - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; } } @@ -1819,14 +1819,14 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase if (!is_equal(start_freq,stop_freq)) { frq_cnt = 2; - if (!is_zero(sweep.ph180_freq)) { - freq[frq_cnt] = sweep.ph180_freq - 0.5f * test_freq_incr; + if (!is_zero(sweep.ph180.freq)) { + freq[frq_cnt] = sweep.ph180.freq - 0.5f * test_freq_incr; } else { freq[frq_cnt] = min_sweep_freq; } - curr_test_freq = freq[frq_cnt]; - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + curr_test.freq = freq[frq_cnt]; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; } else if (frq_cnt < 12 && is_equal(start_freq,stop_freq)) { if (frq_cnt > 2 && phase[frq_cnt] > 161.0f && phase[frq_cnt] < 270.0f && @@ -1849,11 +1849,11 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase found_max_p = true; find_middle = false; - if (!is_zero(sweep.ph270_freq)) { + if (!is_zero(sweep.ph270.freq)) { // set freq cnt back to reinitialize process frq_cnt = 1; // set to 1 because it will be incremented // set frequency back at least one test_freq_incr as it will be added - freq[1] = sweep.ph270_freq - 1.5f * test_freq_incr; + freq[1] = sweep.ph270.freq - 1.5f * test_freq_incr; } } if (frq_cnt > 2 && phase[frq_cnt] > 251.0f && phase[frq_cnt] < 360.0f && @@ -1884,7 +1884,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase if (frq_cnt == 12) { counter = AUTOTUNE_SUCCESS_COUNT; // reset variables for next test - curr_test_freq = freq[0]; + curr_test.freq = freq[0]; frq_cnt = 0; start_freq = 0.0f; //initializes next test that uses dwell test reset_sweep_variables(); @@ -1904,9 +1904,9 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase } else { freq[frq_cnt] = freq[frq_cnt-1] + test_freq_incr; } - curr_test_freq = freq[frq_cnt]; - start_freq = curr_test_freq; - stop_freq = curr_test_freq; + curr_test.freq = freq[frq_cnt]; + start_freq = curr_test.freq; + stop_freq = curr_test.freq; } } if (found_max_p && found_max_d) { @@ -1948,7 +1948,7 @@ void AC_AutoTune_Heli::Log_AutoTuneDetails() // log autotune frequency response data void AC_AutoTune_Heli::Log_AutoTuneSweep() { - Log_Write_AutoTuneSweep(curr_test_freq, curr_test_gain, curr_test_phase); + Log_Write_AutoTuneSweep(curr_test.freq, curr_test.gain, curr_test.phase); } // @LoggerMessage: ATNH @@ -2091,15 +2091,16 @@ void AC_AutoTune_Heli::reset_maxgains_update_gain_variables() // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_sweep_variables() { - sweep.ph180_freq = 0.0f; - sweep.ph180_gain = 0.0f; - sweep.ph180_phase = 0.0f; - sweep.ph270_freq = 0.0f; - sweep.ph270_gain = 0.0f; - sweep.ph270_phase = 0.0f; - sweep.maxgain_gain = 0.0f; - sweep.maxgain_freq = 0.0f; - sweep.maxgain_phase = 0.0f; + sweep.ph180.freq = 0.0f; + sweep.ph180.gain = 0.0f; + sweep.ph180.phase = 0.0f; + sweep.ph270.freq = 0.0f; + sweep.ph270.gain = 0.0f; + sweep.ph270.phase = 0.0f; + sweep.maxgain.gain = 0.0f; + sweep.maxgain.freq = 0.0f; + sweep.maxgain.phase = 0.0f; + sweep.progress = 0; } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index cf0fa5f3bee..ce6937cac6c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -222,9 +222,15 @@ class AC_AutoTune_Heli : public AC_AutoTune float test_phase[20]; // frequency response phase for each dwell test iteration float dwell_start_time_ms; // start time in ms of dwell test uint8_t freq_cnt_max; // counter number for frequency that produced max gain response - float curr_test_freq; // current test frequency - float curr_test_gain; // current test frequency response gain - float curr_test_phase; // current test frequency response phase + + // sweep_info contains information about a specific test's sweep results + struct sweep_info { + float freq; + float gain; + float phase; + }; + sweep_info curr_test; + Vector3f start_angles; // aircraft attitude at the start of test uint32_t settle_time; // time in ms for allowing aircraft to stabilize before initiating test uint32_t phase_out_time; // time in ms to phase out response @@ -257,15 +263,10 @@ class AC_AutoTune_Heli : public AC_AutoTune // sweep_data tracks the overall characteristics in the response to the frequency sweep struct sweep_data { - float maxgain_freq; - float maxgain_gain; - float maxgain_phase; - float ph180_freq; - float ph180_gain; - float ph180_phase; - float ph270_freq; - float ph270_gain; - float ph270_phase; + sweep_info maxgain; + sweep_info ph180; + sweep_info ph270; + uint8_t progress; // set based on phase of frequency response. 0 - start; 1 - reached 180 deg; 2 - reached 270 deg; }; sweep_data sweep; From 7ff6442aeb4ba66fdd266e12d919f8c7f745cda5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 9 Feb 2022 10:42:14 +1100 Subject: [PATCH 0107/3101] AC_AutoTune: use structure assignment for test results --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 06ba328cd2b..322da44dee8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -1345,19 +1345,13 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo // set sweep data if a frequency sweep is being conducted if (!is_equal(start_frq,stop_frq)) { if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f) { - sweep.ph180.freq = curr_test.freq; - sweep.ph180.gain = curr_test.gain; - sweep.ph180.phase = curr_test.phase; + sweep.ph180 = curr_test; } if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f) { - sweep.ph270.freq = curr_test.freq; - sweep.ph270.gain = curr_test.gain; - sweep.ph270.phase = curr_test.phase; + sweep.ph270 = curr_test; } if (curr_test.gain > sweep.maxgain.gain) { - sweep.maxgain.gain = curr_test.gain; - sweep.maxgain.freq = curr_test.freq; - sweep.maxgain.phase = curr_test.phase; + sweep.maxgain = curr_test; } if (now - step_start_time_ms >= sweep_time_ms + 200) { // we have passed the maximum stop time From 95393623205f315a1c1ebc4daf1175815b8d8b96 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 9 Feb 2022 10:46:44 +1100 Subject: [PATCH 0108/3101] AC_AutoTune_Heli: use structure assignment to clear structures --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 322da44dee8..876b84574c8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -2068,14 +2068,8 @@ void AC_AutoTune_Heli::reset_update_gain_variables() // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_maxgains_update_gain_variables() { - max_rate_p.freq = 0.0f; - max_rate_p.gain = 0.0f; - max_rate_p.phase = 0.0f; - max_rate_p.max_allowed = 0.0f; - max_rate_d.freq = 0.0f; - max_rate_d.gain = 0.0f; - max_rate_d.phase = 0.0f; - max_rate_d.max_allowed = 0.0f; + max_rate_p = {}; + max_rate_d = {}; found_max_p = false; found_max_d = false; find_middle = false; @@ -2085,15 +2079,9 @@ void AC_AutoTune_Heli::reset_maxgains_update_gain_variables() // reset the max_gains update gain variables void AC_AutoTune_Heli::reset_sweep_variables() { - sweep.ph180.freq = 0.0f; - sweep.ph180.gain = 0.0f; - sweep.ph180.phase = 0.0f; - sweep.ph270.freq = 0.0f; - sweep.ph270.gain = 0.0f; - sweep.ph270.phase = 0.0f; - sweep.maxgain.gain = 0.0f; - sweep.maxgain.freq = 0.0f; - sweep.maxgain.phase = 0.0f; + sweep.ph180 = {}; + sweep.ph270 = {}; + sweep.maxgain = {}; sweep.progress = 0; } From 28bc05eb8430619d16f96ee7f617f94e2add8ea0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 9 Feb 2022 10:58:17 +1100 Subject: [PATCH 0109/3101] AC_AutoTune: Vector tidying --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 24 ++++++++++++---------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 876b84574c8..2922ed052a5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -1031,13 +1031,11 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, } // limit rate correction for position hold - Vector3f trim_rate_cds; - trim_rate_cds.x = att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x); - trim_rate_cds.y = att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y); - trim_rate_cds.z = att_hold_gain * filt_heading_error_cd.get(); - trim_rate_cds.x = constrain_float(trim_rate_cds.x, -15000.0f, 15000.0f); - trim_rate_cds.y = constrain_float(trim_rate_cds.y, -15000.0f, 15000.0f); - trim_rate_cds.z = constrain_float(trim_rate_cds.z, -15000.0f, 15000.0f); + Vector3f trim_rate_cds { + constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f), + constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f), + constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f) + }; switch (axis) { case ROLL: @@ -1268,7 +1266,10 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo dwell_freq = waveform_freq_rads; } } - Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x); + const Vector2f att_fdbk { + -5730.0f * vel_hold_gain * velocity_bf.y, + 5730.0f * vel_hold_gain * velocity_bf.x + }; filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); } else { target_angle_cd = 0.0f; @@ -1279,9 +1280,10 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); } - Vector2f trim_angle_cd; - trim_angle_cd.x = constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f); - trim_angle_cd.y = constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f); + const Vector2f trim_angle_cd { + constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f), + constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f) + }; switch (axis) { case ROLL: attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); From 452121eca130394271dc00871b9a8761984bce56 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 6 Mar 2022 07:01:26 +1100 Subject: [PATCH 0110/3101] Tools: fixed env install for ubuntu 22.04 --- Tools/environment_install/install-prereqs-ubuntu.sh | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index eddef64ef4e..67287065d6a 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -76,6 +76,11 @@ elif [ ${RELEASE_CODENAME} == 'focal' ] || [ ${RELEASE_CODENAME} == 'ulyssa' ]; SITLCFML_VERSION="2.5" PYTHON_V="python3" PIP=pip3 +elif [ ${RELEASE_CODENAME} == 'jammy' ]; then + SITLFML_VERSION="2.5" + SITLCFML_VERSION="2.5" + PYTHON_V="python3" + PIP=pip3 elif [ ${RELEASE_CODENAME} == 'groovy' ] || [ ${RELEASE_CODENAME} == 'hirsute' ] || [ ${RELEASE_CODENAME} == 'bullseye' ] || @@ -113,7 +118,7 @@ if [ "$ARM_PKG_CONFIG_NOT_PRESENT" -eq 1 ]; then $APT_GET install pkg-config if [ -f /usr/share/pkg-config-crosswrapper ]; then # We are on non-Ubuntu so simulate effect of installing pkg-config-arm-linux-gnueabihf. - sudo ln -s /usr/share/pkg-config-crosswrapper /usr/bin/arm-linux-gnueabihf-pkg-config + sudo ln -sf /usr/share/pkg-config-crosswrapper /usr/bin/arm-linux-gnueabihf-pkg-config else echo "Warning: unable to link to pkg-config-crosswrapper" fi @@ -198,6 +203,7 @@ then elif [ ${RELEASE_CODENAME} == 'groovy' ] || [ ${RELEASE_CODENAME} == 'hirsute' ] || [ ${RELEASE_CODENAME} == 'bullseye' ] || + [ ${RELEASE_CODENAME} == 'jammy' ] || [ ${RELEASE_CODENAME} == 'impish' ]; then BASE_PKGS+=" python-is-python3" SITL_PKGS+=" libpython3-stdlib" # for argparse @@ -229,6 +235,7 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then [ ${RELEASE_CODENAME} == 'hirsute' ] || [ ${RELEASE_CODENAME} == 'focal' ] || [ ${RELEASE_CODENAME} == 'ulyssa' ] || + [ ${RELEASE_CODENAME} == 'jammy' ] || [ ${RELEASE_CODENAME} == 'impish' ]; then SITL_PKGS+=" python3-wxgtk4.0" SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame From 360ddb2aedf92718eea8a97d5dc0b9d8b07b2c20 Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Sun, 6 Mar 2022 22:04:07 +0530 Subject: [PATCH 0111/3101] hwdef:update storage pages and enable watchdog --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat index 7a22fbd3bf4..e44d80a0dfb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat @@ -7,8 +7,8 @@ MCU STM32F4xx STM32F405xx FLASH_RESERVE_START_KB 64 FLASH_SIZE_KB 1024 -# store parameters in pages 11 and 12 -STORAGE_FLASH_PAGE 1 +# store parameters in pages 2 and 3 +STORAGE_FLASH_PAGE 2 define HAL_STORAGE_SIZE 15360 # board ID for firmware load @@ -20,6 +20,7 @@ define STM32_ST_USE_TIMER 5 define CH_CFG_ST_RESOLUTION 32 # enable watchdog +define HAL_WATCHDOG_ENABLED_DEFAULT true # crystal frequency OSCILLATOR_HZ 16000000 @@ -128,4 +129,3 @@ PC7 M9SB INPUT # allow for reboot command for faster development define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 - From 5cd1c7783d108c848f1867690c06298ee72f182e Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Sun, 6 Mar 2022 22:07:23 +0530 Subject: [PATCH 0112/3101] hwdef: hwdef update with watchdog --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat index 6f941affc5b..6c399c81b66 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat @@ -16,6 +16,9 @@ define HAL_STORAGE_SIZE 800 STM32_ST_USE_TIMER 15 define CH_CFG_ST_RESOLUTION 16 +# enable watchdog +define HAL_WATCHDOG_ENABLED_DEFAULT true + # board ID for firmware load APJ_BOARD_ID 1050 @@ -61,6 +64,9 @@ COMPASS RM3100 SPI:rm3100 false ROTATION_PITCH_180 PB6 I2C1_SCL I2C1 PB7 I2C1_SDA I2C1 +define HAL_I2C_CLEAR_ON_TIMEOUT 0 +define HAL_I2C_INTERNAL_MASK 1 + I2C_ORDER I2C1 # allow for reboot command for faster development From 057be63fdd53ae085f89665e33eaacfe51174025 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 24 Dec 2021 00:54:54 +1030 Subject: [PATCH 0113/3101] AC_AttitudeControl: AC_PosControl: add soften for landing --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 12 +++++++++++- libraries/AC_AttitudeControl/AC_PosControl.h | 3 +++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0f8b6e7c48b..b776f9a79f4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -463,9 +463,19 @@ void AC_PosControl::relax_velocity_controller_xy() _pid_vel_xy.set_integrator(_accel_target - _accel_desired); } +/// reduce response for landing +void AC_PosControl::soften_for_landing_xy() +{ + // set target position to current position + _pos_target.xy() = _inav.get_position_xy_cm().topostype(); + + // also prevent I term build up in xy velocity controller. Note + // that this flag is reset on each loop, in update_xy_controller() + set_externally_limited_xy(); +} + /// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. -/// This function is private and contains all the shared xy axis initialisation functions void AC_PosControl::init_xy_controller() { // set roll, pitch lean angle targets to current attitude diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index d19de899613..7ba799ff657 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -96,6 +96,9 @@ class AC_PosControl /// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration. void relax_velocity_controller_xy(); + /// reduce response for landing + void soften_for_landing_xy(); + // init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. /// This function is private and contains all the shared xy axis initialisation functions From 376dc729075b3adf075d2736d3bf498820be4581 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 24 Dec 2021 00:56:33 +1030 Subject: [PATCH 0114/3101] AC_Loiter: use Pos_Control soften_for_landing_xy --- libraries/AC_WPNav/AC_Loiter.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 30f506529ad..edfd47ada61 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -132,14 +132,7 @@ void AC_Loiter::init_target() /// reduce response for landing void AC_Loiter::soften_for_landing() { - const Vector3f& curr_pos = _inav.get_position_neu_cm(); - - // set target position to current position - _pos_control.set_pos_target_xy_cm(curr_pos.x, curr_pos.y); - - // also prevent I term build up in xy velocity controller. Note - // that this flag is reset on each loop, in update_xy_controller() - _pos_control.set_externally_limited_xy(); + _pos_control.soften_for_landing_xy(); } /// set pilot desired acceleration in centi-degrees From b5a4f2455918c63ab48659c84dc77d8ab5728272 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 2 Feb 2022 14:10:27 +1030 Subject: [PATCH 0115/3101] Copter: use position controller for landing reposition --- ArduCopter/mode.cpp | 68 ++++++++++++++++++++++++++++----------- ArduCopter/mode.h | 1 + ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_rtl.cpp | 19 +++++------ 4 files changed, 59 insertions(+), 31 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 7eda3f3c93a..4e7054c8882 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -431,6 +431,39 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, floa // roll_out and pitch_out are returned } +// transform pilot's roll or pitch input into a desired velocity +Vector2f Mode::get_pilot_desired_velocity(float vel_max) const +{ + Vector2f vel; + + // throttle failsafe check + if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { + return vel; + } + // fetch roll and pitch inputs + float roll_out = channel_roll->get_control_in(); + float pitch_out = channel_pitch->get_control_in(); + + // convert roll and pitch inputs to -1 to +1 range + float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX; + roll_out *= scaler; + pitch_out *= scaler; + + // convert roll and pitch inputs into velocity in NE frame + vel = Vector2f(-pitch_out, roll_out); + if (vel.is_zero()) { + return vel; + } + copter.rotate_body_frame_to_NE(vel.x, vel.y); + + // Transform square input range to circular output + // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input + Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y)); + // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max + vel *= vel_max / vel_scaler.length(); + return vel; +} + bool Mode::_TakeOff::triggered(const float target_climb_rate) const { if (!copter.ap.land_complete) { @@ -598,13 +631,12 @@ void Mode::land_run_vertical_control(bool pause_descent) void Mode::land_run_horizontal_control() { - float target_roll = 0.0f; - float target_pitch = 0.0f; + Vector2f vel_correction; float target_yaw_rate = 0; // relax loiter target if we might be landed if (copter.ap.land_complete_maybe) { - loiter_nav->soften_for_landing(); + pos_control->soften_for_landing_xy(); } // process pilot inputs @@ -621,11 +653,14 @@ void Mode::land_run_horizontal_control() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); + // convert pilot input to reposition velocity + // use half maximum acceleration as the maximum velocity to ensure aircraft will + // stop from full reposition speed in less than 1 second. + const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5; + vel_correction = get_pilot_desired_velocity(max_pilot_vel); // record if pilot has overridden roll or pitch - if (!is_zero(target_roll) || !is_zero(target_pitch)) { + if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } @@ -641,11 +676,11 @@ void Mode::land_run_horizontal_control() } // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle - bool doing_precision_landing = false; + copter.ap.prec_land_active = false; #if PRECISION_LANDING == ENABLED - doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired(); + copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing - if (doing_precision_landing) { + if (copter.ap.prec_land_active) { Vector2f target_pos, target_vel; if (!copter.precland.get_target_position_cm(target_pos)) { target_pos = inertial_nav.get_position_xy_cm(); @@ -657,12 +692,10 @@ void Mode::land_run_horizontal_control() Vector2p landing_pos = target_pos.topostype(); // target vel will remain zero if landing target is stationary pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero); - // run pos controller - pos_control->update_xy_controller(); } #endif - if (!doing_precision_landing) { + if (!copter.ap.prec_land_active) { if (copter.ap.prec_land_active) { // precland isn't active anymore but was active a while back // lets run an init again @@ -671,15 +704,12 @@ void Mode::land_run_horizontal_control() loiter_nav->get_stopping_point_xy(stopping_point); loiter_nav->init_target(stopping_point); } - // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); - - // run loiter controller - loiter_nav->update(); + Vector2f accel; + pos_control->input_vel_accel_xy(vel_correction, accel); } - copter.ap.prec_land_active = doing_precision_landing; - + // run pos controller + pos_control->update_xy_controller(); Vector3f thrust_vector = pos_control->get_thrust_vector(); if (g2.wp_navalt_min > 0) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index cc27ab62474..4c9ee573aed 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -90,6 +90,7 @@ class Mode { // pilot input processing void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const; + Vector2f get_pilot_desired_velocity(float vel_max) const; float get_pilot_desired_yaw_rate(float yaw_in); float get_pilot_desired_throttle() const; diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 29fe7803d0f..dae8ecdb654 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -47,7 +47,7 @@ void ModeBrake::run() // relax stop target if we might be landed if (copter.ap.land_complete_maybe) { - loiter_nav->soften_for_landing(); + pos_control->soften_for_landing_xy(); } // use position controller to stop diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9515334c3fd..6c148e57d0d 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -297,8 +297,7 @@ void ModeRTL::descent_start() // called by rtl_run at 100hz or more void ModeRTL::descent_run() { - float target_roll = 0.0f; - float target_pitch = 0.0f; + Vector2f vel_correction; float target_yaw_rate = 0.0f; // if not armed set throttle to zero and exit immediately @@ -321,11 +320,11 @@ void ModeRTL::descent_run() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); + // convert pilot input to reposition velocity + vel_correction = get_pilot_desired_velocity(wp_nav->get_wp_acceleration() * 0.5); // record if pilot has overridden roll or pitch - if (!is_zero(target_roll) || !is_zero(target_pitch)) { + if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } @@ -342,11 +341,9 @@ void ModeRTL::descent_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); - - // run loiter controller - loiter_nav->update(); + Vector2f accel; + pos_control->input_vel_accel_xy(vel_correction, accel); + pos_control->update_xy_controller(); // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle @@ -354,7 +351,7 @@ void ModeRTL::descent_run() pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; From 0ce44ad1ba22b8c117b83e020bd5d28dccccc000 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 1 Feb 2022 01:31:44 +1030 Subject: [PATCH 0116/3101] AC_WPNav: init optionally accepts stopping point --- libraries/AC_WPNav/AC_WPNav.cpp | 15 +++++++++------ libraries/AC_WPNav/AC_WPNav.h | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 05a0bf125df..d1490944342 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -143,9 +143,9 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const /// wp_and_spline_init - initialise straight line and spline waypoint controllers /// speed_cms should be a positive value or left at zero to use the default speed -/// updates target roll, pitch targets and I terms based on vehicle lean angles +/// stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination -void AC_WPNav::wp_and_spline_init(float speed_cms) +void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) { // sanity check parameters @@ -185,13 +185,13 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) _scurve_next_leg.init(); _track_scalar_dt = 1.0f; - // set flag so get_yaw() returns current heading target - _flags.reached_destination = false; + _flags.reached_destination = true; _flags.fast_waypoint = false; // initialise origin and destination to stopping point - Vector3f stopping_point; - get_wp_stopping_point(stopping_point); + if (stopping_point.is_zero()) { + get_wp_stopping_point(stopping_point); + } _origin = _destination = stopping_point; _terrain_alt = false; _this_leg_is_spline = false; @@ -199,6 +199,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms) // initialise the terrain velocity to the current maximum velocity _terrain_vel = _wp_desired_speed_xy_cms; _terrain_accel = 0.0; + + // mark as active + _wp_last_update = AP_HAL::millis(); } /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index b5e1805d718..451b4998a2c 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -52,7 +52,7 @@ class AC_WPNav /// speed_cms is the desired max speed to travel between waypoints. should be a positive value or omitted to use the default speed /// updates target roll, pitch targets and I terms based on vehicle lean angles /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination - void wp_and_spline_init(float speed_cms = 0.0f); + void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}); /// set current target horizontal speed during wp navigation void set_speed_xy(float speed_cms); From fb6c3ebb729a4b428da5dd14832cd95d71709a1e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 11 Feb 2022 13:35:43 +1030 Subject: [PATCH 0117/3101] Copter: auto and guided takeoff use postion controller --- ArduCopter/mode.h | 16 +++++-- ArduCopter/mode_auto.cpp | 85 ++++++++++++++++++++++++------------- ArduCopter/mode_guided.cpp | 35 ++++++++------- ArduCopter/takeoff.cpp | 87 +++++++++++++++++++++++++++----------- 4 files changed, 150 insertions(+), 73 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 4c9ee573aed..ea51472f422 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -197,15 +197,23 @@ class Mode { virtual bool do_user_takeoff_start(float takeoff_alt_cm); - // method shared by both Guided and Auto for takeoff. This is - // waypoint navigation but the user can control the yaw. + // method shared by both Guided and Auto for takeoff. + // position controller controls vehicle but the user can control the yaw. void auto_takeoff_run(); - void auto_takeoff_set_start_alt(void); + void auto_takeoff_start(float complete_alt_cm, bool terrain_alt); + bool auto_takeoff_get_position(Vector3p& completion_pos); // altitude above-ekf-origin below which auto takeoff does not control horizontal position static bool auto_takeoff_no_nav_active; static float auto_takeoff_no_nav_alt_cm; + // auto takeoff variables + static float auto_take_off_start_alt_cm; // start altitude expressed as cm above ekf origin + static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin + static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain + static bool auto_takeoff_complete; // true when takeoff is complete + static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm + public: // Navigation Yaw control class AutoYaw { @@ -940,6 +948,8 @@ class ModeGuided : public Mode { bool is_taking_off() const override; + // initialises position controller to implement take-off + // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available bool do_user_takeoff_start(float takeoff_alt_cm) override; enum class SubMode { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 8e3a56c9f2a..d7136944b8c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -262,39 +262,43 @@ void ModeAuto::takeoff_start(const Location& dest_loc) { _mode = SubMode::TAKEOFF; - Location dest(dest_loc); - if (!copter.current_loc.initialised()) { // vehicle doesn't know where it is ATM. We should not // initialise our takeoff destination without knowing this! return; } - // set horizontal target - dest.lat = copter.current_loc.lat; - dest.lng = copter.current_loc.lng; + // calculate current and target altitudes + // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin + int32_t alt_target_cm; + bool alt_target_terrain = false; + float current_alt_cm = inertial_nav.get_position_z_up_cm(); + float terrain_offset; // terrain's altitude in cm above the ekf origin + if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset(terrain_offset)) { + // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain + current_alt_cm -= terrain_offset; - // get altitude target - int32_t alt_target; - if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) { - // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); - // fall back to altitude above current altitude - alt_target = copter.current_loc.alt + dest.alt; + // specify alt_target_cm as alt-above-terrain + alt_target_cm = dest_loc.alt; + alt_target_terrain = true; + } else { + // set horizontal target + Location dest(dest_loc); + dest.lat = copter.current_loc.lat; + dest.lng = copter.current_loc.lng; + + // get altitude target above EKF origin + if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { + // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data + AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + // fall back to altitude above current altitude + alt_target_cm = current_alt_cm + dest.alt; + } } // sanity check target - int32_t alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0); - if (alt_target < alt_target_min_cm ) { - dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME); - } - - // set waypoint controller target - if (!wp_nav->set_wp_destination_loc(dest)) { - // failure to set destination can only be because of missing terrain data - copter.failsafe_terrain_on_event(); - return; - } + int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0); + alt_target_cm = MAX(alt_target_cm, alt_target_min_cm); // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); @@ -302,13 +306,25 @@ void ModeAuto::takeoff_start(const Location& dest_loc) // clear i term when we're taking off set_throttle_takeoff(); - // get initial alt for WP_NAVALT_MIN - auto_takeoff_set_start_alt(); + // initialise alt for WP_NAVALT_MIN and set completion alt + auto_takeoff_start(alt_target_cm, alt_target_terrain); } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination void ModeAuto::wp_start(const Location& dest_loc) { + // init wpnav and set origin if transitioning from takeoff + if (!wp_nav->is_active()) { + Vector3f stopping_point; + if (_mode == SubMode::TAKEOFF) { + Vector3p takeoff_complete_pos; + if (auto_takeoff_get_position(takeoff_complete_pos)) { + stopping_point = takeoff_complete_pos.tofloat(); + } + } + wp_nav->wp_and_spline_init(0, stopping_point); + } + // send target to waypoint controller if (!wp_nav->set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data @@ -1194,6 +1210,18 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) } } + // init wpnav and set origin if transitioning from takeoff + if (!wp_nav->is_active()) { + Vector3f stopping_point; + if (_mode == SubMode::TAKEOFF) { + Vector3p takeoff_complete_pos; + if (auto_takeoff_get_position(takeoff_complete_pos)) { + stopping_point = takeoff_complete_pos.tofloat(); + } + } + wp_nav->wp_and_spline_init(0, stopping_point); + } + // get waypoint's location from command and send to wp_nav const Location dest_loc = loc_from_cmd(cmd, default_loc); if (!wp_nav->set_wp_destination_loc(dest_loc)) { @@ -1637,18 +1665,15 @@ void ModeAuto::do_RTL(void) // verify_takeoff - check if we have completed the takeoff bool ModeAuto::verify_takeoff() { - // have we reached our target altitude? - const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); - #if LANDING_GEAR_ENABLED == ENABLED // if we have reached our destination - if (reached_wp_dest) { + if (auto_takeoff_complete) { // retract the landing gear copter.landinggear.retract_after_takeoff(); } #endif - return reached_wp_dest; + return auto_takeoff_complete; } // verify_land - returns true if landing has been completed diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 8ed8c8eacb0..3b1214c8912 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -98,14 +98,15 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0; }; -// do_user_takeoff_start - initialises waypoint controller to implement take-off +// initialises position controller to implement take-off +// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) { guided_mode = SubMode::TakeOff; - // initialise wpnav destination - Location target_loc = copter.current_loc; - Location::AltFrame frame = Location::AltFrame::ABOVE_HOME; + // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain) + int32_t alt_target_cm; + bool alt_target_terrain = false; if (wp_nav->rangefinder_used_and_healthy() && wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER && takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) { @@ -113,15 +114,19 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) { return false; } - frame = Location::AltFrame::ABOVE_TERRAIN; - } - target_loc.set_alt_cm(takeoff_alt_cm, frame); + // provide target altitude as alt-above-terrain + alt_target_cm = takeoff_alt_cm; + alt_target_terrain = true; + } else { + // interpret altitude as alt-above-home + Location target_loc = copter.current_loc; + target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME); - if (!wp_nav->set_wp_destination_loc(target_loc)) { - // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); - // failure is propagated to GCS with NAK - return false; + // provide target altitude as alt-above-ekf-origin + if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { + // this should never happen but we reject the command just in case + return false; + } } // initialise yaw @@ -130,8 +135,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) // clear i term when we're taking off set_throttle_takeoff(); - // get initial alt for WP_NAVALT_MIN - auto_takeoff_set_start_alt(); + // initialise alt for WP_NAVALT_MIN and set completion alt + auto_takeoff_start(alt_target_cm, alt_target_terrain); // record takeoff has not completed takeoff_complete = false; @@ -629,7 +634,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ void ModeGuided::takeoff_run() { auto_takeoff_run(); - if (!takeoff_complete && wp_nav->reached_wp_destination()) { + if (auto_takeoff_complete && !takeoff_complete) { takeoff_complete = true; #if LANDING_GEAR_ENABLED == ENABLED // optionally retract landing gear diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 687287336b0..4f12db12566 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -4,6 +4,11 @@ Mode::_TakeOff Mode::takeoff; bool Mode::auto_takeoff_no_nav_active = false; float Mode::auto_takeoff_no_nav_alt_cm = 0; +float Mode::auto_take_off_start_alt_cm = 0; +float Mode::auto_take_off_complete_alt_cm = 0; +bool Mode::auto_takeoff_terrain_alt = false; +bool Mode::auto_takeoff_complete = false; +Vector3p Mode::auto_takeoff_complete_pos; // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude @@ -93,13 +98,21 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) } } +// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes +// auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate void Mode::auto_takeoff_run() { // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed) { // do not spool down tradheli when on the ground with motor interlock enabled make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); - wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); + return; + } + + // get terrain offset + float terr_offset = 0.0f; + if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "auto takeoff: failed to get terrain offset"); return; } @@ -116,17 +129,18 @@ void Mode::auto_takeoff_run() } } - // aircraft stays in landed state until rotor speed runup has finished + // aircraft stays in landed state until rotor speed run up has finished if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { set_land_complete(false); } else { // motors have not completed spool up yet so relax navigation and position controllers - wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); + pos_control->relax_velocity_controller_xy(); + pos_control->update_xy_controller(); pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero pos_control->update_z_controller(); attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); return; } @@ -135,51 +149,74 @@ void Mode::auto_takeoff_run() // check if vehicle has reached no_nav_alt threshold if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) { auto_takeoff_no_nav_active = false; - wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy(); - } else { - // shift the navigation target horizontally to our current position - wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); } - // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup - pos_control->set_externally_limited_xy(); - } - - // run waypoint controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - - Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f}; - if (!auto_takeoff_no_nav_active) { - thrustvector = wp_nav->get_thrust_vector(); + pos_control->relax_velocity_controller_xy(); + } else { + Vector2f vel; + Vector2f accel; + pos_control->input_vel_accel_xy(vel, accel); } + pos_control->update_xy_controller(); - // WP_Nav has set the vertical position control targets + // command the aircraft to the take off altitude + float pos_z = auto_take_off_complete_alt_cm + terr_offset; + float vel_z = 0.0; + copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0); + // run the vertical position controller and set output throttle - copter.pos_control->update_z_controller(); + pos_control->update_z_controller(); // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from position controller, yaw rate from pilot - attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from position controller, yaw rate from mavlink command or mission item - attitude_control->input_thrust_vector_rate_heading(thrustvector, auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll & pitch from position controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(thrustvector, auto_yaw.yaw(), auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); + } + + // handle takeoff completion + bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_start_alt_cm) * 0.90); + bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.5; + auto_takeoff_complete = reached_altitude && reached_climb_rate; + + // calculate completion for location in case it is needed for a smooth transition to wp_nav + if (auto_takeoff_complete) { + const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm(); + auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z}; } } -void Mode::auto_takeoff_set_start_alt(void) +void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt) { + auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm(); + auto_take_off_complete_alt_cm = complete_alt_cm; + auto_takeoff_terrain_alt = terrain_alt; + auto_takeoff_complete = false; if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + auto_takeoff_no_nav_alt_cm = auto_take_off_start_alt_cm + g2.wp_navalt_min * 100; auto_takeoff_no_nav_active = true; } else { auto_takeoff_no_nav_active = false; } } +// return takeoff final position if takeoff has completed successfully +bool Mode::auto_takeoff_get_position(Vector3p& complete_pos) +{ + // only provide location if takeoff has completed + if (!auto_takeoff_complete) { + return false; + } + + complete_pos = auto_takeoff_complete_pos; + return true; +} + bool Mode::is_taking_off() const { if (!has_user_takeoff(false)) { From 56e47cb8ccf5232b093ac978e7f761da65eebea6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 31 Jan 2022 00:45:30 +1030 Subject: [PATCH 0118/3101] AC_PosControl: Decay posiiton error during relax --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index b776f9a79f4..ec9c84922b3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -458,7 +458,6 @@ void AC_PosControl::relax_velocity_controller_xy() // decay resultant acceleration and therefore current attitude target to zero float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC); - _accel_target.xy() *= decay; _pid_vel_xy.set_integrator(_accel_target - _accel_desired); } @@ -466,11 +465,11 @@ void AC_PosControl::relax_velocity_controller_xy() /// reduce response for landing void AC_PosControl::soften_for_landing_xy() { - // set target position to current position - _pos_target.xy() = _inav.get_position_xy_cm().topostype(); + // decay position error to zero + _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); - // also prevent I term build up in xy velocity controller. Note - // that this flag is reset on each loop, in update_xy_controller() + // Prevent I term build up in xy velocity controller. + // Note that this flag is reset on each loop in update_xy_controller() set_externally_limited_xy(); } From 93cff95448fe9e5ceddc7f55580015cacdd775e6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 22 Feb 2022 23:53:00 +1030 Subject: [PATCH 0119/3101] Copter: remove loiter_nav from auto --- ArduCopter/mode.cpp | 8 ---- ArduCopter/mode.h | 2 - ArduCopter/mode_auto.cpp | 87 ++++++++++++++++++++-------------------- ArduCopter/mode_land.cpp | 16 ++++---- ArduCopter/mode_rtl.cpp | 15 +++---- 5 files changed, 59 insertions(+), 69 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4e7054c8882..f475e7ffc88 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -696,14 +696,6 @@ void Mode::land_run_horizontal_control() #endif if (!copter.ap.prec_land_active) { - if (copter.ap.prec_land_active) { - // precland isn't active anymore but was active a while back - // lets run an init again - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - loiter_nav->init_target(stopping_point); - } Vector2f accel; pos_control->input_vel_accel_xy(vel_correction, accel); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index ea51472f422..3becb062342 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -424,7 +424,6 @@ class ModeAuto : public Mode { void takeoff_start(const Location& dest_loc); void wp_start(const Location& dest_loc); void land_start(); - void land_start(const Vector2f& destination); void circle_movetoedge_start(const Location &circle_center, float radius_m); void circle_start(); void nav_guided_start(); @@ -495,7 +494,6 @@ class ModeAuto : public Mode { Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const; - void payload_place_start(const Vector2f& destination); void payload_place_run(); bool payload_place_run_should_run(); void payload_place_run_loiter(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index d7136944b8c..0aa7d09c2af 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -344,21 +344,20 @@ void ModeAuto::wp_start(const Location& dest_loc) // auto_land_start - initialises controller to implement a landing void ModeAuto::land_start() { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); + _mode = SubMode::LAND; - // call location specific land start function - land_start(stopping_point); -} + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); -// auto_land_start - initialises controller to implement a landing -void ModeAuto::land_start(const Vector2f& destination) -{ - _mode = SubMode::LAND; + // initialise the vertical position controller + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } - // initialise loiter target destination - loiter_nav->init_target(destination); + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -485,12 +484,29 @@ bool ModeAuto::is_taking_off() const // auto_payload_place_start - initialises controller to implement a placing void ModeAuto::payload_place_start() { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); + _mode = SubMode::NAV_PAYLOAD_PLACE; + nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; + + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise the vertical position controller + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + + // initialise the vertical position controller + if (!pos_control->is_active_z()) { + pos_control->init_z_controller(); + } - // call location specific place start function - payload_place_start(stopping_point); + // initialise yaw + auto_yaw.set_mode(AUTO_YAW_HOLD); } // returns true if pilot's yaw input should be used to adjust vehicle's heading @@ -908,8 +924,6 @@ void ModeAuto::land_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; } @@ -1015,8 +1029,14 @@ void ModeAuto::loiter_to_alt_run() } if (!loiter_to_alt.loiter_start_done) { - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + _mode = SubMode::LOITER_TO_ALT; loiter_to_alt.loiter_start_done = true; } @@ -1054,31 +1074,12 @@ void ModeAuto::loiter_to_alt_run() pos_control->update_z_controller(); } -// auto_payload_place_start - initialises controller to implement placement of a load -void ModeAuto::payload_place_start(const Vector2f& destination) -{ - _mode = SubMode::NAV_PAYLOAD_PLACE; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - - // initialise loiter target destination - loiter_nav->init_target(destination); - - // initialise the vertical position controller - pos_control->init_z_controller(); - - // initialise yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); -} - // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more void ModeAuto::payload_place_run() { if (!payload_place_run_should_run()) { zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; } @@ -1409,6 +1410,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); } // do_spline_wp - initiate move to next waypoint @@ -1685,11 +1687,8 @@ bool ModeAuto::verify_land() case State::FlyToLocation: // check if we've reached the location if (copter.wp_nav->reached_wp_destination()) { - // get destination so we can use it for loiter target - const Vector2f& dest = copter.wp_nav->get_wp_destination().xy(); - // initialise landing controller - land_start(dest); + land_start(); // advance to next state state = State::Descending; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index e06f00d3fc9..2f2c3f278ae 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -5,17 +5,19 @@ bool ModeLand::init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do control_position = copter.position_ok(); - if (control_position) { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - loiter_nav->init_target(stopping_point); + + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise the horizontal position controller + if (control_position && !pos_control->is_active_xy()) { + pos_control->init_xy_controller(); } // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -76,8 +78,6 @@ void ModeLand::gps_run() // Land State Machine Determination if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); } else { // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 6c148e57d0d..e0fdebce10f 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -273,9 +273,6 @@ void ModeRTL::descent_start() _state = SubMode::FINAL_DESCENT; _state_complete = false; - // Set wp navigation target to above home - loiter_nav->init_target(wp_nav->get_wp_destination().xy()); - // initialise altitude target to stopping point pos_control->init_z_controller_stopping_point(); @@ -363,8 +360,14 @@ void ModeRTL::land_start() _state = SubMode::LAND; _state_complete = false; - // Set wp navigation target to above home - loiter_nav->init_target(wp_nav->get_wp_destination().xy()); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise loiter target destination + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -405,8 +408,6 @@ void ModeRTL::land_run(bool disarm_on_land) // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; } From cf5565f17c71fe3474900341ba3a33cb72a14dc6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 23 Feb 2022 15:25:35 +1030 Subject: [PATCH 0120/3101] Copter: tighten auto_takeoff_complete checks --- ArduCopter/takeoff.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 4f12db12566..7dfd4e27783 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -180,7 +180,7 @@ void Mode::auto_takeoff_run() // handle takeoff completion bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_start_alt_cm) * 0.90); - bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.5; + bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1; auto_takeoff_complete = reached_altitude && reached_climb_rate; // calculate completion for location in case it is needed for a smooth transition to wp_nav From 7e7da560d53900391e57b89d8cb66348217b5e71 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 6 Mar 2022 13:00:02 +1100 Subject: [PATCH 0121/3101] Vagrant: add support for Ubuntu 22.04, Jammy --- Vagrantfile | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/Vagrantfile b/Vagrantfile index b33b36935e4..8036e639ea1 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -227,5 +227,23 @@ Vagrant.configure(VAGRANTFILE_API_VERSION) do |config| impish.vm.boot_timeout = 1200 end -end + # 22.04 LTS EOL Apr 2032 + config.vm.define "jammy", autostart: false do |jammy| + jammy.vm.box = "ubuntu/jammy64" + jammy.vm.provision :shell, path: "Tools/vagrant/initvagrant.sh" + jammy.vm.provider "virtualbox" do |vb| + vb.name = "ArduPilot (jammy)" + end + jammy.vm.boot_timeout = 1200 + end + config.vm.define "jammy-desktop", autostart: false do |jammy| + jammy.vm.box = "ubuntu/jammy64" + jammy.vm.provision :shell, path: "Tools/vagrant/initvagrant-desktop.sh" + jammy.vm.provider "virtualbox" do |vb| + vb.name = "ArduPilot (jammy-desktop)" + vb.gui = true + end + jammy.vm.boot_timeout = 1200 + end +end From f2fe55b5dae643a53833324b474b9bdf2a10139d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 7 Mar 2022 12:45:19 +1100 Subject: [PATCH 0122/3101] autotest: correct exception handling for Python 3.10 --- Tools/autotest/common.py | 6 ++++-- Tools/scripts/build_binaries.py | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index fd60578c0e0..1070070e0a2 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -6558,10 +6558,12 @@ def get_stacktrace(self): def get_exception_stacktrace(self, e): if sys.version_info[0] >= 3: ret = "%s\n" % e - ret += ''.join(traceback.format_exception(etype=type(e), - value=e, + ret += ''.join(traceback.format_exception(type(e), + e, tb=e.__traceback__)) return ret + + # Python2: return traceback.format_exc(e) def bin_logs(self): diff --git a/Tools/scripts/build_binaries.py b/Tools/scripts/build_binaries.py index 4af52ba655c..ac3125c4996 100755 --- a/Tools/scripts/build_binaries.py +++ b/Tools/scripts/build_binaries.py @@ -527,10 +527,12 @@ def build_vehicle(self, tag, vehicle, boards, vehicle_binaries_subdir, def get_exception_stacktrace(self, e): if sys.version_info[0] >= 3: ret = "%s\n" % e - ret += ''.join(traceback.format_exception(etype=type(e), - value=e, + ret += ''.join(traceback.format_exception(type(e), + e, tb=e.__traceback__)) return ret + + # Python2: return traceback.format_exc(e) def print_exception_caught(self, e, send_statustext=True): From 041b2b594bab43a6ad92aa78f7263d05dde52f67 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 28 Feb 2022 14:48:30 -0700 Subject: [PATCH 0123/3101] AP_Compass: Fix compass priority instance message to make sense to users --- libraries/AP_Compass/AP_Compass.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index e527286cfcc..46f9141cae4 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1901,7 +1901,7 @@ bool Compass::configured(char *failure_msg, uint8_t failure_msg_len) for (Priority i(0); i Date: Wed, 2 Mar 2022 23:54:22 +0000 Subject: [PATCH 0124/3101] Rover: loiter: sailboats don't try and sail directly into the wind --- Rover/mode_loiter.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/Rover/mode_loiter.cpp b/Rover/mode_loiter.cpp index 31cd4b824b2..f1ba5ac2771 100644 --- a/Rover/mode_loiter.cpp +++ b/Rover/mode_loiter.cpp @@ -56,8 +56,20 @@ void ModeLoiter::update() _desired_speed *= yaw_error_ratio; } + // 0 turn rate is no limit + float turn_rate = 0.0; + + // make sure sailboats don't try and sail directly into the wind + if (g2.sailboat.use_indirect_route(_desired_yaw_cd)) { + _desired_yaw_cd = g2.sailboat.calc_heading(_desired_yaw_cd); + if (g2.sailboat.tacking()) { + // use pivot turn rate for tacks + turn_rate = g2.wp_nav.get_pivot_rate(); + } + } + // run steering and throttle controllers - calc_steering_to_heading(_desired_yaw_cd); + calc_steering_to_heading(_desired_yaw_cd, turn_rate); calc_throttle(_desired_speed, true); } From ea442dae1eb5ea5576b5ebaf91788b25cba0fa1e Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 4 Mar 2022 22:52:04 +0000 Subject: [PATCH 0125/3101] AP_Arming: don't arming check servo functions set to GPIO --- libraries/AP_Arming/AP_Arming.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index af7dc98d1ec..a09f0e4be7f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -797,7 +797,7 @@ bool AP_Arming::servo_checks(bool report) const bool check_passed = true; for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { const SRV_Channel *c = SRV_Channels::srv_channel(i); - if (c == nullptr || c->get_function() == SRV_Channel::k_none) { + if (c == nullptr || c->get_function() <= SRV_Channel::k_none) { continue; } From 515db9685873b80704bb909ed9dd9c19e54942dc Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 4 Mar 2022 22:52:39 +0000 Subject: [PATCH 0126/3101] AP_PiccoloCAN: GPIO servo does not count as active --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index d7124d54df9..1881bc46193 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -749,7 +749,7 @@ bool AP_PiccoloCAN::is_servo_channel_active(uint8_t chan) SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(chan); // Ignore if the servo channel does not have a function assigned - if (function == SRV_Channel::k_none) { + if (function <= SRV_Channel::k_none) { return false; } From 30c08c1e7c104cbf365611fc7802078e3c3e1bd7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Mar 2022 16:10:22 +1100 Subject: [PATCH 0127/3101] RC_Channel: rename within_min_dz to in_min_dz for consistency ... consistency with in_trim_dz --- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 5b8b471a22d..7005da477af 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -332,7 +332,7 @@ bool RC_Channel::in_trim_dz() const /* return trues if input is within deadzone of min */ -bool RC_Channel::within_min_dz() const +bool RC_Channel::in_min_dz() const { return radio_in < radio_min + dead_zone; } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index bec21d8c18f..f06fecccbee 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -53,7 +53,7 @@ class RC_Channel { float norm_input_ignore_trim() const; // returns true if input is within deadzone of min - bool within_min_dz() const; + bool in_min_dz() const; uint8_t percent_input() const; From 302c8e4b9804fdb3e3ecc70c42a5be8195a41ae3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 5 Mar 2022 16:10:22 +1100 Subject: [PATCH 0128/3101] ArduPlane: rename within_min_dz to in_min_dz for consistency ... consistency with in_trim_dz --- ArduPlane/radio.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index eb5ea21dd2e..65c735e9fbd 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -430,7 +430,7 @@ bool Plane::throttle_at_zero(void) const to center stick area in conjunction with sprung throttle, cannot use in_trim, must use rc_min */ if (((!(g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM) && channel_throttle->in_trim_dz()) || - (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM && channel_throttle->within_min_dz()))) { + (g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM && channel_throttle->in_min_dz()))) { return true; } return false; From 4d40ef5d0a8be02a931d42d6c44e268d9fbdfda2 Mon Sep 17 00:00:00 2001 From: RuffaloVM Date: Sat, 5 Mar 2022 22:29:50 +0900 Subject: [PATCH 0129/3101] AP_NavEKF : remove the space around the operator --- libraries/AP_NavEKF/EKFGSF_yaw.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index bfc424ac7ab..8a938c6d0cc 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -93,7 +93,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, } // Always run the AHRS prediction cycle for each model - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { predict(mdl_idx); } @@ -105,7 +105,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, // To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth // equal to the weighting value before it is summed. Vector2F yaw_vector = {}; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]); yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]); } @@ -114,7 +114,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, // Example for future reference showing how a full GSF covariance matrix could be calculated if required /* memset(&GSF.P, 0, sizeof(GSF.P)); - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { ftype delta[3]; for (uint8_t row = 0; row < 3; row++) { delta[row] = EKF[mdl_idx].X[row] - GSF.X[row]; @@ -128,7 +128,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, */ GSF.yaw_variance = 0.0f; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { ftype yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw); GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta)); } @@ -144,7 +144,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc) if (!vel_fuse_running) { // Perform in-flight alignment resetEKFGSF(); - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { // Use the firstGPS measurement to set the velocities and corresponding variances EKF[mdl_idx].X[0] = vel[0]; EKF[mdl_idx].X[1] = vel[1]; @@ -157,7 +157,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc) ftype total_w = 0.0f; ftype newWeight[(uint8_t)N_MODELS_EKFGSF]; bool state_update_failed = false; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { // Update states and covariances using GPS NE velocity measurements fused as direct state observations if (!correct(mdl_idx, vel, velObsVar)) { state_update_failed = true; @@ -168,7 +168,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc) // Calculate weighting for each model assuming a normal error distribution const ftype min_weight = 1e-5f; uint8_t n_clips = 0; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx]; if (newWeight[mdl_idx] < min_weight) { n_clips++; @@ -181,7 +181,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc) // Reset the filters if all weights have underflowed due to excessive innovation variances if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) { ftype total_w_inv = 1.0f / total_w; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv; } } else { @@ -641,7 +641,7 @@ bool EKFGSF_yaw::getVelInnovLength(ftype &velInnovLength) const return false; } velInnovLength = 0.0f; - for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) { + for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { velInnovLength += GSF.weights[mdl_idx] * sqrtF((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1]))); } return true; From 54e57ccff0c02cabfc8f573ec77fd5ab313c70fd Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 6 Mar 2022 17:03:40 +0000 Subject: [PATCH 0130/3101] Plane: don't prevent stick mixing in none RC failsafe --- ArduPlane/Attitude.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 52d455af6d2..b76d8ec2768 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -82,9 +82,8 @@ bool Plane::stick_mixing_enabled(void) if (g.stick_mixing != StickMixing::NONE && g.stick_mixing != StickMixing::VTOL_YAW && stickmixing && - failsafe.state == FAILSAFE_NONE && !rc_failsafe_active()) { - // we're in an auto mode, and haven't triggered failsafe + // we're in an auto mode, and haven't triggered rc failsafe return true; } else { return false; From c7d72821daf1ef06d071439edeb571c457c233e1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 7 Mar 2022 09:30:16 +1100 Subject: [PATCH 0131/3101] AP_RangeFinder: add AP_RANGEFINDER_LEDDARVU8_ENABLED --- libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp | 4 ++++ libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h | 8 ++++++++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 1f791066fcb..b903a904f5b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -15,6 +15,8 @@ #include "AP_RangeFinder_LeddarVu8.h" +#if AP_RANGEFINDER_LEDDARVU8_ENABLED + #include #include @@ -202,3 +204,5 @@ bool AP_RangeFinder_LeddarVu8::parse_byte(uint8_t b, bool &valid_reading, uint16 valid_reading = false; return false; } + +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h index 75fa8dd67f7..78f99f5d4a2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h @@ -3,6 +3,12 @@ #include "AP_RangeFinder.h" #include "AP_RangeFinder_Backend_Serial.h" +#ifndef AP_RANGEFINDER_LEDDARVU8_ENABLED +#define AP_RANGEFINDER_LEDDARVU8_ENABLED 1 +#endif + +#if AP_RANGEFINDER_LEDDARVU8_ENABLED + #define LEDDARVU8_PAYLOAD_LENGTH (8*2) class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial @@ -90,3 +96,5 @@ class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial uint32_t last_distance_ms; // system time of last successful distance sensor read uint32_t last_distance_request_ms; // system time of last request to sensor to send distances }; + +#endif From 93031e297d803ec5866c980fb26f27ee14dc910f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Mar 2022 13:41:31 +1100 Subject: [PATCH 0132/3101] HAL_ChibiOS: fixed min/max inversion in MCU voltage logging --- libraries/AP_HAL_ChibiOS/AnalogIn.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index 539acb76656..f206e984e10 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -513,8 +513,9 @@ void AnalogIn::_timer_tick(void) _mcu_temperature = ((110 - 30) / (TS_CAL2 - TS_CAL1)) * (float(buf_adc3[1]) - TS_CAL1) + 30; _mcu_voltage = 3.3 * VREFINT_CAL / float(buf_adc3[2]+0.001); - _mcu_voltage_min = 3.3 * VREFINT_CAL / float(min_adc3[2]+0.001); - _mcu_voltage_max = 3.3 * VREFINT_CAL / float(max_adc3[2]+0.001); + // note min/max swap due to inversion + _mcu_voltage_min = 3.3 * VREFINT_CAL / float(max_adc3[2]+0.001); + _mcu_voltage_max = 3.3 * VREFINT_CAL / float(min_adc3[2]+0.001); } #endif } From 18696e923ee8d2c6642f5b64aadf4aa0e613a2e7 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 26 Jan 2022 15:17:14 +1030 Subject: [PATCH 0133/3101] Copter: WP Pause support --- ArduCopter/GCS_Mavlink.cpp | 7 ++++--- ArduCopter/mode.h | 3 +++ ArduCopter/mode_auto.cpp | 12 ++++++++++++ 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b02b1be8791..718f06d725f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1013,15 +1013,16 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_comma // requested pause from GCS if ((int8_t) packet.param1 == 0) { - copter.mode_auto.mission.stop(); - copter.mode_auto.loiter_start(); + // copter.mode_auto.mission.stop(); + copter.mode_auto.pause_mission(); gcs().send_text(MAV_SEVERITY_INFO, "Paused mission"); return MAV_RESULT_ACCEPTED; } // requested resume from GCS if ((int8_t) packet.param1 == 1) { - copter.mode_auto.mission.resume(); + // copter.mode_auto.mission.resume(); + copter.mode_auto.continue_mission(); gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission"); return MAV_RESULT_ACCEPTED; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3becb062342..a16f3b5d696 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -427,6 +427,9 @@ class ModeAuto : public Mode { void circle_movetoedge_start(const Location &circle_center, float radius_m); void circle_start(); void nav_guided_start(); + + void pause_mission(); + void continue_mission(); bool is_landing() const override; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 0aa7d09c2af..dfa4e0166ce 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -673,6 +673,18 @@ void ModeAuto::exit_mission() } } +// pause_mission - Prevent aircraft from progressing along the track +void ModeAuto::pause_mission() +{ + wp_nav->set_pause(); +} + +// continue_mission - Allow aircraft to progress along the track +void ModeAuto::continue_mission() +{ + wp_nav->set_continue(); +} + // do_guided - start guided mode bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) { From 289b1ca75a26fb85ab26e170f5ae3db45d611aa1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 27 Jan 2022 19:07:38 +1030 Subject: [PATCH 0134/3101] AC_WPNav: Support pause --- libraries/AC_WPNav/AC_WPNav.cpp | 44 ++++++++++++++++++++------------- libraries/AC_WPNav/AC_WPNav.h | 12 +++++++-- 2 files changed, 37 insertions(+), 19 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index d1490944342..032a013d453 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -197,8 +197,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) _this_leg_is_spline = false; // initialise the terrain velocity to the current maximum velocity - _terrain_vel = _wp_desired_speed_xy_cms; - _terrain_accel = 0.0; + _offset_vel = _wp_desired_speed_xy_cms; + _offset_accel = 0.0; + _paused = false; // mark as active _wp_last_update = AP_HAL::millis(); @@ -209,8 +210,8 @@ void AC_WPNav::set_speed_xy(float speed_cms) { // range check target speed and protect against divide by zero if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) { - // update terrain following speed scalar - _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms; + // update horizontal velocity speed offset scalar + _offset_vel = speed_cms * _offset_vel / _wp_desired_speed_xy_cms; // initialize the desired wp speed _wp_desired_speed_xy_cms = speed_cms; @@ -463,14 +464,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) // get current position and adjust altitude to origin and destination's frame (i.e. _frame) const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset}; - - // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft Vector3f curr_target_vel = _pos_control.get_vel_desired_cms(); curr_target_vel.z -= _pos_control.get_vel_offset_z_cms(); + // Use _track_scalar_dt to slow down progression of the position target moving too far in front of aircraft + // _track_scalar_dt does not scale the velocity or acceleration float track_scaler_dt = 1.0f; // check target velocity is non-zero - if (is_positive(curr_target_vel.length())) { + if (is_positive(curr_target_vel.length_squared())) { Vector3f track_direction = curr_target_vel.normalized(); const float track_error = _pos_control.get_pos_error_cm().dot(track_direction); const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction); @@ -478,13 +479,15 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); } - float vel_time_scalar = 1.0; + // Use vel_scaler_dt to slow down the trajectory time + // vel_scaler_dt scales the velocity and acceleration to be kinematically constent + float vel_scaler_dt = 1.0; if (is_positive(_wp_desired_speed_xy_cms)) { - update_vel_accel(_terrain_vel, _terrain_accel, dt, 0.0, 0.0); - shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0, - _terrain_vel, _terrain_accel, - -_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_jerk_xy_cmsss(), dt, true); - vel_time_scalar = _terrain_vel / _wp_desired_speed_xy_cms; + update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0); + const float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0; + shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -_wp_accel_cmss, _wp_accel_cmss, + _pos_control.get_shaping_jerk_xy_cmsss(), dt, true); + vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms; } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk @@ -501,16 +504,23 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) if (!_this_leg_is_spline) { // update target position, velocity and acceleration target_pos = _origin; - s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel); + s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel); } else { // splinetarget_vel target_vel = curr_target_vel; - _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel); + _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel); s_finished = _spline_this_leg.reached_destination(); } - target_vel *= vel_time_scalar; - target_accel *= sq(vel_time_scalar); + Vector3f accel_offset; + if (is_positive(target_vel.length_squared())) { + Vector3f track_direction = target_vel.normalized(); + accel_offset = track_direction * _offset_accel * target_vel.length() / _wp_desired_speed_xy_cms; + } + + target_vel *= vel_scaler_dt; + target_accel *= sq(vel_scaler_dt); + target_accel += accel_offset; // convert final_target.z to altitude above the ekf origin target_pos.z += _pos_control.get_pos_offset_z_cm(); diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 451b4998a2c..13fdc4e1719 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -57,6 +57,13 @@ class AC_WPNav /// set current target horizontal speed during wp navigation void set_speed_xy(float speed_cms); + /// set pause or resume during wp navigation + void set_pause() { _paused = true; } + void set_resume() { _paused = false; } + + /// get paused status + bool paused() { return _paused; } + /// set current target climb or descent rate during wp navigation void set_speed_up(float speed_up_cms); void set_speed_down(float speed_down_cms); @@ -258,8 +265,9 @@ class AC_WPNav Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _destination; // target destination in cm from ekf origin float _track_scalar_dt; // time compression multiplier to slow the progress along the track - float _terrain_vel; // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain - float _terrain_accel; // acceleration value used to change _terrain_vel + float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain + float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain + bool _paused; // flag for pausing waypoint controller // terrain following variables bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin From 670af8d469079796c450e5836a22e26518c73ff6 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 27 Jan 2022 19:30:15 +1030 Subject: [PATCH 0135/3101] Copter: Add pause in guided mode --- ArduCopter/mode.h | 1 + ArduCopter/mode_guided.cpp | 35 +++++++++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index a16f3b5d696..8a2e1d44959 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1008,6 +1008,7 @@ class ModeGuided : public Mode { void pos_control_run(); void accel_control_run(); void velaccel_control_run(); + void pause_control_run(); void posvelaccel_control_run(); void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 3b1214c8912..ddf076f7028 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -47,6 +47,11 @@ bool ModeGuided::init(bool ignore_checks) // should be called at 100hz or more void ModeGuided::run() { + // if (paused) { + // pause_control_run(); + // return; + // } + // call the correct auto controller switch (guided_mode) { @@ -849,6 +854,36 @@ void ModeGuided::velaccel_control_run() } } +// velaccel_control_run - runs the guided velocity and acceleration controller +// called from guided_run +void ModeGuided::pause_control_run() +{ + // if not armed set throttle to zero and exit immediately + if (is_disarmed_or_landed()) { + // do not spool down tradheli when on the ground with motor interlock enabled + make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); + return; + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // set the horizontal velocity and acceleration targets to zero + Vector2f vel_xy, accel_xy; + pos_control->input_vel_accel_xy(vel_xy, accel_xy, false); + + // set the vertical velocity and acceleration targets to zero + float vel_z = 0.0; + pos_control->input_vel_accel_z(vel_z, 0.0, false, false); + + // call velocity controller which includes z axis controller + pos_control->update_xy_controller(); + pos_control->update_z_controller(); + + // call attitude controller + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0); +} + // posvelaccel_control_run - runs the guided position, velocity and acceleration controller // called from guided_run void ModeGuided::posvelaccel_control_run() From f0e1b3eb9fb7fd6724482784e72a6ba1d8f1642d Mon Sep 17 00:00:00 2001 From: m Date: Thu, 27 Jan 2022 21:36:21 +0300 Subject: [PATCH 0136/3101] Copter: Pause/Continue in AUTO and GUIDED modes with SCurves --- ArduCopter/GCS_Mavlink.cpp | 35 ++++++++++++++--------------------- ArduCopter/mode.h | 18 +++++++++++++++--- ArduCopter/mode_auto.cpp | 36 ++++++++++++++++++++++++------------ ArduCopter/mode_guided.cpp | 29 ++++++++++++++++++++++++----- 4 files changed, 77 insertions(+), 41 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 718f06d725f..e4136c439ad 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1005,31 +1005,24 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet) { -#if MODE_AUTO_ENABLED - if (copter.flightmode->mode_number() != Mode::Number::AUTO) { - // only supported in AUTO mode + // requested pause + if ((uint8_t) packet.param1 == 0) { + if (copter.flightmode->pause()) { + return MAV_RESULT_ACCEPTED; + } + send_text(MAV_SEVERITY_INFO, "Failed to pause"); return MAV_RESULT_FAILED; } - // requested pause from GCS - if ((int8_t) packet.param1 == 0) { - // copter.mode_auto.mission.stop(); - copter.mode_auto.pause_mission(); - gcs().send_text(MAV_SEVERITY_INFO, "Paused mission"); - return MAV_RESULT_ACCEPTED; - } - - // requested resume from GCS - if ((int8_t) packet.param1 == 1) { - // copter.mode_auto.mission.resume(); - copter.mode_auto.continue_mission(); - gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission"); - return MAV_RESULT_ACCEPTED; + // requested resume + if ((uint8_t) packet.param1 == 1) { + if (copter.flightmode->resume()) { + return MAV_RESULT_ACCEPTED; + } + send_text(MAV_SEVERITY_INFO, "Failed to resume"); + return MAV_RESULT_FAILED; } -#endif - - // fail pause or continue - return MAV_RESULT_FAILED; + return MAV_RESULT_DENIED; } void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8a2e1d44959..bf3e302a0bf 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -110,6 +110,10 @@ class Mode { // returns true if pilot's yaw input should be used to adjust vehicle's heading virtual bool use_pilot_yaw() const {return true; } + // pause and resume a mode + virtual bool pause() { return false; }; + virtual bool resume() { return false; }; + protected: // helper functions @@ -419,6 +423,10 @@ class ModeAuto : public Mode { // Auto SubMode mode() const { return _mode; } + // pause continue in auto mode + bool pause() override; + bool resume() override; + bool loiter_start(); void rtl_start(); void takeoff_start(const Location& dest_loc); @@ -427,9 +435,6 @@ class ModeAuto : public Mode { void circle_movetoedge_start(const Location &circle_center, float radius_m); void circle_start(); void nav_guided_start(); - - void pause_mission(); - void continue_mission(); bool is_landing() const override; @@ -973,6 +978,10 @@ class ModeGuided : public Mode { bool use_pilot_yaw() const override; + // pause continue in guided mode + bool pause() override; + bool resume() override; + protected: const char *name() const override { return "GUIDED"; } @@ -1016,6 +1025,9 @@ class ModeGuided : public Mode { SubMode guided_mode = SubMode::TakeOff; bool send_notification; // used to send one time notification to ground station bool takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear) + + // guided mode is paused or not + bool _paused; }; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dfa4e0166ce..088142da18a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -673,18 +673,6 @@ void ModeAuto::exit_mission() } } -// pause_mission - Prevent aircraft from progressing along the track -void ModeAuto::pause_mission() -{ - wp_nav->set_pause(); -} - -// continue_mission - Allow aircraft to progress along the track -void ModeAuto::continue_mission() -{ - wp_nav->set_continue(); -} - // do_guided - start guided mode bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) { @@ -2078,4 +2066,28 @@ bool ModeAuto::verify_nav_script_time() } #endif +// pause - Prevent aircraft from progressing along the track +bool ModeAuto::pause() +{ + // do not pause if already paused or not in the WP sub mode or already reached to the destination + if(wp_nav->paused() || _mode != SubMode::WP || wp_nav->reached_wp_destination()) { + return false; + } + + wp_nav->set_pause(); + return true; +} + +// resume - Allow aircraft to progress along the track +bool ModeAuto::resume() +{ + // do not resume if not paused before + if(!wp_nav->paused()) { + return false; + } + + wp_nav->set_resume(); + return true; +} + #endif diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index ddf076f7028..501a9475d72 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -40,6 +40,10 @@ bool ModeGuided::init(bool ignore_checks) guided_vel_target_cms.zero(); guided_accel_target_cmss.zero(); send_notification = false; + + // clear pause state when entering guided mode + _paused = false; + return true; } @@ -47,10 +51,11 @@ bool ModeGuided::init(bool ignore_checks) // should be called at 100hz or more void ModeGuided::run() { - // if (paused) { - // pause_control_run(); - // return; - // } + // run pause control if the vehicle is paused + if (_paused) { + pause_control_run(); + return; + } // call the correct auto controller switch (guided_mode) { @@ -854,7 +859,7 @@ void ModeGuided::velaccel_control_run() } } -// velaccel_control_run - runs the guided velocity and acceleration controller +// pause_control_run - runs the guided mode pause controller // called from guided_run void ModeGuided::pause_control_run() { @@ -1199,4 +1204,18 @@ uint32_t ModeGuided::get_timeout_ms() const return MAX(copter.g2.guided_timeout, 0.1) * 1000; } +// pause guide mode +bool ModeGuided::pause() +{ + _paused = true; + return true; +} + +// resume guided mode +bool ModeGuided::resume() +{ + _paused = false; + return true; +} + #endif From 37abfb98dcc00cdf0952180da9629b2403cf1ed7 Mon Sep 17 00:00:00 2001 From: m Date: Thu, 24 Feb 2022 12:37:08 +0300 Subject: [PATCH 0137/3101] autotest: Copter Pause/Continue in AUTO and GUIDED modes with SCurves --- Tools/autotest/arducopter.py | 231 +++++++++++++++++++++++++++++++---- Tools/autotest/common.py | 22 ++++ 2 files changed, 227 insertions(+), 26 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3f238082bcd..5ca36c518c3 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -4025,6 +4025,47 @@ def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3): self.progress("Received proper target velocity commands") + def wait_for_local_velocity(self, vx, vy, vz_up, timeout=10): + """ Wait for local target velocity""" + + # debug messages + self.progress("Waiting for local NED velocity target: (%f, %f, %f)" % (vx, vy, -vz_up)) + self.progress("Setting LOCAL_POSITION_NED message rate to 10Hz") + + # set position local ned message stream rate + self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED, 10) + + # wait for position local ned message + tstart = self.get_sim_time() + while self.get_sim_time_cached() - tstart < timeout: + + # get position target local ned message + m = self.mav.recv_match(type="LOCAL_POSITION_NED", blocking=True, timeout=1) + + # could not be able to get a valid target local ned message within given time + if m is None: + + # raise an error that did not receive a valid target local ned message within given time + raise NotAchievedException("Did not receive any position local ned message for 1 second!") + + # got a valid target local ned message within given time + else: + + # debug message + self.progress("Received local position ned message: %s" % str(m)) + + # check if velocity values are in range + if vx - m.vx <= 0.1 and vy - m.vy <= 0.1 and vz_up - (-m.vz) <= 0.1: + + # get out of function + self.progress("Vehicle successfully reached to target velocity!") + return + + # raise an exception + error_message = "Did not receive target velocities vx, vy, vz_up, wanted=(%f, %f, %f) got=(%f, %f, %f)" + error_message = error_message % (vx, vy, vz_up, m.vx, m.vy, -m.vz) + raise NotAchievedException(error_message) + def test_position_target_message_mode(self): " Ensure that POSITION_TARGET_LOCAL_NED messages are sent in Guided Mode only " self.hover() @@ -7850,39 +7891,173 @@ def AP_Avoidance(self): self.context_pop() def PAUSE_CONTINUE(self): - self.load_mission("copter_mission.txt", strict=False) + self.load_mission(filename="copter_mission.txt", strict=False) + self.set_parameter(name="AUTO_OPTIONS", value=3) + self.change_mode(mode="AUTO") + self.wait_ready_to_arm() + self.arm_vehicle() - self.set_parameter("AUTO_OPTIONS", 3) - self.change_mode('AUTO') + self.wait_current_waypoint(wpnum=3, timeout=500) + self.send_pause_command() + self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5) + self.send_resume_command() + + self.wait_current_waypoint(wpnum=4, timeout=500) + self.send_pause_command() + self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5) + self.send_resume_command() + + self.wait_disarmed(timeout=500) + + def PAUSE_CONTINUE_GUIDED(self): + self.start_subtest("Started test for Pause/Continue in GUIDED mode with LOCATION!") + self.change_mode(mode="GUIDED") self.wait_ready_to_arm() self.arm_vehicle() + self.set_parameter(name="GUID_TIMEOUT", value=120) + self.user_takeoff(alt_min=30) - self.wait_waypoint(4, 4) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, - 0, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + # send vehicle to global position target + location = self.home_relative_loc_ne(n=300, e=0) + target_typemask = MAV_POS_TARGET_TYPE_MASK.POS_ONLY + self.mav.mav.set_position_target_global_int_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # relative altitude frame + target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, # target typemask as pos only + int(location.lat * 1e7), # lat + int(location.lng * 1e7), # lon + 30, # alt + 0, # vx + 0, # vy + 0, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0) # yawrate - self.wait_groundspeed(0, 1, minimum_duration=5) + self.wait_distance_to_home(distance_min=100, distance_max=150, timeout=120) + self.send_pause_command() + self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5) + self.send_resume_command() + self.wait_location(loc=location, timeout=120) - self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, - 1, # param1 - 0, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - 0 # param7 - ) + self.end_subtest("Ended test for Pause/Continue in GUIDED mode with LOCATION!") + self.start_subtest("Started test for Pause/Continue in GUIDED mode with DESTINATION!") + self.guided_achieve_heading(heading=270) + + # move vehicle on x direction + location = self.offset_location_ne(location=self.mav.location(), metres_north=0, metres_east=-300) + self.mav.mav.set_position_target_local_ned_send( + 0, # system time in milliseconds + 1, # target system + 1, # target component + mavutil.mavlink.MAV_FRAME_BODY_NED, # coordinate frame MAV_FRAME_BODY_NED + MAV_POS_TARGET_TYPE_MASK.POS_ONLY, # type mask (pos only) + 300, # position x + 0, # position y + 0, # position z + 0, # velocity x + 0, # velocity y + 0, # velocity z + 0, # accel x + 0, # accel y + 0, # accel z + 0, # yaw + 0) # yaw rate + + self.wait_location(loc=location, accuracy=200, timeout=120) + self.send_pause_command() + self.wait_groundspeed(speed_min=0, speed_max=1, minimum_duration=5) + self.send_resume_command() + self.wait_location(loc=location, timeout=120) + + self.end_subtest("Ended test for Pause/Continue in GUIDED mode with DESTINATION!") + self.start_subtest("Started test for Pause/Continue in GUIDED mode with VELOCITY!") + + # give velocity command + vx, vy, vz_up = (5, 5, 0) + self.test_guided_local_velocity_target(vx=vx, vy=vy, vz_up=vz_up, timeout=10) + + self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10) + self.send_pause_command() + self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) + self.send_resume_command() + self.wait_for_local_velocity(vx=vx, vy=vy, vz_up=vz_up, timeout=10) + self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10) + self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) + + self.end_subtest("Ended test for Pause/Continue in GUIDED mode with VELOCITY!") + self.start_subtest("Started test for Pause/Continue in GUIDED mode with ACCELERATION!") + + # give acceleration command + ax, ay, az_up = (1, 1, 0) + target_typemask = (MAV_POS_TARGET_TYPE_MASK.POS_IGNORE | MAV_POS_TARGET_TYPE_MASK.VEL_IGNORE | + MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE) + self.mav.mav.set_position_target_local_ned_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, + 0, # x + 0, # y + 0, # z + 0, # vx + 0, # vy + 0, # vz + ax, # afx + ay, # afy + -az_up, # afz + 0, # yaw + 0, # yawrate + ) - self.wait_groundspeed(5, 100) + self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10) + self.send_pause_command() + self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) + self.send_resume_command() + self.wait_for_local_velocity(vx=5, vy=5, vz_up=0, timeout=10) + self.test_guided_local_velocity_target(vx=0, vy=0, vz_up=0, timeout=10) + self.wait_for_local_velocity(vx=0, vy=0, vz_up=0, timeout=10) + self.end_subtest("Ended test for Pause/Continue in GUIDED mode with ACCELERATION!") + + # start pause/continue subtest with posvelaccel + self.start_subtest("Started test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!") + self.guided_achieve_heading(heading=0) + + # give posvelaccel command + x, y, z_up = (-300, 0, 30) + target_typemask = (MAV_POS_TARGET_TYPE_MASK.YAW_IGNORE | MAV_POS_TARGET_TYPE_MASK.YAW_RATE_IGNORE) + self.mav.mav.set_position_target_local_ned_send( + 0, # timestamp + 1, # target system_id + 1, # target component id + mavutil.mavlink.MAV_FRAME_LOCAL_NED, + target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE, + x, # x + y, # y + -z_up, # z + 0, # vx + 0, # vy + 0, # vz + 0, # afx + 0, # afy + 0, # afz + 0, # yaw + 0, # yawrate + ) - self.wait_disarmed() + self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=400, distance_max=450, timeout=120) + self.send_pause_command() + self.wait_for_local_velocity(0, 0, 0, timeout=10) + self.send_resume_command() + self.wait_distance_to_local_position(local_position=(x, y, -z_up), distance_min=0, distance_max=10, timeout=120) + + self.end_subtest("Ended test for Pause/Continue in GUIDED mode with POSITION and VELOCITY and ACCELERATION!") + self.do_RTL(timeout=120) def DO_CHANGE_SPEED(self): self.load_mission("mission.txt", strict=False) @@ -8471,9 +8646,13 @@ def tests1e(self): self.test_altitude_types), ("PAUSE_CONTINUE", - "Test MAV_CMD_PAUSE_CONTINUE", + "Test MAV_CMD_DO_PAUSE_CONTINUE in AUTO mode", self.PAUSE_CONTINUE), + ("PAUSE_CONTINUE_GUIDED", + "Test MAV_CMD_DO_PAUSE_CONTINUE in GUIDED mode", + self.PAUSE_CONTINUE_GUIDED), + ("RichenPower", "Test RichenPower generator", self.test_richenpower), diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 1070070e0a2..7efa598cc7f 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -11856,3 +11856,25 @@ def load_default_params_file(self, filename): '''load a file from Tools/autotest/default_params''' filepath = util.reltopdir(os.path.join("Tools", "autotest", "default_params", filename)) self.repeatedly_apply_parameter_file(filepath) + + def send_pause_command(self): + '''pause AUTO/GUIDED modes''' + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, + 0, # 0: pause, 1: continue + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 0) # param7 + + def send_resume_command(self): + '''resume AUTO/GUIDED modes''' + self.run_cmd(mavutil.mavlink.MAV_CMD_DO_PAUSE_CONTINUE, + 1, # 0: pause, 1: continue + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 0) # param7 From 6d63e1f7d37d24f6ce47a15d3499e80d913b4dbc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Mar 2022 10:58:26 +1100 Subject: [PATCH 0138/3101] Plane: added Q_LAND_ALTCHG parameter this is the threshold height change over 4 seconds for a landing to be detected. It can be raised if landing detection is very slow --- ArduPlane/quadplane.cpp | 11 ++++++++++- ArduPlane/quadplane.h | 3 +++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index f405fc2c8bc..07ccdcec810 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -434,6 +434,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp AP_SUBGROUPPTR(weathervane, "WVANE_", 30, QuadPlane, AC_WeatherVane), + // @Param: LAND_ALTCHG + // @DisplayName: Land detection altitude change threshold + // @Description: The maximum altitude change allowed during land detection. You can raise this value if you find that landing detection takes a long time to complete. It is the maximum change in altitude over a period of 4 seconds for landing to be detected + // @Units: m + // @Range: 0.1 0.6 + // @Increment: 0.05 + // @User: Standard + AP_GROUPINFO("LAND_ALTCHG", 31, QuadPlane, landing_detect.detect_alt_change, 0.2), + AP_GROUPEND }; @@ -2995,7 +3004,7 @@ bool QuadPlane::land_detector(uint32_t timeout_ms) // we only consider the vehicle landed when the motors have been // at minimum for timeout_ms+1000 and the vertical position estimate has not // changed by more than 20cm for timeout_ms - if (fabsf(height - landing_detect.vpos_start_m) > 0.2) { + if (fabsf(height - landing_detect.vpos_start_m) > landing_detect.detect_alt_change) { // height has changed, call off landing detection landing_detect.land_start_ms = 0; return false; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index d20404d1ff1..7b184afc005 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -402,6 +402,9 @@ class QuadPlane uint32_t lower_limit_start_ms; uint32_t land_start_ms; float vpos_start_m; + + // landing detection threshold in meters + AP_Float detect_alt_change; } landing_detect; // throttle mix acceleration filter From da950952a59ae7573c723a8f4e6f1dac3fa4066a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Feb 2022 19:32:33 +1100 Subject: [PATCH 0139/3101] Tools: added HolybroG4_GPS bootloader --- Tools/bootloaders/HolybroG4_GPS_bl.bin | Bin 0 -> 20988 bytes Tools/bootloaders/HolybroG4_GPS_bl.elf | Bin 0 -> 334076 bytes Tools/bootloaders/HolybroG4_GPS_bl.hex | 1314 ++++++++++++++++++++++++ 3 files changed, 1314 insertions(+) create mode 100755 Tools/bootloaders/HolybroG4_GPS_bl.bin create mode 100755 Tools/bootloaders/HolybroG4_GPS_bl.elf create mode 100644 Tools/bootloaders/HolybroG4_GPS_bl.hex diff --git a/Tools/bootloaders/HolybroG4_GPS_bl.bin b/Tools/bootloaders/HolybroG4_GPS_bl.bin new file mode 100755 index 0000000000000000000000000000000000000000..6e4b243bc58279de6f0d71eca6cb605a9a0103d8 GIT binary patch literal 20988 zcmd744R}*U+BZJuBgtu6+LTr-ZGkiaETlkaQLFH1nxv;65efn>qPwOotfvK~0`B7C zN=jj^pu5GgE41PlexNMc7VA>o8nozwAJ3EUv9`EA+Z8p^>Uz=-a+=Vd_cuv#-QDNC zuK)jf-~V;}+iT95nKN_G%suzqbI*O>6QU!=VV(%jT8NN^@WA+cG2S8M|1hk);h)n# z{PwT$AG?UK6X6KLIfPM!e=YOBOTTap5jqfN-16^rCM^ERZ+S;)2bm>!Np_~UOjhS3Sc$y3<490QxaT7OpG4LwpDBFr zXBSyB-%?XF=b~IB9N@)Yi^Q_nzstT?tmj`VW$gE2lOk~zIT zlwI;!fbCHKn~sajw#kosp6yRyFp?sSVT_q+OlXaS26d5?WLqCokQm|Qdb@zJFbo$| z^V!q&fcl}wA;w`GC@JeAhGb_S<4lZ@u@?(z$>=!dv!`DyBm-}G1o8R)MBdiN*`5r% zA2jNY+Du{HP$r6HiYh*+*CeWV5Mbe@@5F&}_0UOp1#7BD} z1l1coa=oV>INIqs6l8^RXRI$Ha$Kx01tgL<_UmY07zQ|9u>SO~3#)tB{BozQPp~}` zVfQ}UpU6CGKPkQ_-gaiW@5il2`vtZfc}!RrGNB(q+cXDobsw1-!z<>V>$cIE&kA#c zdu)v33dWAcaBy9Gf}>CCK+Bh&vh!hfh+=}{(0cy7(@)vwp!G^;bH*c>`_Ls_WKtd| zr9c}=9NayQT~JE?q|>|YsXgpX<<3*-VjmIn19Qb}Q4`RL7rsgpm9JcdFQ4w{XN5KW z35iVp!GI|C1&rd}fKBuVeB#ysyRh7u8?cIT0ZKdeKurxdl@2p@q>W%UpD7k!>)If1YehQKW zE0HS|M&1x6a-qt|w~rW6=a53=RmuoxgG}!Z@FFww94N$g=|rkOK)ejmwP4sSGjEJ* z5!x9!dE^<;?kfAL0-;maUNA6`W3!k;M-<-5$XO#?%a0X4VrVx;M>w&#ONwvhWZhVD zX;LvUmks^mK2AO{8XNzFTg*~Qt65?en%$L%RU%gD7F-!y@;WLda|df=mdnQ=BX@^B4N~yhi(oze0M>e(jklu4ah~l1gsa;ApbX zInB#)3NL3U1n|PjBo}K;t6}8(My_%--9t=GYxi$xx_keIo_o%(?b&et?w$>k?@j;d z8O3AhQanbytsCDclY7m^tS&`ZeD)&At;O6O5bHquh9dN=fe1zfN(;1K#K79?f&EizCNn|jR@{40M_U)&% zR6J z;@v)8?-b%$h+j$5I;>F*^^cKkn;fYn&1}}OAW0IunWE+pXeIB6k_tMRpd1K*7HWvx zr|PEme@g54QRgckbF*e18R)J3>(@G&(@!v@T&FQ>8u={EAtDc`SNHy?n9mvs+=1t% z=yNboCTGSKi;SG4#0B{{O+&2gU(}9ZPSSv%F%od#qQ3a}yTZF8;4*qf{z{qI8ZXCl z@fPgl)78+EW48rL0P*7hnfrn-_T^bW?bE|!I+0{I`xxU*ONjgzl{H#HIV%*v5$+1t zj}&9|{7vP>hE^K;MiqjLK(d4WV_1P0Z3>Hi(-GfZQ`xCd-^|+mQHLV(H^jT)&2EwBLN`=GQugJ=<;$EqraFi)rjAG<_Ds`aBhs z(nMyfm*&34G;>Xy!dRH=zYS0tK7{r4uWOA*zJQQGzayLkZbn8aZf+i$@Y=LF$=&Y- zjcjS5{_qT$BiY_kv95%9Jk3eu(<;;CDOR(H{8zQC?`7_nB~0U~!G~Eo>i<@?Q3%!O zgQmYuGe1Ca{_!Y<`uVFwcB?e@*{kH54zUKv%>ffR{>!UffuWDXS z#SEpWd=|KQ0}l$`Gj@a65uo#S@0f0?dTGO}Z(+{XznYIZ)Aj44bGA0xQfU;%U9V!4 zx*s{;EWCN>;Hyh9rYFZh-wkv%tQ*q@d3`Xtj?_?Yw0seI@gDl?^idqe`vjBzWpE^U z7BG3EFUE_AoUW=N(`aTfC!Y$Zv_$tK(Acb3uh7`^S1Fb>hcv*_+~^9}Hk2Oim3p~0 zI%6rXsv%~yV4&X0$uEU9z}l_q67=o{b&=TerB=M2oCDzd*VBY{MiycDdCBMD4|C24L?TTM|a${T2CCs_s)59n`? zaW3vdqr2RlCOs3|6Yf{1f!8`ri{ENtWP^HGvJHG|>Pm04=k{n{JLI;A*(Q>eFS7EJ z5lt|W5P9zynaPWHg*_uk3xjPzvcHfCW;41TpWlG>BbK}evJHt7$+cnv;ZWpT(K4L|o! z`Pz9iqWQrvvyTr7oWYcx#mar*d?VX}9g)!e@5nIo%d8`bui6IgpXd>jMAIRXHOo!p zPe+T<3oGXH&!dcd-Pmm|@T83`kpVf)-T1YRFy>$3IU%^3{z_SdmHW6l@h~J7Q7~l#%P)ro)3a$4f+ZYHJr|x3EXADiK?6h6 zP2rToMx@fBsm5^XVH;BU(bS{iNr!34Ez#6Y)Vw`f(-TeqGRz0-_2^R^KfGhBOI#Lh zPahQYEM%!Og>gY9ZqT0r2)9GNX63bEez<-!)%iu&{vE zkg<2s5{poR??1%|J$dT|lgmjVIK#0L#LP9m^$^>ro39H_=9tEJ2LFp;%%H<%q;ce- z7%kdpd1V6dP1iD|iz{^SI(T_P84;BB#9I=}*qP4{dzL#C>x1Tp7Iv(yn$wz%eRD99 zEpqb4F(cyNMxGyjeg!l4`JliPztYLf{4#Lpc4E8?7>p~ehRXlP0aI?zb7gMb*|MND z*Oh2WuO-usqNYXeoL6d)mPzbku1he_DVbY5p`T+pdHtA1w02zMvJ~r4mXUuF`6|Fv zwGRJ%p0>y2npqql&0QY(OTe+)HZaBBXxI5NOA?Gi?PSqd%*bNozXH=rStlDbaQp52 z>8o6s#Rk;*B4D`T*Xc$vwS|x%Px={) zllr7^-QX#OST8)3*Kx>2OzAaGUf)Si;=eh#>+fyM!lQOgkkQ2VESxgcd6AIc5wl=o zWF#4$$LInk#hgbxp150qldrlQz*d{DDZOQp1R( z5o=nc_~}tcw%G9g!&q0~!mez~OrN2fu}t-8EN}VgiEKy7PS*z#JJef|;`q=_bBK2| zV72_-U-{6s#Y|rH)=%3S?H5JP|M^3ftxfi|zK_MH0$HLBBWA|-2M+FH7B;t?vh3~N zym;=`nQv)=2^?t@OnJ~M(DwJMn=%L5ddO6fmTan@@@Q`H_nSt@fEu12lW{Jh(pDr>hN8PM^L|OO?;7fnlUe@=4R_KXH zg302Xj}ol>w6aqCF7W=~e`){r5Y`Pa$u%CtikiG28yub4D@Hjj#l~}EYN-42jdWEC zXoY)8N%a^^q_IF`%(T}xsW0wgZ+fF`FtgdvOmuml3NQC}aR-N39F&2$AnfjblZqUiYsl_V^6N7xK9mS*+<^PUPitp zy*vqPuB?@l$TPH~hbt0XCaKAu?2mU<{4%{pUu^y*w@F`e%9Mz4oDTPb+tX9#U&D+M zdxP;Z)?3pN=X;coP*{@)F<#$3W$~1fsl_bz+;#U6c{ik!$t6CeegR{`31@hZdtVWD z%f_o+X(gmFtvC&PhUZpBoB0hm?xc45+J9%9X9mq*@7V9tm4NCX-IszyNDk!t*~8kno0IaZ?Vh>+TrJK zd(y5A3fc>PdVHDZCBX%@QGLvSG6%yfUpy|MG<3RL&?KE86!pN0JWL;dub9VZ>H_b7EuS z%b#5o%(S0#)E|l30n=@CL_BsBXt1!Oboj}<*ZuT+-DR8){bftA|A+Iyqi;NexeU{} zlf!iGFlU>u%wH-vWuCim$zw`(&GlQZ?x5qzw*Dz-;J;`~Jfw!c3u|+-z1r!ChfQM7 z{&71CHque{@6cps?xrp{AXcUl@&B61UjuA=9&^y!tw5JX!gPQH#VonY`DB4P_Shz?mAX z@42Y+HSod8i{2(d;wHle8<8NzApSU^DOT=;ysB}fOBz>3eRF!9o$WC=SsAsBHAS>8 zg^}N`Tg;2uk`Z6ljb?YySm)RIsQt6fe6&N$(ei-1^cg_)DGeH=h_+;pGToZjihZrR z7BRtuv3h|yjOQEFflNhv<0VGEx28SOb*I!gBDn2S^-g9{ya+kybm|Cm*Q(4ptwdq& z8Wmbnqy!jJAev4Y7P2^fGAPGm)2`os|?gUky1Yl>JWhaRy$=n68z&;-x9 zs5quAP8ENvOgHMpdhyn`{;Je8Jb4sptEGdUHH*c06>8{$iX3u0D6r4jv%Mzq&usKJ1rKyk>r(zhT)Wm8Ifln%KPzPN0t+z}7iAlN~?smQ1FqjS;IBrm3! z(y(5Cr}AzFG8d=-XSvUQP%fsMt9S$0Au(NLEKX;R=0)>|Jxd#u+xdUQZ~aGnWK}f2 zl+N_9=T@Oz4V_a@D-SL>t!&}boTrt|3+U4weXfr_B|O#8NmYoQp9)ZK#Z>XEa_53h z1^UXo6Ntr2DKc3($sfN}u%vYFRc>8S2VQbW?W#VboH(92M5ZPC&H^ee?OvdUo>j9& zZSOyF=(+=^sYPkq1j$i-Vvuo`$;Ano5q=Sw^SZZC_7ooBKT^ojc)n zT1TJ8@ey_vm-LY$Sa;)@V12KeF(0(Or+j23{m!d2!`oNBcafiYC0@xqDyCfv`&Qze z3ctG$VW@D#|D-~W??m971p|`i&Sf0VB+&Kojmr9^PT9cEkf&*K9CgZin;I%mwNKIeX7%*BRKy%GQyTiJ#;nkz#e1Vq1?)2 zeUg-RicMyZdp&e*qtmkWJluSDOG1_nm4K>AHceP=xi+qC<-kbhY)Mv`@zN;Q570 z+y4>T&*`D9S78JdW5(-4weD4~5_~0sjxbC+1 zltm3Y6`OUZQfxI`Cj<=wO6sXRAzl;?zZguzYQ9%BUMDKqPl|;@erDcIWz}Z3hweW& zjcHuVm20dQDvX#*1!oC*p7bGngJ5XqA;a-vX=Zv2`X0sN4Opwdql3&U^JTkc6}Lo! z@_zfHUcEWl{Nt(EaYJ0>YVMmhODj9aA`W*`jv&`PH1&;P&j#i6iT)D_2|$@Oy z4gQIHzMo5ktQ?2k?EbsLzlNn$v-0-vugBUWW5eyk<=fuzCcsWgOZKK8Z@V*dX?Vv_ z!r})V?UA9OXYD>ev+&clB+EzLO#bmUUG~Xt=%enNS@$$GI%d3*`cy<_h|( zthi83OkK)xBi&QJ0B&lYC$<)Z`WJ|8%ZtQDX$vi^tZ(@(T3{_asB4nAw0l=6^!kRl z4!dF|^(7~i1W){ynVTCs)KJstPFOkYGTGR)wrO4TUD>qGMAMZFYzk`VH>1B*mMwS- zWp|8jEv0Zjxs#bU%g4@}>$|yw&AJK}gae~&)+D_5k22<+D6NJzkE)^dqn}4tqXDat zK>n`9T70_dYFW^w`G{OH`b>Wu7q$%bsG%Q^f>MM(_Oe;G0m{z+?MKE>0yl`OklHYM zgLu2q7ig-o%M9BDNI6kRO;vuK_AHTSjKdl3IUg;^!xq*8v2W$TvLs8wNqKTd8e{`vr#;vkeVY3Tl1L0181#cpI3WCPsbHr zgmcd27$-Wfdjy>5!^<&FR2Mj)u+{?#*Ly(WNhi489pIyhX3pX0OK_YiFY8Op`n-=B@`RBqpVp?ZUCv!}1UPByYbzgt+NmgwHA4WP=NpaqNK1pb zUnDcZm-V2sAN6BbIcpuX_2~ZNoTgt)S?(?g{a=}hjQ@JxTeE(>h> zeP?v0b5AJC7erdI53ERAAc=QkKdT<)4ugv&7^QM4UF6;s7OW{u5kr2XWN@1vNWYIU z9|1SBNFQG$k9@kW3Nq%tqED0+3(T#bC?4!%f1=OpjB}6q<0wb_O^m-j+QN&wz&$5J zA6+lflgpj{3+Qcw@d>8D3tqO&uWUj8-ocaRPSSCZ8WN&B5o!0ARqqcnGmFh!M7weW zxThL2jLw#pky5Woc01l3+~uIBw#P}a*CBfxq_MxAlasi|u;VeW^iN5;Hbr5kv5uS4ry%3e1QiVI$T;_D=$JED8fQbRDhG@QMRA~X;CK3&JZyswDvOxZ`zDmBqv zx@N)Az?(rP>9uJ8_ZfftKlFc}(V%5Kv{amyL@1<=%={^xS3u%Jx_;?y0 z4Jhpz@ORAL3TEaFz^5AescLB5p)6k$LP?6*vr){Rz*E6~CAW7~r>~zlv@OZvqqQ>D zpHOxzpnEeh7fmjcVgKuQOq}if36uOlvdku3vol&Q62=Iy6_~?aR3U|V#q;; zw=df#8s@8^H^XEG<{}aE5C;f%lK#Tf%}(rJL*% zePt5!M3VG`oq77_{en(^x=Zo7%?~(ao-^O~7exY{d_vLyx|x zv`-a@Ii)5^)WBw;hMtI?$?|KaLQ4!irfRHFIw_yn*|J}Z-@-H|iYZ&z#?LM$zUdLK z##&q+&ciM~K}_jbxP@2>d?wKn)dxNSYtvl_+f8eagU6hI=D z9{&{>XZ=K;0A0ca@l$wy{3n&?vILa%j?uYeWDfou@MFj#Mm5wvo;Pxg=D{|Cyu;&p z-$nD7Xx(SW^S(r0((8~f;oFtczFyq%wU%&G_F_k=`&vU7Bb6aHweOUeJu6!?>hI9$ z&iXEr>zdc?D`J}7X|sxlifEadPPzS8(jzSzXpeu=+NtDQ@6hRK$?IuJllV?szSx42 zanPt!ITO4Qx=_j|^&)qefli2O*)v7%gpk{2H-noRAR$hCzy!_N<0Je(I4X06qjm=f zF|>0J@|YbWvGVS4Ojadk@8BK3>4Jv$4*2KX@teA6tfq*@iA0|`?|`3B`F2>GWJr>)51yt*C*O+l`tD<26sKJM30*ulF8_ z%uqugs?4Gsr;(mvAv-02^3>3u0NZdTR`V^DcS_Rh5jjXA$xhlI#`ybaA0Z(@lL&e& zNQK^i0_wd7$k|?AJllFqT%{zeqx8|NTAkT$oA?m=`CBzF{Yg95z241GX|Z;l`}vH8 z(qXsJ`L&xh4#V$hFmR<0PmlK@0lk0Vf9S(E|I`PypkAy(FVxT<)b*majqiV7OJ=-o zkMmVXMoH+`FluOtQYi@wrZ@)!v!eUW2RNDWl=f>WzfQ0wp|=Thp|sj{{~~H6loMAn726>)q*69h3ae8iv%M%t|2pjkCB{J zc+Uz&$T&9HE{=r)s)&(T$45Ztj?P>!JRz9qjx!ljJZ6ZH_U4xF2IFcalZ09ysoxE1 z>UdYvy7Sdd-~TsP!|MsU<$o^w#`k6arKI=Y%RUn=%ehvsyHfTFOzKaS^t#Z_?t^VB zJQiT&?VXIg8evtZwLNaJ-nqv0$vu%*@%)%U7imY-w`j(PQ{_g_TPGjA% zZz-Lx2We+N>;zXx8j>-?RFQ z;B`CWd)@wJce?bXNM?WOpX~06&JFb?Wn>*+)^|=YIW@9QL;C%sY-n4_UOVGQ&mQJw zoeuf>9~Ist=6i1w-^Yqg&iFOfMWXmOLFhl1Sa4^Gl6btsHY=0JC7g)HLT6xky6wc0=b0DP78R zcex#~B008~y1$F!NhzBmez+@1>>7+?w6ki($Dz%&`iSwBHrP=4r`j}gr-eOAQg=fk z7i2i?naD1^vtlrgi)?3%>AmR+aeuXfvGxTT3WKMg4Qk^v%@0a%!J5EDCcZT-y!#T# zDfAbE-X8_M=XY#zZ;)BY4a#qDCK2E{GE?i`;y%)bRD5Bf0X7?fGfjcSn1M~9joNH7 zX2GVTSbT+r+90D1c1L!hr|ebEwdlhi15EDYZJDAOeqAH}!_(rOnOR38!9j|}Ci|Jw zZ$aAhMO$C28aqxXq{SOxOo700#C{?g-Msitps?^<5AEL~Rp>CfUyFVtGwHWCVssR> zWz@A`UrFgy7TG+&5SdNnhdnv1iP%etyxpVAv2+VT%Ks)q$CBTHF=}8Z{$5v|ic{8D zXn52lChn)adMxzWXl15la})FuIZjTV##PEUbLNet8Ip8C6?pJ0^yrQ;W(Ia;SObBb zIN|Sx8=H9xPTM1pvFNNF^FI=R1v0QfW;8n>PhTY_iqkP(UYrIlwsln7 zT4c{aDEYQd#a2V-q*?61Y%{Q$Eb~vxDeK;+ z5Yp12n^)9*t;}&n{u{*-5pClu&E@Uerb~%~J2B7L=&`_?r#z`Wm75BA4i%3>oop@iV2^lXT#KVly64oEY6LHCl{_ z+Op_~iv81Oi`{vU6h&VG>+ECKVB`j8->C)E_dC;}o9C20d~OeXMuFX~pfTYf`aL_$ zq0b5MmQP?T(Y~K*Gl*OxbfBYLvyf-Au;SO~c>!px71lZQ{NdT?Io%WM#yW7$()ukw z=)a{VCVx`Ad_CTOI+j7q)$uCQ%3EuSx~kHpcgx?E(!b|3QNR8IpOWU8yd+heg4W12 zaUFDL@Wb!_x%9R0)Bc_P=Xg#%U01c+J^|Y7=SJ(QGNih4_e4}5wLBVGO^QW_!hRkh2f z-VB>yaxF8P^HVNiljC8<{rIAgcMP_?_WrJH zu7NeWTQOLPB3>&s(T!^gv+|7NzPaLS^ZV8G|*o2 z&Dti-{^oSPk!#M7xW*R)1bwIDyAhn~)c|cR#mAUm2j15Egq(@p2Na{VVyYQyvG@u6i-rW@) z^${Oyu8YRr^6}<}0@ikBb}FO^X1XnBXG^x%Hl561X2UaXWmjKY_2<_o}Op^7BJ3!%uD_0G9(t>b*#BN zU}*nP%IMhbjxXQ+Z#(=4ZaKk!+;yZU-k#6G=N2XDv*PwYIVH*1?oN%XOI1&I z%!kgN?wXHAbYd^vHA&3F^5kIB>@;~BJfZdQYyL2>u<9vDo;OQm-iZH7osFnNvcK>h ztXk-Psr<8xdNY{`&Qyb2E{V8966C^(V=saenxh;rk)d4hRGV;J+~Wc`;58B3T+DH9 zz<+FesG;P=54iNVz7ul?R*mM5qC7_awwp;rca&nNDl6>60?dkkJH*VrQTiig` zI=icYKcjVJSt%{vk~~c zF9}3`d1}x$v915iG@E?ZLq~lS^LpB&+i%%g*0+BmnYF-oT32wEv)Bec@LcJKW$UX` zIJB25-Y`CApNC2Ig!4wEp`!@@YfGGQa^P_N!n)a8xf_q|I^9M!8QW0m*!J_#7#^Qk zG5DvH$o+2wsypURq2u@m+&2LC%%`)rGBq*7*~X4d((<=xp+h*=j*PiTS# z+n_N02@2E8%xChnvp#Jb@^kz3d-(nFdte6+rZWk9^1XcC1JZ;&3%o1g6G6<0jgegx zYmgqfbY~1_R8O$TNHKQ>Z(H57w;tQY+&F6rXe7LF_p$ADcFuoLis91;`c*@PV;Bdd z&>cU(?84pq?5W;lXs-@@wH4CnAADLP)0=6n6wg49{YUUe#UGb0Owj~+Xb8DD&AtQJ z37CG8_;xTO>6z{(`%^uT%NE8VXyA>kY5W#DY#jL+IFFue!gB(t!84KEgJ%+{{wlW5 z;k;Oby?1>m!wXTEZhE9zu5}>ztpy!CeL!Vq)A}m2*C5M#kVBB15 zTGUR=!q?d$ALmkFGbB!8OMxFI^@VDNKWQpSC?R|4ofywXBHuX@=h7ohQ}9V8L_Rm7 zE7iG9T)f&GwM<6+AAS#gWY&$i1LKDN9`ML7IK35movd5PtTZot5iJS+X|8?tI6PtX z86vr``fP~4?{3~;lDK={xo5{ux&B+ZqmhBZ3h64|?lt2ai5ohcqpy(r(0vg7wQ(-@ z1-uz1$-0P7S50*N>OJpiMjX z%DpL7hg%`#6)mkwE21rIs{XCA#*i&ewh{TGkzL3cs(K^PQc)j`y&0e)2WH|&4q zjyUk@I)$WA-@R#Elg$y=Ot|rHxM`G?4jQ3njqQ@Q>J@vuSX-6v-6{Pnz1_}>@u$%R^wT!<>H;Pv0DEfUtTIlSDuL!M^jMSHq zXX~U!dkFAxW(8c|Mc#cKV8@@PWz?}sz^w$_Y>`6dpn06olPfZNMnl0dW`D2rS_X8t zfZaDnVP6c;T3;XzAKZ4mlT7a_XPbApVG$?CFKUwCakJ%Ig`rbu_g&QFHbOhGUE1EU zU1D;#yH{u0%O}^M{oE$|bZ?jBC?{D&ekM${rfemb+x!p z^eLGQpc6fvD^O4TRpQ##*-@WV>TAG>?v8?J{Swp<`eIPGWW>%mRL+LX&|nl#7BS7c z0pZ*9sZI*%FM-&YyULTDqA zq{@GcCCcBfjD(Ml4$!)9F+nfKhLa&!^gTC0C-^s$et?MGmYr=Z`eK~j;+SyLM z!=(Mv9(&~UYUs7>Zi9Q(_;&$qQ#$1y8OHVca(2B%GJ=P;9LU% zX!&zf+^gLfY#I4hyWe;;(~HZRjt&7@xXhfJ2v*+ zUh19jy2_Yy${EoNn>WqB96InO@zT0HeB?5!8l-0UYwq>JcMWqw^9}6+{Pm1DC7awU z+>FUwp1xTPHK_31tD?SDmqQOm`%GPc}s+&sQU-liNgHV+);Me9qKSoc!*?d5bXpL?5*Rpa7(n0u33X`$_g*R0-Nqg-HpyS`WE$(XfRqiI~$L@(4P0=WLHveCRvU4)$VuP9CY0I=t0Aq zO;W8iqnehbKJJxhxgcYg{mY)iX;aeU<`}E@d6}S|rKJtHgZNOC{-_Q=?G|zk*1mRa z6_I})HdntQy_3GnepPo0JaDLoJzbfVRp*oh&v$Nk^23t2I!t+QcGdBs*c|;WP+rvv zdq|2iBOUvDf!>niJ)WkAr@QM-ssS3OJFE>#*A)M;86t$&b7LV%=UV)ox_!?l-a@j}uAiY;~HGB=|{0i-5L&wo`vJ`hR6#5nTD=3paNWXB5%0$~+ zfcMeFCAe)Qr;X%F4fgB338nER%cN9kuGfE&6fpCbx%tVQh`R;2x5n(rLysRBW%e6I zYU3l`2holPAqH#Xs38h#K#9RhES>OguqY%BHoX|EO2DGG^@thv!qApc3iS$1)_JWCXH3k?)RKQ(#eK%;Zx7JBj%sKC_EU{7_LbR7 zztd9`rS~gA=PDdniIoECb=#n0ZgSsp=_5PRE(W);P|8Ydj{;X5R$E3Z=PJGQ3MjLt zi0096)Mq@mgAID@>@c5uTG?x)Ux~LK-{FTnL|AwP8Igm&(!DRBvhemkK4`=I(=xOl zlt<`Y@x}3_4fgmm@vifk6Vu|bLHD!&4939K?`xG{WZvvN>5rO{B#Fao{ z5P8cnP>F+>R#noqcpklorD=@vACF^IWh`%1bxn~=a=3R(b?y(PE_ZyDquf=+RqU>E zRK!EBq2(dZxXxGGRkihnPU`Z&qw!lB6IM&`7U~Q0eTAT zkF<|TQJ+S53)0gqPWerDCzLjCXpEj&8?oPb950!gD!_Sj&sX3V9?Dl&o_DW?Yyv(> z#~7m@cnPDVU1(3PIwQr~dfHAonsv$Lx^q`{2U!l)p_~Wbdo2vI$G{Lj8sbXJscePvIM!GAytI7B~ zZQbO`v~}qS`_}ER+_$a?p?h6VW%s)C2x;plubI3)eNFm$>c#1~m5$g%-*-1{$Sp0y zsW{oatST82Y{ZY0ZJE2ljsDg6-R~+Ml3cN@Vo!y?BDr!|<(^7^W%8P3Yxb=1uSs6J zY%ToBj1+fWc++@;E^;oqi{#Q>WDj@wTzUy~D9}gf-D#!gk%#ud57VW`nak&hDYc}@ z-KEqUQcB>3W=tv7yY=uaNn-igVSf|Ka%>a!b5>4Z6OfY*x?$Mp?`n*<8vAVx%*Oay zPMKZKtYpCbz(08%Pjsb1`|>Idx`*ZE=2i_fDe6o#?|Eqwfwo1(ngf@p`;rrLutx7)Pvto7E%eNjY#>B`dq`G!%g}1__lsu(Ltm% zrHx4WkfOWjSm^1uLC51bm^|LbgOCI&PVI^w^;G)RTNo!!QbBdmqjuaB#SMhGUd@(H+`RgGTR84iXk^>i0dlF!s{&OG(gO9?fbn{*cXNCKN0KFM$z#hYi z7_Ei=G6aR2%> z#)`Gye?|E8#bWXs*U&jynG{m z<;M2h7r`~s9xdj4`HIDPxkU?Zz8P`aH%GyelAo4Wt~FNOyJqc{xr8wIz^H_*fxcBj z82`J^NQB@_gbX6wo=JqOrV`=4Dfm4c#AhH3BYzZO1mPU=EPe`&MmBvx+Mc}}o{&oD*4_*`|O-5n< xAfHD6Z+<)0qHd#OnnF8C|7vv6*Vt@^oJJlU68$>?U)`JW&cWKH>HjbP{ui96Wh4Lq literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/HolybroG4_GPS_bl.elf b/Tools/bootloaders/HolybroG4_GPS_bl.elf new file mode 100755 index 0000000000000000000000000000000000000000..f7d0a88cac650ad465fed88a9ba3c4afee05f29f GIT binary patch literal 334076 zcmeFZ4Oo;_{y%)~mzlX|Kp2D!0riGaLqQ$Iiqc;V!*GM3qN%M}yA7b*K$fJoYipaM zXpz<~g}W*Au5FgR2u3Tlgr&9BR=>fQ4Q%_Zn{AU&+dTr#4G8!1IWvrr+wb@MpX>QQ z*Y#Z2{|wi;@AEz9d%ov;KIiRwUT!w!%~v>%Bat6Z(g~${MIz0{`)GoJ5Rn*W6PcJ0 zmrRmqe5GeGM?8y(M4bda>cn9emKJ)3qAC|70h|f(1gS$YHl7e0kr6IEgiz{?GS(Aq{fF^2HbUU!NPM;9hw+Ik34yy9|8+9=1Nk8(I3nXMQ~4zA`}852Js1KE z0fqoWfFZyTUvFa#I^3;~7!Lx3T` z5MT%}1Q-Gg0fqoWfFZyTUvFa#I^ z3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoW zfFZyTUvFa#I^3;~7!Lx3T`5MT%} z1Q-Gg0fqoWfFZyTUvFa#I^3;~9~ z|7i%Ah}6r{7rNZ4a{9b@}5aA=;vTJ@wkBqz2|I;EOZG(Fm?j+nG+@(DK9sS%D zL~4ecH2>ezj9T#TVLv9PZ#eCPKO*DD`2YVF_A$!;54iugdOed!q>)W}NpoQ{nIg54 z^pw_ORi6oGC+hmA^?td>Jn+`hC`b`(+cM8#Rzp01oH+HAw z_E99~5%?Ph`ZPJ3YGNKp3~3+@#6SbS-*_%6q=gjFul1_c-fCd9&ATtaOC_$bE`6#w ztV<3Vh#C4#NEeO+rn6Tc`%PX&2cKQya&$_Lrw90*&veCb&p6*#{-E4=e2MR;4F|g< zz64=hzbTjoJpzsixxnf!GC7PZ>RxDf(3(%}7rGyDaJdo04u&zfDk?g+)0m5#&p+uD z`}rP>0SBZ-DRZ-px$C|r%X=PV*441vfuPB{({=Ljdc9p?!72yWr| z*>{CQTDyyX+uFV6w>0MPITbP0!@QB0VwI48{L5-tcz*718l&v(PN=4-M0H1>sNM$G z&{wQ3vrm|pT=r0rQu;3-9aB8V0CRgY8`H7mN#m$fVL71pcYsLtGah}-V2*v{rZ_t&C;YZj*a5Zp= zD~WuXBhr^69QuFpv~R83-`1jiM?6m`E#FwTqqqH**s8kYs?(LH^NB2|WN64QsP_z# zzYqBd(V9fHRx>H0`lV)2o_=#e(NoTuJ}!$s#}!?n>U^DZMw`}1)T;)kE6#J)^+cVc zOATb=WQC}+2Bl`BA}Uul#3P;H6V+xd6eEl3S}mr5sAU?buIVRgp3bQ^^~p%HPb2Db ztq*O3OlWupqhr_3S$e1t+?k`cM0mz!EpIU&9`~t z8*CnKj?G;LUm1L5ZmB4FLsoN{$JWftoVu@XmaWWfZ6U1`Q{tw%JkjJJL5>&5D6o1A(? zlNOvfOJ;3G-94dHq3v7qp;--)WH{Ous9wa%J7~Sv9(bvU)W~Uis%7639!|Zr?}ftn z4SSmR6>zCX+;Np*gu?jza&T|qvlsE1J&X^jf2vSuh<86Vk33FnQI$*FyOermyi%ud zC`nx3#D>R;l077Ys3}`$*<6}XBh-@&?i5k~FhqUdenvUZX+nMKoG}?^+PPS!V@jv~@QxX<7?(3B^B_`MD9rr~rSz@G|#gr;eDSr2v% zk#vVTu$k2JsfPk2R%%UA4ExYlvR>8_&`w5aPx#RmDv8>uo5pv2M$@z*%~w9|dczEI z?)2ure{E6)%P4F7RR)`(R!lYQBkDQ*^3y*l5L5g7H{*Fe^z8N*t10FJg;QfSb3im3 zYQnsK)0+dCvFF-2nP3FY?~IDNwSUI|Mi~pIex*%rh*G13s0_^H6ZPQZLpS>8LE^`L zGW|JUIOdr)s?!5QCWWNe`#AZ!g+%?E&dYYRoTVD%NVoP^_Z6V`{6iO&ng;6oMwbGd zMA8FohtLC2+ISv%(-L2Y*jnA@=WM*31qoBMNy^sfbk6QjJ)6ZVyZyEz zGHG08YSAI(r|#>N=b_W}JvYA^)@oDFjjtvTx8FEzmQ2(_{Xl@2_49?Vj#UNIrXJ%9K_^{+PfdN*Akob&4FBCfVMFYWU%>N9k-lsYn1KR^9du3o4UG%ka? z`ddG3!~4*`F70a~!X>z98V~nAu<0A5*j(Q;>eUI;;@aO0$b4a*W&b2qAnC1zO4U5> z(L@(fkLg^Uw?I!N>fiO^&KHDV&*N%8=zf5wrT#D77?Mx{9SmJnhM9hf`NxA)>KCsN z)vZ(C=dX|}nw3hR^`yL>dDe#V-1myOi>S#+_0GcIca^|05K#Iux?`6~hY&sG)dWOvPwkBccUAGIWj7 z?5Fj1=a6ZZ=%jtzmknQGj&-)b+#&$Q(;KPt*@Yzrk}Xp$h*F0&nt^irpJfS zzH4Z2xP8bH5G{dFKhlGF z0jS-eFNAj2=yR0=|1>J|zoI=&|K}lP<5yQJ_k2a$3~gIa-_H?qVZ3MFxpK0*F*%wH za9QzgV^N~Vb#76~mPQiI4e%t1f=aVL774$ld9 z>k}~6=B6!p^8lw>_5B{lxo^{sB-J`+br@gW=gv^l(@1Kz!mE!D7y>bbs5^(qWKp@b z-`fXT>TV2>-FaLfoilYvT}DAlqrCSIa90KVl8sCI_$)umFgqH3XK$n3MruDkE3P-} zIewPdrff77@altob0~gQuXYpfhVxlZ)qeQ#!{lc~9tHQ&Y}g1*qm%@U*t}}#Thz=? zx3+PaF>Rwm5?~G5IN5YMJ=5wlv~!tepCL2Whcqqr%z?J35YIRM@cZlG_m{)()0mfO zo{n+P$dflujf&*`WW~|c&D)iKP0m@n_V1HE{iAxhz3>{tlQ+gUiHc2;6d7o6HN1M+ zK-xXrE@7%oNpBU>1WlYFr}Ak$?A=m?n?m6xgd1k0r3wfi?0@zL;l>%0Lg9gaZkHI4 z1Z!G)DzA3-XUqHn%!q`}e=qlPzfOHQ<`u`eyGMJKSS4*ANuA;*>Su!m(87-T{Ocg6 zUNv-M5yqso2L{fmiSF93O@y=EkLRes3X-)&4-!4+ytpK5t7(6tvS6N|zNROq*;h_c z?8^PxT=d*W_0jubu_#hnI#7P0UrKu>aa3TQK&fZ@M+FK|r((d$QM#@_e!mPVF+|n& zC+v5C$_`Nv^^e(4IX8r;ZAf`jD5W<<|GHlcR9m2vS?t}qu}E1Q%FhyzEIe$f<9X%) zXYOuGhJ>47U-Rm!ezCWDJ^Fz8_u6cGQJ$sa;O(hOkt?j_js4PWnnK0gMLFhzL+tv1 zlRC0iOKC+eD#6ts<{&nq_RahFTGLEZV64E^zTN$w9A`s2Tuf^F?hCg?2j$mB0dLxu zX}gHDgV!*YM;8-GYl@j4Q_N4^+v{DDtJ&|h-8ZLsRr$1rbj+LG1L=yOt{sx$|8`(+ z@7|@{^t}N|ByCy?H~F9beK!&L0%UMSiIvp;KMa{_Q&v&R#%%{?H_R%ENlV&HCd!K8 zfZ8&n(CS(2;r9ziB-^xk(+fs*3A~`L9x^EQ<|~Ra3M}B|)SnG}<>$&9djB=U*pXH= zxgaVOx@6#Q{@fjobK{(~PLnTXUbHN29;?U&oT?1`r+-2r@8SbiVYgE}c3DwMffZ@K z^joj_O_Ho6%;VI=!C8t)F`#Z~IpPNQd!BOgZC##SPK(c6G>Yn2bMh=ddX5qUiQ@*I z^N+%N>;O0M8T2LEXDL?ADCghSm-dT=G)o8exB3$^b9##g;cG;&MP z4Hy+ywAt?CGm_iRZ09l(+c@=u3r0l`Ru2kFanJjef7g7E9vo258GFb4g2zgIOn+7P z2O6=Txi71EUlB=5s(k$F7P=Du?Vj!bYUJh|bQ%JjA*y4}xbd#Dg#4b^q%=;|LZ$21 zzww&_2}BRxMN1t>;IhXF>I;|wI@)5VOYJ+0##eSg_MYA`>u4Tr{(%&&l2b@6`m{pv z>CpQ&IPm`C&=LQc?dchleb#m^W4zCh@n#!ck!_u~t>{A!-*dV&KKCzf8bVqJ{q~HP z+REP75D&NP-lm1l2ff92=yZw8%b93q&KgihGzJ9^< zjg#Lr1fm5}E2U*&SApigTVI!QuCarRS14zl({L(g)>IdHl$P>e)Cx(Xqa(5HcW=`^ zwZE&i+cP#sHJ`aJV;z}<@|x$2lhURr1=U2eXvfo+U`Jqwj81P7G z8LpY&;MJ$JWy*K{_qzYn_>cS0Z-Aswy9YgL?Cf-m=-f^vG}2Nuo*mMI?H8`4y;4Fh z!t+{!*P2FZa}>@-wZ2aO!*>3<*BiT2>aF#}l=YePLe~-D31w6u(K4aGy1TgZeeR%> z!)RkUX6cebB_^SisUgeo8~^XLPca9*-+vM%5|!lUFzpM`S+2*?wrL25J*D^hi#vZG zMKyusz>d6=Sa+B^EM3){&{a*iG@@QVK&{R1{N0Jp0O5hxEGm_zH+BgUF!RFZ?DyuG zk0Z>EIKpL-Uk_eBBsJ*48~ZXm=E8al=H$&3`viYv9(fmY8Tpo+ z7D>M$(pTO`n6(KSE{!fq^VB)x+M5t$H?>h80flBko=K zm@R8BFG`$8Y7+|*F=u#h;MAFemluuQ5Z4fozPuQ-N$h}#7Wl~kdfd7L1KsENfp|ge z9aCxTuTn=9CAtOm+CHfv)|RnZIAF(`lJLgCAl%RPb<$TS)SusH0+eO`mJ3SJ|{o~yYjI{YGPh!&lk_7Ep?oC*+}~PtpzG4 z8GHBM__)&;kc?;A=nBfF8QMK=5Jx(UyJ^yO7-G{XlyUp|Pw`2!uE9$r=KK-yZ?!e7xx8r!z3$o(Ne?m)n z`VHhV(bs|1N>z`y>;t^5n9X8fY4ZIxkM!b%a?1;&0^ufp>Qwe-rXH(cIK%ad;ZbHFNo*%-4=4}RBg%uH`J zPK?=~rgV%Q6_|n^_E?`~zfhZK<7>IuSHpVyvXMhy7k6fa`a0FXb%EIEO6+`}y>RCI zc+ZifgT24nWIt|oqh9-XB?|L6r$!CZXXXX@plxINLXwftD%g12y1O{_RA0=iuv^w$ zqd-s9?zThs`2&eIlbx@ffE*8Z|0i*rTNeNES)r2DrYo_boHS=!k7Wg;(4m$GFyxzYshR#RBzlePoc<=eQeI6;6w#$E_&(QWt+a>LnN8wH*EYdH( zgrBx)>_`+yO~wMEE*vo6Sv1h{wd4>B#d&nkY5f~i#s>K3-b&;btVF78ip9EY;$k{z_i`!v!L1{zTa74#>)c zGc^$#uW+fO0w(gXX0q!+pDs5%p_xc{e~9jzW51196sbu})q`*AUX>$<8y!xm#r~QB z`$C_Wi$xaYC0KT{V*isHb_bPB*AJCVy!zard7*iM-Ag8>d$Ovlu;aHI! zTCcS@!+q{}CHnVtr1=_n@ao*RNPxIWuhl_3uwvkUlu(LSTVSsmijq8rqU7rOq$(%h zVRi8;Qp=V3G%b~p-DFxID(M~>F}Dw-*g?k< z`nX${r%Y=g8h4AJ^!gJO!|BY4TbwDCT*t1IpH(`NXgO1WJ)h3E@JW{*D7lsKM2~M(M;Ex1( z_-CEzt!c_%8*`LLiJ{92`{f9Yv+qqF?>gToO~2e#Nw?S(_d#!J75cHtBRm zh0wJ{sB-?NPY?bCVSi~Oa?96R6(LXU4^k^pMIp`xOVh#N)MS3 z=Q{zZl-oU94?d%(E5_5`htPh9k!CZv9iu$C6-T={SFu_UoiZTKCDUGO%~QR3FN+^( zWKopoX5s=(mQiZdMw>CW(~$cSW|bn(N19~cfoHV+-3rdO$MLTAk)3q8E;ICQS@!N( zadO08#yzAYUWxOqm|HY)M;=^HUSHee8accTPH^R%^B8VfEMQH7y*{y4TfNAoTE$7~ z1Vd(SmA2ZU2Xl1eV_DQH&LQe5;F8_+PDO3GP345z61pRBIV|rp74_wxnT#h1*8jQ5 z{Tb&wXm1jM9w%?rZV=HwNqiGUlY7Ly8hdSn6Vfdm+-~=38CH}$6xq!Vtz6NjB+9!; z4-O7FzQ!1k_HLNA*deR5?4V<+}gbgPug(*pu&^?6W zHR!9rF_%m!_N5n1DL60?P4>0K-8cUA zUhf+1*wL<|(a`}xDDFHX9qInFu(wNyfvs%DYMhyy zc55`wNh#;)q{EH3q%7*++7rFt-rS~vo}Oo%zBX>oXN|ELAGLGYhZ{}l@3&(=>aH=~ zM=Fn3a`HQVNjdDlp3b)ud(b}7c9z5y)@sXVpS-2GXX)ByxrTr@SLmIXi}spIN1dAo zEk*8<4A-6#b5VxtOo@c2t&~2?O3ze~v?JPKna(L+0-MUc#Bl(w`Z?m*@CV|cZ3{Up zt3L2MOsh)RKi}prsn7qEWSr66?Zo?^uQzdKbR@L1e zdY9GRo<`|14rdB_@VA4%(-zNu6TDjoHx^R4pWedFnBwDSO!r;i%%@(46ND!Q`P4CZ z?;hlA+rX^{*AME!)q`JzdZQJ+k-+}mjK26(`Q@r)O7szR-r&<+W}!c$r$Z0^bPz2? z`qODX^+rhf1*H8%{?vbsvK-W!!E2P8WS_sT+^KSW9VBIjB-NF-nT#ihI%!x=Z^x++ z4-cL*6||im(U#We!5M=d^s2f6v3JV~OTcPDuTJ#<=jSi*uns2ifVNC>P4MSg4colDRWGuou=R$ZSI%RqgBhgC(7GilYCipvXE z;OW9S)tL$tTF2kyGZjMG@13VsCHTVl)6o!n%h+!SkA((ku>C?+Xe@N%%wZcw7e{!6 zc!0Fp(YLbU=s6W_WA!iq==#Q>Jb0i+*{zVt7?&+*Wk2b{ta8HM?dUN5$4Nt%62HV% z+^Hld-80&B<%v(Vl@qyHf5tQE9;YhtNp~GLrJa0CTQYl~0rSAp#MvI@7R+ZAgTj7{ zV$rgv#FL~5??|&(=EW<)HrZo!r`?lu7iW7JqgjUMGLXi`k^*69npRp9$z0iAS&B2dXB|@bhScPF1NIufwndBV_Z%v#dt+qdwVWjwLMA- zT60xzE~)LR7SvebVa#~ki(TVluZ-8Y2|Vne{TMlE?pGoABic^4h!z*P@YvU}#PqW1 z-Hb|t(j{W;B`er@*!9`%;tRX->CBXV@Pt+wnx!jeAN0Qw;9_45>Ay?<-H+(MOSYPx z@*UT9xhGXny`K%G5L;=UkGqfhPr#a>Gd`a3Lw>Evfbkvmx0IWF4e->1Ki91dTeT%C zgW#mdo(Lg(6i*HFmD+TqB5C#Lo=veCKAI|d^-*o>Y&tg+TmIN$73Y6lxud7Le#XV! z6Q}aAm-*(I;!iZ+^j^hZwf9d);l1A-@v{hVMm3Gs_)YBZ&N^r%nYHh13TWoin zGVv#Dchti&5NuQeWMidKsoU;I48?R9jVBX_V>rZ=>6EMG>q^Y_celc(N8g}*h0amb ze(S|PNx2@j272EhBxG|DZx68^?shB+*?Eg7-#_}P-LSGx4}Q^~YmSDskJ%XI2ewOU zp=)Ww+ZEf((=gU;FFCH=3!IMNNkZj-6!gVlF6rH}c$Z?GsR!TaCzDVYF{lSKB-}!} z@)Fj&Z1?=klyc%TW75eZ6~8GzuDxGr?l+#l&N=I)iQdhkO%YePl@SD)O#$%as`p0gpofya6E`8EkC3AMN6-Hp?* z3CD!`j*@&kTqC6JfEh!M?=Z zuK1?a%GR%qL>RXdGfLIh2ExhIhFsUQ&BHyitRcDjW|QfJ?<~2h{`Ot@T;1D^c4c2a z<*95@o9_2Kc))<&|HRViG-fqlR4uI2)9d&AFVr;}-C>)U(yT_O4Wp?Q7UJl8(y(*(XrK7~h{{Dq)OJ51)+XUz5w$}+yt<=5Y^xGm zYxm8+J%SzHn{hwq=HFI@e3kjsPdxOA^$x}pYTx$i2IMT_sFbchz|g>;04gq=jR$D9Q0`;S7pcOf~wRa8zi z98#8R(YMp~QLo!w>28N|AN2g4o|W{tQ)pl9=BTx}`F8i-~XGc|CG>t^C4DdB5nJd>3AJ&k40+N2!yYL26m;obu~242CCU^xxubvf|{wzE;??n zYf4azp^{G@E;(28w+8M7S`R06u0E@Imgn8Gd=_?wCvtN1>BpVft=!IZH@C~<-syFmkY21Y1LNm@-Sn;SKLYnFieAjK>>`C(=)kpew-G(Z$sP6Vt6?GT?>ML;T z346;wyihWIL;&9iTMTd%`@Rfe*{^H$ffO|T6vC$mJ3`?;`*7lOPSTd$k^p;%u5p9VmG=}j z#Crtyk!tQiE~~zAxATZMyH(h1Rp@w9eE_GhXt^G&h=zj4aq=ksP7nT5=auCRw_^;D zUcuP*(QD3hce&e!Gd;<0zRe2zwX((91niqX++Nmg=0DtGZ8_>m@}#(rxN~uel)I(S z{apwrt$3XBm+i62k!~|*oU&PY6uY^0ACX^d#2KpiWTRpFgnq9U+g_6=1USKXd|mE^>b-{|6%xkrpRtldE1|tcd~=(H&>UMW%sM0I5L^Wy&i5yp|gyt1DIFh zTeP_jFAySAiTZ#yvmpj^DN(n0O_>?(Qh<(sW3k7Q-Hb9Ca87)2uQ~y%tf64U{zjo@&S%N z*2uNf{6g8ga9zb-&Sl z?fPp%7K(L;wV|S%Rvo9L;6-=z_9 zpxHDdzx_&85c1ouRpu$k8|%?IL$b<|X}m+s>l9ZyqFtSiiHKR3dfL%xkWDij2*tkG zBpQaBTI4z0)|%RH2t?r?D3_bj&!3CSO;kQ;)Pv%XbfsgOpmc?#>%o{I0c#iX*eoS{ zx_f#kD|ALMA)~sLo}h&6$KS5Ot*<_c7dcPoWY~z0DxYfw-qZp;nhT|yUui^% z=vfvmQ9;{;sY-hmEJei^&AU4Jl_YOx18DAY??jtpM;8k*;ztu^3u4w^qO6`IpIv1+Ip>nvki z#t8j0D#P|C#pUZ^{b^acQCEk{NrPyw%s*0|&J1n1_y26tD%{ilz4MoNjz3mazQZ{RyV=hUR+T4vs!DL*6QkAJ?#<BZS2R=}HS8zJgt zo0p8g9%q7ao4Kh%8yzJaY7|a!KRzpE9l}{&Q`eE$;?5%`;W&|_LN<5C*r%NBSkYeF zx+z(2yaK1u?V8n2GJfToF>N94^VyAmbfPy1nXTs=aeuLPiqfv(8tDF6z_R(m^Ib;E zm9AU+xA#-&xNW<{IOj7W)>4T=tvyw!k8hwWv+ArEokK!!hf~jtd>%}hK zav4qXSV}?H~LEy3_Jqi;?OTKg})P$Jt&3ZmWG#=IHh(G}&G{ z-iE$%5^GBAq|f5}#U5Mk!w9Ew526Ma`>o(?xyXqftPz|qBb>&)0M5C7IWx9h+FdrD z-v2*kGF;?~&Xsmb^&fy|s^5{B;lr8gs*>?X1F8JpK&mhq>um#cn1obuxnpzxVj6vt zOWTVZ*+;RS;}Mz$j%OV=ICC-Dm38F`vvMtE4+dI3*~^nE2|J`I!) zcF39kYCjSx^~*lqRu%HS=@V`D`Rz^I)C5=)+(bv_wgc&{j)`OvHx)PImL;|)Z+IB1 zk}}kxNbAs0>bz{Y4r?6-_{bvy&J9T_-HCf@z<@J%Y3i39)~P~!ay4Dg{KGF#eYln0 zr%U$mxLwEF+Wpq1zj%_HcetZUcKq86|DjtAjW2B)@zx$lRK2k{U$wp8eR%ZPRy^mt z-))>+tnyJ~LgNgzF}&yA-JGZ%&Nb9J)We&jaOO(XKW%<9z{d(cZbO69p&r^Sshc?C zBNYeV?=}@?xQunRno#o#qtY3L&mp!bw{~{KE1edj3AZ6@yy5kj#b&H0wYPb0R&G!% zI6*&@Ptx!6JzxG*^Gxj7(^>PO0h4l?&YC2A!}9&^*r|!?CftO!;9m1z{Bz2mgyng? zhs@|}i=8lVf(!Q*TtO=3cO11aNR4!=MLO`4NhZW zR}7fy_cRau8uEE=R%!SB!oc0)={0H;srEl#emP1VO{zjAd;=vs?;oo=Vl#b0{hMyF ze!xxV_EGzWf}i#Y^_E((KB_ijTy$;5#+SA0o~AqPFXKL(vHoE#7xxMr2ljX#)^gkr zYfdl4c~^cYSAol-7DhckfP3!?{R6-LpxZIJvFrE*hkC+GOMMXaddzFuow2dFbN6U6 zWw!5_Dd&Wc;W+o9H=;+z##hD($ZwW%&2XK4(NEGxosvOgkD~w5oCJBSe}DCys;L`= zYY%Nd)<}0UHi7HVmQ&ayd~o3PvRh`2lI5u5`DlDOzc zvRlQhdpx5anccb!cOr-_rgmUEMGe-YkmL$uM)wKk$`rW>ZX4ZGHy+x~T{~qQ+DQML z9f!75Ifb@8o-m#=+OHnW8$vl?g>D^z?3^9DoC&RQ*u8q+l>!D)EdVci31`CVnJ91jacR_L0 zrLRX9jVkQ$r1qRHF^lGCOYSN6aqWU^p667(YmdNZ@i-yD;!YW;N2In)M-ERB&R+_)+9k4bSi}sXQTMR`5 ze){ze_Bq1xpi_~NNDJY3Pp&5}mvajZiJ^019(SE{)x$zO&J2l*IO1^+lir1@$9>WS z5Ee?*pE!T7T>_Qv40PF zS+T}FkiBq2*G7G0ZYeMha>er-sggf89W9!d_t{a4d z1LwL+J(r2jRvXrl_(Dg>?aUMY;@$&&9YT@&4Bniu!|m+({W|K4a%0BJiYi@fFq|B% zTGI@^#(?)$(dX4x+U+fyZ2kW0ZJK}XA|laq_Tg_}^547TYEMu3?9x<|K}qx_s1ldh z>P`K^RM48E%G{pN+rhc%yT^N1_|!ce?eu0nT;pFDEGNUE*zvuK#w;9;DOP1_t6LIO zS;}skV;qC|Q#Km{*%sMoJ^3K&hu)UIt&gN%bxKO5DQD_es9qt<9hQ8LG2vvXarChC zlV{^$w@YmqtFs!c;J!k2nDk(xe)TZ-8Drwf7w?QO-`@Z$FMm;aVm{4bUB&OTmDY4+ ztb?c@^=(H;Px1B-f0wx=JJQ^zPmSq0W(Ot|!899KJM4 zO+p)?YmF@)WBH5DC}nebcI!6JFOr&^q7rp1QJJa?1~c?M<@DrmkLTv(J??ZxYl-ei zRAwP&y#8wPKRnkecGn>EKI56=>cU+SnkEj^KVi>Sd1{?O$j6!$a(!n-_f?P`b&T@p zLuHU#2D#}9l`LTAaa6}Fg?nT$7#QMqpZ2_(jJ;dP?i`}B&-!VqFX6`>+$M{QOgvJ; z*Kc*>M4TKxYlwT>&6fzJ))uMBch)eg7P}K$JX@N#c(_?x+$&O?C1Wd*|ExOa#MUF8 z+!B&X)TjIDE;-tmo$fvPae{~VhEhQ-%gz_+&RUtI{@LivE7|s9H@_&bwUZngVpx^vIDgUM8UIO-u zb8(A}jv3VdAMj`6-8>vd{RiQvaprK?OzIbg`O3m!vnbzB;iq@2^q{*%EE3Qo4`Wv| z*Uj04^>j>MiZO?d5XdBizzYz6kUoNlXo z`Eb01ylFcX?iDf&J~3mIFQb{#QcG?*?sbR8&D;___!&m7bAIgd&x&`iaASq}L%Hpj z$~6>?Etl3OdU&inm$(y4Xn8LL|M~{i$=+0y61Cs~oPt_?8ReEjZXZ^HsB^miCSv79 z=>>fm<;%-Wu(|bMhpzc)I)j5q*yCQ1Q<71>*;9w|;Py&;xa>Px>FtEqbk3Gp!YMYK zdDHLZxDml%))Xk;YN|M&=!5SSm z_sZ$rstdt;Lpsy5vCs8HjG**B&6ln8ObmO*t#psp+7vxs4?1=FEvjXnH;wk;|Ij#_ zOG}IvAkBc=niSL0*Tn)V6L%U9oj~6AqV6AplbY7PE)*><#XO2#0qX!{hIA&X*!*30 zoL9!vP$j2rfef@6lP6^ z@D?30!}TDbUW~pZ5AR;&zNv)P<+E?mvKoqnKmvc{tRW4&Gilp4ji&vE)?H;&%EH4~ zLD7>aqW-HtOIe~N%&5bTg}wCMv)sg~ZcAonD;aCCzjxNKjGm1W^@BHNR^%p~NzBGK zowc4lxGyP<#r)U5uDs5((;1C+{E|DvUE#jWUFZ3!dvtPLs0VbHRJJ60_9lH3YKPfP z$@9y~cVPt-=?^4rv*#V8u(-nfXG*eL(iFkt)Do^IskP1(?zi0n_PDd5LCqU=p3R;~ z6_l6WaW6y8Imz3d|LoYGI4;RNO|ID0$^|Su<+kD*#QQ?+kM7|o&L>x(?;BT@6ZMz< zwu%=$ZzpYcUe+Fu8#wfaJ?)ugwtImxcg`1m6ZmDHM8O9;-+qm2Eq^2mOp?7=!s+*Ap zcY2&FN|HQI+=x#KrwP45{~)cNUe}5fWcy)0bW;`Q2E-lGug4qOi`3I0xgFo`>ZF&H z@tCW{Hql;%8xA-J2;bW$dOB`vDHFi83n#iy>oG;$X-Oq~+KLkB(Rr8;y?eZ~R$#n) zKt~REA3?9q#cB8+oC(psi?tQXj*%JVp?63foxBha`xB>GzUf7dbG>Ov?(hxIZHXC$ zzjH=+Oeu7n>raitm??})_7uX7k=n37WS3@|GV$LHJW=@iAmG!3%c0YJeZoQs|CfWK z_tVT=<}(4wWQ4veb?!9D{03!d^S`Ghcn!*8>`&lM;vD>+fVsJD+#}KvUc-B6c%3ua z7lm&O3f3mMqw(E9bWsZ21aLX@1O92?5$8UEFFho#E2PPX7;o0oI>Jyy%jJQ;5^gn| zG!M9LNN-3+jhQgIMpXjMj0-+G4$|+6F2`L1TE9|LanE6BP8Q*t42{MDznoMZ3 z)F#?;0JsmvEX3DFYGU6kPmS~H*66~hd5b*>p6RV^XGsn>bFo_-D=7H30N<@~k7PmP z2M4*`vO>@JNb9}G#|syhwRX@Nl66iC%Ze!+^>4DMBmrl7VOeF6MZeY~Hk=m*Hw;p# z5t+Q}SYr)Nf-3WKQ@pTxy$;fwu!wl*e`#oM4ERUm?k(=#I3|kb6ZCxqj=o(z#6|s4 zm_>M3ZjX1J%^j9xZ5b&Em=}p|DhYkZ$~#CUahRI8H0}j@l7Cpo_l%+cJwU&8)Pv`6 zeroWAW4WC)p01*3dyhC%QQo;_=qYHuZU^?5>)i9tf8+#R#Nlf!aM_9DAz&q-wRL{y&8S?i>Gw z#YCe2XCmUL45Ds0gjSME(w3LezIX~+glX!d7A=!q4uEQ;O9Z z2OWp_KnjPk1+C2ofA+BdL->~t`;Wq3IqZK6{)dMBhv45g>~9R+uEE`zLlm>Up?RIC z0`A>>7y5Pq`xKZTsgALsJB_$4NLRO5<=5R7U0A=SHne7~#eCzFM1 z4<#$gPPtdWHo-Va%NTAyxFrlur_>Zze%uq~=xF>Px84+2VmcX_jZY7R=WGeekFEEXJxh&b_!i4i@Y{8z{$Oca0nRRkpd`(Yz$CbaCk;rER5gWsA!m zDQhc>Te*1UBP-ih#;sbs3irumiro>sslUcFa56ND%%Zc%Bf^D~N%OFWg8c}KJF)N- z!jM14!z9n6+=Y`QEn!}r`-oO;jh}~GXmWg^#cjdOl2~4x+TT_OUV*Q}e9o)Ud^AFm z&~7+B^nW#ma}ED*4b;YPUyh$z!Y$)4`eFPOO(HQBN=-}31?)X6DX}#eu#*ziu*kD3 zpN@<}LAe!Y^^O|!qX-{A%vTFO-$lOYf8!G)^00PXR34G1y>+QPDIyPizKeWc{u`e) zBCi7SDkAcH7x~6u+`FY*?Afq(Madqccrw1Q7MwnCxRtJJKp6^aLHR(@S#&7))LUrB!x)Sm&f{KK0;S*`Qy6~-Qr@vE-v$af zg7SenEs7^&3*+xZ+?~7fp8<9 z>aRn=6>rf!A3DbUBy>+|D7g47eEX6rHAy%x-J}~+O)}0+@s{xZCEjMd*Wqo!dkx-~ z<9$2c>3H9!n^IHoE(=MIPMr)tT?112bA|~1mka*)1l+*z&HPRDo{7y+I9B1ZtQqf| zC2=?@<~4dhp_R6eDBMX&#EdCm4kc>LA#0Oie!P;|U_m%9)32qixc!2!67j>>uOxbi zdgUR~imyYlw&AYfT{YAalntka4Y(;%ya%^45|gbhoc&zr{?VR>50Wn5z`3xJ3g~km za^g!-e1Tv-ZP?I)uiiMZw=KF$H2f8}BtmyIHeaT350BBfZ`HhBpC7qi-9)CieAJ@g zYDmU20Z}~E`ehJ{uhor0Xs4+U)y2(IuGr^UhyPP$=DrEu63;EX3xB_ zsqSz6Ny^P=_j{cE9dVvz$SbOCq9@h0G;xnJ&V9Nq1$;}~sjfKpDL>y4jnnDB`ol8B zkc=|7*h=Lrb${rmUq)In#|R2an?e6G2$|XkG`Rv`4_p!uNBxFjKX49>$!pf$Hoau+ z%DXGeSFc$=yXmZos*ADUU%6^s+1m2T^=sB%nv_b;y&+e&ZxhXem4xL$-0!u7$O zM7SAYgI9nL?-xMho9_Sicjfhv1NT$7d2q|%a^R-HjfE?RyQ8H1PAZ^$v zFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyTUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg0fqoWfFZyT zUvFa#I^3;~7!Lx3T`5MT%}1Q-Gg z0fqoWfFZyTUvFa#I^3;~7!Lx3T` z5MT%}1Q-Gg0fqoWfFZyTUvFa#I^ z|K}kvP8=?Cr77eYn#z%52-Amyj}{Z@diXPKL>>RZ?0`xGUf_T%9*<0Wpp;Eim&I<7hE~yNtVBNaT{ZWG;mpFQ#!M z8f-d#9O3Vci5^7~hfAt*M6$!bD8~95A^(#YEBu%8$_UGZlX)WfctV^p@>Xe^>3-t_ zhHCLaVLShuf0s{XxZ##wLnImR2)!P30q6ucH{72$OCR!JEpV zv<#Z+O3OiMJLs?+E0uF|jP!lnR>bWBPfkqqR$&{><$F2slG$(+11cwF4v~yt_>nU5 zw_85>@PqgN{NB4qV~8{A-*KY4Ek)XibBUA*e}slN)WAm45u7PM{iVRs_>px)!}cP6 z5#+6aKPM)!%DC0AO}t-tfR;iYj(-X9%i+&Oy*#6{v|(3TN& z<|Xm7FNt?t5?^ph{L)L}Z@DDi1DXzzk^EO*LRW&OxKq5SzSW?U;E%MSNH~H*Z6gvs zahQKE_-VgF{a1c2F3P@GuELAuFAM__k^c>NDc01{wDd>i3}agS3C5gAoR~wTOXH|+G+id* zXn!6qPoCwIFxruP9EhjlDSatkRy0Nz;xWXB@}hJBXb0$FJ$^#2En4&4bcUkI0kKdqGFW97-PoJs!Nnd0b3~AQBn#BXVipKc@Rfkkbx1(r`SbPlL_| zP18`C{-{5qFV#H~PV*TKcTvb9X%d}3)GZPYcH#)93C&w0!$SS8*(D;kkSR9 zX!9J?sz zCCFKgIA=`aR^v9q{o(^cHD<7K%O}Hzf)bv`F4g^BGnGmRZI-lb5jxU0BlQ1Wd}O}( zU*S)5ec$&fe+p>Y*CYAMm(UYINAkZ^|GCIG6Y;cusiU+5^lbPe^&JUckMJ;lVVi>H zNBLhuc%*+*`cI%E`BD1lFddQe5$H6eg$)vFue9DNpNjBB;c(g)-4;4X!G4g3;~jZK z`Z5$RS4mq<+l==c9uTXA2P3u$l`&BXkEz45=hHDDq9>A)2u_rDBs~%r+Fq7o{GSh5 z!{Zs%vw+Hq#L={q;b6;!XT-%Z(KH{Lj{fX$BWa4~NIG+v9+|I_HS(Pg|A@Aa zY1qCyhyKEK@qOQt5k}(YCkK7t7tuLFZ$}u-FGAD4IkNB2 z`i#U;zDRqae39^pu=^C)vm^Qxf3GDbF=BU9zUch#=W;LkIolEtsMqDLy9`4VT zBjYs0jcBuZk-DJiq8Ehcm0?^$_FSZVDTrGbmPyCvl9A~UOlbQYPPZabha>BZ%E^wy zSPMDBb{>^e_+z{j|CQja1~2WGv`#5ae}9Gh(X}ssTMQisX+9BNH~b^Vosn_N5$7DS z_P(^vDg8C_+zTBdePv{MXgoE>4D0(YX`pz{JY@4IL|J@;>-!Vj#>tH zh3DLn`p|r=@p$0(8r-FOEk9OX2l)Sne15Eal;4f`ZxB!Y5n3BTkIbLO*9_+$87uyG z&?5FYFO?Um=ZO4E>5=@>aQbA7uapl@?qx7U`(Z$Sh!*vJC@}osMML?W>xm$m=(dd@ z8PO2Plbe<>gbD;E>NM%5MxY?FTl(21RmlC5Ms=XFQ@)RNNuzy+)E?hz{B_+ z9wq~qLtnYvI=VA+5d3SUpF@px(f|V1OE)3a-O|+v|Aq99fsl<7wN3ApE<^ZzQVK#g znLmaEVpj0r%nyQ-H@Ctsm^-lFESkr_q%xQrQ6{6=38YNsKOtN)e}E9Pc{%(R^9sm} zGH-^oX!G3&8D*Xf`7!1!XcB8yA=7I98Pdj?Poub_%|>v>n*~TnFef48GV?XyA7lOr z{Fj@jAjd@Wmyk2oOczv1<|*J$HrInc#XJGAxZO)67i> znQmSQnKR70p^4qx3Or|;hY&u?ydODcnQf3W+dL7uTxtFWoL8Cq@x0o62ST#VpMdik z^KE!uYc7Y(Ip(k6pKA_4&VQIk!+)K5E96{neirioFY4YrK8h;q8?Rfpy1S~Y(%o4| zy0dmdfB*qP5+Fc;u!;&A7BMQaZz^D9QBYLeQBhD)QKO=wxDA8LC^|0aDC)T3&N%A0 zi_0j^qK@O}`~BWq)k(&g$LIIHfBpKyDegJ<+;h)8cdK)kN|Q4Xl#$L|2#<0k$~f9t zhwvEZPe?n~nF!8t&NT2G;Ecs{yc37E2ReOGii4aNkmJG58wgKu+Q2!{5lDN8b2Y4b zsIw5yX6FRZCpkAF?PO;qG@s%ej^|Vdy9vfL=M6|W+!=y!i}Pz}c!bjg2}e4M@I1=N z2Iq8#J@9DfbmTk3Sp%(RI!7bNSv3j>0&B5BOu?F`*VozzTF}Hafa(`OncqmI#?igav z=n{?39XSTr?=?2TeVQ_#6*=9(+3fy+B3sXioF-tibYpp5#;UYI zua`Mpq^W1Vu?-Nq?O1NvOM_-NQ8QBNW(EUR4IjEkK zur3KXr-0%vrM%0u1j9Xx@-_u|{aV#&kk==acZHvqq28FSD?_pYQu46BWGs?L2>Wj+ zQSP!DLmnxzIeY-=Z^)xGV&yKYHRN=S*tyH<40*IhJg^~ZhQ<;u`~^eK)L33_>s&+5 z(n!*s3;pG6jTPrEM^8 zkw)rsmk%`LF&b&eT|UT=$7-Z8cX@*$kJCs~?()HgT&$5X$P_9p(b#xk&|#^@CIEvH z$7`$^7_?~B*i>Lp;{=Vh0D~UOG&bFRkHufEv03gV=#O%R#^xDaUWJK8y$A=Jw4!Y^ z=GXkSNV6Rcpx}H2M1x3?ijJp#1+TzrVz5x%xjRS|faF#zGrWMUUdabp#W3NeJ_Ge- z5Ck8JI$LC|f#kw}B4!-e#sG}e3&1`kA0LWunFwtKU(s8LISMRCkgxa)L}%kg)hOu! zI<+?GABllNeh2Q-p+41`1*-hLDNFD6Dd9z=+-Az6U7)lASqi|fv;_EfltgbroGoN& zf1hIQ07brL$}&nJPnZwtCm-@9od@I`0Doy~9U5@?I53Fs!@)+lXgdt={uQ|pL6X@3 zPVp~!4@{b(ToNrl&?gM}nJL>4qqO9U?lk}-$yL;onOHr@_qi#J)KVm01G3MAIk!L`)l$v@$61tE!Im-y97GK9ROlJ1%tq1%%4Sl#vqBe9Qj#ps zhE#o$V5LFG)F&xn^&mB<&qpDGvkHGb>Y%;+#4M6Mu0zGjR~R}7T}D35Pz-GH0fsh! zS>Aw96&se7jO0sJP0uS)4q$ckKziP(kx^Ttef$LaYAldTR9VO+02a+p6Q ztjyEvTc}{I(TLIeakLThN3)E4vg#!$WqvM-B#qKjKpkMUTK-EAVs=3s?b`DVBj1 z4DvUJS@?dT_7bgiSX&FfRL~8vp*lQF?Jt_P@)ykx{$iweay}Wfzi4WI(d^(aMkXfD zB|`^)F>+{%jtcUmzi8UZUsR5woy_HM5#EM}aun_A+>;rmqiEOUU4(Gx2@+PL5Kh06 zoeaN;mgLlykRE!Egr8u!=-Bcf5sqv|r?R~@c8d1s4o>gXR5ij#VwK_lD18N&55u#p z-+{p$k!k=MVV|;H)^anek-gJyGdi|INwNOTXnk1_%0|NyIIFo#SbUvEgGwie?ytuG*%O* zgAuz`r>l?0nZs>5t|6WxcDu$J<6&ZVXsjtt*CTePPB$jrjivpSjvF7J&-}J)Y(hLv z?AID=j^_Yxj(lwy|0~P)sE*qh8$AWBV3)==#rKl)F^z4G&t(peYpl&kd{6XuCSC&2 zjA~+MNoTPI%IL}cjo3L-w@K+k;fR2ZJ@JP9BoI4}5lrk&{h2)^d+r=V{2@iUWsiiN z#G8fenYLklhLVWq6E7jku{bD3-^at{1B7$%Uk(Q<5du;5XcZy}(f6q5zJRHFhDn8d zCXP9dm4&#R*AN_+VPXVgYJ-@gf*24d1Vnm1I%BfC3ph?V*6bjfi!nOdk6FtY=Daf# zlhHUxeRaU|R)!~|aKQ5jd8qF{Gd$g>fEv&oi;kix%$s3L>mryiJ|vZg-WFMl>Zpt# zY+nGxzK;^^`sJ~2g~!nUiy7C)qrxq9}EXg={n6_T(AFlLfM^6YDwX0k$( z_IG5PlA`2ddjYCCK2=p>+OR_88=1HQQI6GyXLK7LE*~K9HOh*UWp{Qrr=YB4_HTvrXDAc@39_;u6>8oxVsqgl zWj~=Y3s|j@{j|nxV0A|Ja~i|sfSj`Th!U145u3YWm683bC}D-?8411vX1^xn#fS_u zn=VtvKos5~{z7Zqi9D+kt#%tUv!_x26QpYY_OHto5u^MAs_oD&PlrkO^%;8|GHyr_sVA=%iBoz-oy@&`zi>W08iM!`C^fg2L9 zfE1E!@)69(bf;8ELuLVmE_(;{sFpoIQb40TjI$1qQlS?ru^KwkwYeG8`8 zjB~8!S}>WbW#k9E(UtA@sn$+V#o=&UtX$0(=Ab1n60$7mQ|tsRXTd=#tMDn&8dBgO zrE5TGLpEF@Rlm}gA^Be9C&q)vJWV>Sc=0Ry*r&=?P{ka``rfBRHiIH>LVhvGVoHZ~ zXu*{#XMUu(e4sI5an8^49^`jdr#$m`K_I1pY z6qgT}qCL-Nkl#~5o)tGjrQy(zzGPR?Y~rP{ayw}AQ7+;ut*&B{fCE?hTQt+Yh=Ice z?T(P4A?Pvb*FpUgq`eJDKLg|&1pWf(Mz-!BfKrGlEf1jT@DNI9p@h*wK7q%9V(lW;_#tt8q{Ndl(rvB%%7^qBf(;F9mpIjWCJ>~sz*|- zjm$HsM^bD$RGE4tCAx)q2K7ja$494n9FP3y1FK&pqdVE}b78MtVKfvHYWh|QBR|(p zm;a2m!Z@_DrjL$p`g!y(Bw1k`5~PkA;{S7U`~NA&6~@FMb6TCfbsnrBcGBuc%g}bT z6;;!u%3ZX2&|66fJx!|zy_J;69$G!9X;K_MI@L6{2c`#BS2eu?EapDuIa7uzFp{a7 zCe=!yO=fDE6q|!lrlv`Wwt$kUX;M5sI@L5cOO|KyWI4YFi)ckkbCC>P$~-wLkt%Ox zo8swZEWz%_*LOg|vucYrb%h1rfPBIZ8I{t?qnB`DnC zEd->KK>8!l573P)-D^OZ3~UUbdn1rqyfoAR@aypc#!T}_GaOt<5p9=4k%}S6pamAF z2SO{X0Br(j^FiY|4@Jwv{Gnehx{Ap#spu-Eh?=hEsRsEgXMlg1Z^ac2ns=6wZs!fj zaV!2@s7K!&*&fYsIyEjtNeARfD=`DKlUB>9k1vc}uEYXaJeoEYD-6^d` zS^u*UG=Bd%%kWb3xq8v*kHlZ7_n_L3kaQN38A-mnFr;3-qDGj)dKF;A5KqXw;4+YC zdl=-R`CKo2LJp&!b}*`8ETE@y`JR+RsO>2roCHwvpGT6qV%kYG%fG?^Ix~`0W=SJV zsr4toEMk_KC+nA>O2uIejYUXaFA&N@)0G!8#I-+*Yk&1z`?I+ASI@OSi)(-N8<4%l zwZGn6jtg-)j;{~{xEu$pmXu=|x=$27}8ui!TE{A?xf7`D*7tWzT;Q#h`hWS4n&**q0!#p&_Llrpg!6i1? zVTn!pXE82d>A9>XYw}tV4spF&ExbkahW05)Xa^hG7XmSCE=zGko39&bJJ`@>+*&)> z&}Q5gC)m(d>4Xz(Xsd+62{yD+A67Vf(>nz)^vgm?JdmIjQSheyMW<12bYd4 zY-m3TbiZ_h4ef1+#D=!Z|0%}9;KB`UmdRy7DNGRyG`=Ii2N~`ah^KfJj}2|csYI_M zkVQ$@(2fNg+IInus|{_w9NNK#_MIRbcCevMJuExe&?aWvXR=*jLz|eF2sX5dB@)4g zHnF@!u%TUv43c)Rq0P8T`#lVF+|VXgWd|GD#H#Ie%mEwP#A@tdLz`H=9c*Y5Yp{b2 zZDNgyU_*O2@@q;28`><<_(ZUwO>BbwG;_d)b_+P0?O;QD8L(Npv0y{{Bw#D-U_+ZZ ztkX7Tag>d*p-tPllsPycVy^CqaEJsIGbM<@hW2R2kf36g1TomqKAJHks2V%3Jth`x zXn%lO^baS6Hz8i?<&o@n&@BazxnP0;h_*e5%vlOrTD6!6d@F#F{Vf@fRE!up!I*>< za=%j4YO-?c^ueA|LIR082kvzv=Ya^0${IxbpE_VF2wr!A>Hl)AS z_8p?4){l6)4s7M|0m88$NPn$Oh5fZQhO=%(vlf?_>Z0sc|5BxBvoO-cj>iiMqIwWn zhI&{Eh|N*7q1+JM1bicDXpEPdrEEC&0lga#e*vuv#C`{ZZ!ZY10uoCXtTx1DW)_R^ z9SGl$xMcP!Lu@jmOwJ==E+Cs7hi`p3NmR!O^orap3cSWt{bvwZ`T+7?hQg>D;}G<& z#FFI;4RO7h^$rND5qlD#%NLmB$tCzecI3#-ck^(J3DN5T`9Ff?CIoH(6ub$xj}V(w z?}kY+$y|j*Mrr_P-&4*kC?vL61LN-VTS$if{y#k&=h zFdUcsM?l$$9Pb0DUupy>Cay&aZr+G{%t#K(U3jTamHR+7N1C$m7M~K@PwDf8>_X{G z8Q}woj4)g=mBR~={+ypf@e9!ZdYG{mEbZkx5#h<^N~~plhj>j#fvL^CAcXc{%n+MR zxh2qs)CkvJGTM;h7#+1C#pQ!0&uH@zH2J{SrjQnW2OK7AP~4A#(VC=K2~d8D8gAeY)^}Hy9e1w0z}(O zh|=wTky*}mpNDcL0IJj{W$ z$@HTTI)Vi7r$(l!U=gig5d%zlC=8+sL8@Fu;T)M5P=z2lN-R?dQanC7^`&CuQ0V7k zlrTCN9I{q(()dzDSSkF~14J7gN?q$;;$s)6^+u$w0kGD;6unvNmjm4Zz(!!EHNop% z!DjTjVE*d*jc6-^skE@yMHz)XiByjPx@SBDcCH-7q(XAxi49)%YU9`h2TFa;HulevSn|Co^ZJwV}+ok@Vg1N~@= zvw=BJ%KXDYS%jzsjDF6K4(~xUm#Omq0LrO|It9RA)=+A3j?h!M|2+rM_7qGIJBCyE zIieepP2k@MPz5_j)Ujaq0q5x^4rS+x5*CYF)~^C|zdTp;WxxC$1fCnvRRBGOgG-QA)%RsWPvJWdy&a%B`jZIYf+wbMuyyX}9j0)~*#{xwMiE(oEcFx) zs!Y)Q>?jQADIDY|vGyq(6t4!9%!KnMa@-40KRtz`s>OKlh;1VB8naMSIHm_)hAjMe+N=6$}Ts}Z-A9^Ba+#vvedI|@Jxsk=a1LKvN!a=b%v#7Kt z19}RlJYtz)=N(Y8_~*-(AiU(6T8QjNP2IwY!v~J>CK)8;?W%CR%L$~nz zf!_mAQ#dQ*Y~)Cz|MC>xmm1gTaWnX0i)gz5!nDRUqANB20|>7HbQi1<1K9=t2?D)Q zPUroF%fYk=;!=Q|BDX3Og+ssVCvx>V1Wq{t>vRE;yBYMkUKu}qDV;_=aGwmcpgE&=GD zYPK|vY{o1p>M-J=)T-H%8d?i#rrDAb*#Syswj{;b3rc3TJO`rB^2Mv!avfO2cNBh* zkgRsqY)O?yPzVN8AxMrA%M^kXkB?5xwhcL49TcL3(Lq1VD&pj3w$GM*Sp{t$A!A+Z zrYK>p??mbyfO6U)4PZ^%LcO^C4fMYNI!~KM)eWGDchx)`d@)G0Ee5ft8u{NNSun=a&EqCV=Zdyax6@4w2#+ zHG88!pm1+wRCuP8WqS||_W^qSeDdDYaqj*Tc>W0ZuXA^AuDgo6!ohcxL>phCt5s?5 zs$f>4xtcTgT{Z0IVp1P9W#2Wl8hTk&4$z0C6L+g=yC?8&fWB(lzB`z;`*GH0CT5Zi zPU6~gA zFWSyUY)|zXvbc(GAr}Hak2IEOaU-3N-vYgjB#c~(4`A|PsKdd4{x}RTmZ(YiXrM2C&eV0=EUd&Am+_n*632UBX*&9{agKuhyw$q`UZncfwS&x?hKZi7y zy|JFP`DdViBnf429Kli@3qvmesIm`JWxoLEIz>X+hpDn(5A<4qD*FZqC|>55q5%l{ zsqc5Gvak1}rgV;C**E)93ssaZ@hv{BvlLx$W~B-qF(NEp?RzM=d6mqQ4+E=P!406R z%eO=KRx z#TfH+IgHbYQ9Fs)97v@Ht{LaZ5o9z59!Eh3VT)zpfunD-(Cbmw z`1dGUpJl}H^5idyegS?iinjVMSmUu2*l|%5=c51ji=zJV=>NVb>K~8pxG3r$kM6uE zDi1@UJ06c#%b@=8XtfOLACI=40Haz9^^Zr(4#%Ud_ZazqeLPyNm;PTJk5=oY!SQHy zuC*K9;r{=3u9fF<{nhDkElME$bFBv}NBmqBA_>v=sOJp<6Av(HCKd9Twkf9FWO@z3pE690fRN9F7_|q( zJU|R_?}O}h2xysP4^Ex(ER>3#n2Gk!wQ{>gX?#K^CZlnXnj1HY=kg3sM&W>myGAO9 zdow)jb-}q-?w$PGTKn@LWzMyZ#@pn7JlEO)A(?Zn%r$eal}P4XE0N5(Rw9{mt(5$4 z=URic_CpcnScl;morQ4%|Ydr#Ni~iHO)?1*= z|MR(4mgv8pYdrvwVP=y#$~qp(1V>p_lMjxv?otMLk;D86@*^ObqpUvx$sA>6v(Fr5 zWpmFQWo6^e9A#yD&KzZB!_6FJWedeoR-Th|ENbIe9Nf@7LFvLH$L7jzq=)2G9A3>o z12c{}4GRINEO7_u1?nWmG*Lz*IY~$NXLB!@y$r7a)5IvhTi$`nN^K*_<+=o|ZbEJ@mA$7^aJ~gzT zFVt8DD0#`JM7{$>Vo{=$Z{qyHX-7o>c*LvhQ&9f`!TSOHm8|jEL;|y@i8GQV8-0pw z07YOKAk9F@Q$9ILOkyR!B*?*Q0fh&;lyts9MgaPk;(1N8+rQi30eWc42Hms`%8{Z3 zKpS+|HW&kR6rkM(JTh%o33&ukx7&cLZJlk<`pY&r8iJ1k@YimGb@pa_(9H0tH?C;O1q!i7z5TJD67fUMc!%h|RyAg;wbEWdEH!5}^q7KSE z6h!ba7GKPJzU(IfdkA|FU6}%hv(QVFo)oL}j2mHU)Z-|cj31VOa|aM@T=vy^zEH$Y zGa4F=2(+4r$X-EL3E8lf`La9(uZEdc5=nsAsg*cMP}oy4?buC&lIbOLA$&G~zuwD& zUP25?8|EM7b&#)yu_owh^vZOkCT9P(w4hUaUmDNs*^U@!Z%4$~jxxQ~V+N|X2L0(| z=&;JyLs$PqEUaWvTb5B&$k+jsH_$-!n98h9wjrkg@Z5q;`|AyeN|81R!*yl z=6?LV)d~m|d|mSV~)gQh7!75sR!r6=F7CIOvD(o#_Bt0DeGV9Nk~l&g9~C0{VF0e&T* zXDNzvF>(qzK1T!^mj^DfQ;Nfx_-_i-;c|&NS*a6zvstt)!EmM2nXInOJPwHu0hBtE zYxutUH=z4Tf;yAcLQx7!$z1?iohjX@&T!yE06lA=4wrocb$9~wKdSRF>dKs`j30a= zTC_bAs54Prl$sBTGc!3&tY%JE0>6wojYCfS=vI(ZP;eA=0E;?a$qqg*E!qZQNl_Jb zypsJ0Wb6cR93J1Dwj)@I_K@>$hlVKT%WNFCpMLHt?B^>K;TG#YI3#x*yL6NT}cvht{uXh zuLE{9pyzOi`JbDY%6VXL2-gNb+7v3JE81IOa0r)9<&%*2C}2>>L%4qj;cI~E+jU6N zkOv89Vraw7en%%+b#xrUrPRUYs5x;5){Kq@R-p2@LpCrEC1wezdWc|1rOY+L8p zMBI*bq#;~e5?osv!nGyAwWYyaKf>oS^5m@uC{a5R(4rVpqFf`)=pkG`l3YI;k~!Bq zgzHD@T<_q_x!!NVo9uM1cjyTmx5)_*ZG87BI}4=3BD_!o8hPnjOlrKA|4lZAbvy<< z>;~jYi1!!Hu%Spw@IDmwFdmyHWS5xsFz#Rz;zJO_^>9MG2FM*44++`DV0!`MGa>{`&{uuwsQ*FB~my{{S3Ttkx;g&rTa-#cDiag1h|3Fw8T1SgcyZ@Ys5| z61_^UVE7I!H#)s__(hb!>07d$be`+OTGovWPsBT9w7&2gghStgKk1%pk48@pFPgW^ zaMxoQ4KZQfiF7YY@zA*!;muC>=G1OPBqD6}?ro_DfntQp8KIymo8 z$g561w1o;y<55=M$nV@9JyY8guvosrt=GrC*!9GH@Vjd`#v&0NyZyR>M$r`M7KhS zdy7a>|2%izoP~yayGT)y%IE~fZ5OG}z*uEiT#+|xlaC`Vl!vfoHrTv>7vbScb8sQU ztPSR(KFnZ_NPR`-uIMbbIo2*f88q{_LWYe9PimS6D8xd_515h{gv>)K)M4Va(A6^! zvu9*dPAg>CPkFdPtO(ETnk}i9nLWg?o~`Ljx+F)nNFM(5R;#e{6)ZT;k>-Nnlc>lTg7GMwAlKCIh>{iCvSt6|+P3K?btS%dmXcvac~eJW&##;oXWOtS7! zk>U+Ewk&pR+u}XhTwdh&kEO?-K{#*jBEx-6vMPamXZW;qNP8(8tVnA(&E%=~m4&60_S-K;ivt#o!ZrF-_p8 zMgb4M7d->&VniPcC}vE_>!6$q>}&x1_F^PC56<)%;4Z@vhgQ5IRe@HFS;$E8U<-~7 zA&z~V3V2vr(hA^=5VgD>a;^d>1;{6MQGp9Y!Iyyozen`bfMUid1wI7!J^%{v@h>P~ zLU5n}X-a|e-N->4E5bY}@I@20Ob{d5h^1Pw8G4*>Uk2qL5cMq})ruX##hTUyr#EtW zfO?5|_RT8mobB zpl^llAA?3K0M~DBJ=nA;qN*CXc}ARSl|M^u{lf>Vkfc#*%tO{axjKlbZo5Hh#9Ubq z=&25Fn_?JS?P(yK2geZpSZ#h5z&QHn0d+cY96lKgm}RRcnskRXD32M zpCaZ#B;N@zN-suqPw4v=qF9DYrk82IsZ2}`$q*BlQUq1w2Up3n0xwwCfi^&w6KPN- zxBE;+U*2&LFNddt0289hXrq3rSTC!q0k0wF7XdY@PrRa9q=hzR0{XK{%t2DY2QoCo zdj+RLfa(<~a1;Pkd#ocnz(}uPR#3*MEo0sfVB&@CKuj}z8_##vu^l|noe`p0RqJ=4 z*3U+6P0Wptd~!dTU&OV5gCKYupomuojz?e#0Iof2lPOhgCX6^`*VZuCt5mMRU(gY4 z{4lM`^{Vm?xh@5@em!LF!|T1^L*;sNe$jOxo{LP*WUhArxdVaQ+H++}m8)ySDZB5v z9D~+|Uxf_3lcJozqU`sPk|dK45DamqEaLjZE{J+Wi5u_%Qx46tP-fj#*jfg{Ktt7N z`#!k#0%~ZpMpZriF`{x*9TbKnrjs@YjNx(&Qa1GPiJ=6DJXp~%)Tcy}pzzXe11|?M zp_3*BE7nC*Kv@NuCj$5z^aEt|1&e5)?4#rs$}RyHiJ@7Pjn5)9^!F)|b(Dk)$VxVO zoLfo3`kIox0dg<$W%vIPiE6=O@}bN!#d`|!i@1DvGq`RdXCF#oH}|`rl4JvFYUM$NvqUo~O$eT?gT{y`s@GiLv$8*F z)&*ixz`9TejS)-8I>~2c)6uMx#ALD-KgZQl1+ajZ70@#=s~T6 zkHz1Le#pc3p!|=cvGH2^Qk11HMOpe%lqJ;XwAH03cvG{;r!PfW`cjnD;ZhX5sWX%y zSQSd*o}GD zF#IMuywgM9k;=l<==9WK-w$JLOWzMuVc!p5d@^1L%`>F$lih%bsKrkcO5Z2D2Vpnk zlkudr7x-i;HkYixCsVn3flo$335fa&{*V0&k2oJj!jiuC_ClQHm~wowx=@Sc7GmX{TH zPgJDR3%n;9qRNo;p1j)RBZ!MA@5yVh{{Tc?UGoMPGEBQM7j(}%cJ}mqi-s`=iF%ZV^VPQ65!#f!>Uko1> zCT4km$1{cx3lpTBRJd{~(IP4M_(;TS$F%(!Nc9~O?`!@|U-di=0(3?CLI)?(!G!@@CqSeOQuPoU?= z7v;Q#C|CKcCVW=*k1YDECVbZ2h$1$~z+cT!m<6l>AFt4u4Q#Ms!e=F?mqnk|gwIMW z0Swlv*5Fu@L7pW&7#k0!e=Gc0t`99XC*d03qGp}pOwfg ztb3q`S|)tf)-dggNeMnH!|aw{qS1Dt&&shb-}hPBSMzIhT!7Y@7?w|ghBFfEiIMq?u$(w!#wx^CLrdboE*x4s=kF9WvCDB_OW544N=(XU z+4GPT z8MHEk3q_<7Md?jM$z+sG%)&0Yl1WOGk4`eLhP=(b%xcC;yeJxdZlI+WizrR971SL3 zFWTzyR$iv8wN!YF+L6Xl3qX%iP>*~jAF35B6*;#7M~8J^!fK4kq6Rw@s`NkzP^A zCq;5k!NSP}We-w9d?hr2T;EeENs6$ z!7$(+cA;nF0PgGJ|LRSM*S)p@ zjBL&_45d8Jvzu~d$EpXtx@F?Q%m_0gsC z%mM7`#+VZszuYV%CRr(CRs0H54F$%?jC;*q-YsyicdNAZNE>=ylo690K*#aB-k}$T zr{WKIQN`{1JgS z00(h`!4BuaT4bugEM2<|&boQDL2 z?1StHi@`uhydOJw9L5rYd9-O8W3fDE85~1=m-~9S4RpC1Fl+Po;XrV9aJk)=3K~HE zMVH(AC70X#xy$YS+~xLu?s8`(bEiTB<#K0L=dNU!E_YT<-a3ZqXlMD3HXZ6%FSUWG z9O_ta9i~Ga>#Ln^ZfwT-wL8)A`ocoAL%p%-oNF%xqBb_2^&x(sz)%~T&PAzxFrwbr zbZ%C=X?kPRxlQj~U}MwyDb;`!z7riEoVOgZY>UlY9siGyEkB1@ZN~6w;}0Kg0TSVx zr*mp*FNy><`r7dcrQEdC`=8D%fe*^#<&STr4)#CQ0*l)Jbg=)alv4Yj4)#BpUjjLP zj{m)H@W|o=d!EYIjvrNJAs4+<>0qa_A27Au>A=@s#j-}}YdhGf0qalUr5nAl@4|) zsfgaGbg)xNd-=Y0v^IG^;`B~sw84HEh}wLN4lZPv=4CF**N%=zeTs0LrqtV&(Xn%(!2D0kZ)+LrtmG`2zE@j{bs;x^IxPfZxQU-1y zHV^TqPXad(82rw`8Ah#}M;0AK* zQU-1yw=S^+tkUSMOBuL(V)2bw;&y>0F@uTf%Z4m)tj0TbIr=DHCl}ZCyHhbeL|S^F|^2 zo7%c`-YHZE2yBZ3irTt#uyuJ3WazC+2V0liepFkR4z@0d=&eiq^)!X)txE@6m&Zb5 zwla27+jQhpz(O0DBvACkgJFoUGZ723J}$<+}W~@)~o=r6p*~62pssK9Qq;7 zmC44-I zKND)B&gh2j5qZOarKTa~3b1TqnmIu3K;SljzGwb{N@!cFk!?P2xrvoB9Q?s{xFeX? zby-)+o-FHbqcKLcBqR4z4VJYQY&8H~)=R-B_)ukC z<(Kt9#E%82vbG{{EI^g@dIYWosIoF$gb&!!U|D-YQB~G^!E!g#u&mD@@D#u(*#cf& z*3$b~qqAj%nlA=9jd;=aSf-@2WgnLGL!^5PP^NY+XUirw+RARIRX}iI*;1qBE8hQz%b@M`(gCY;$eX?N1 zGBmHzvOD|SX-I!EK%0ED>`%pS0^xc<`Xuh3;%9QI6Jw>Efb?DO_o>!CP)+z_T_5u) zVP3D`UqVW~2+D4VcmjZ5dZy1IY!>MNDYrmUC6TVb@}q2GCfwb^Kpft+bc5rc&`~1a zhPXd!k@~td3xra0Gr7IopR?S3bh%L2mjxJy`B?a-E_WYU%X0VZ4s`(C z`F=kdNqSHLYEi|#z8V?lOT*#dI1EtE$bTuT`^Tn@o*ZqMuoqthv760&^WUL5`C1+A zh(8FjRwrMp(+YXV0<=2$TAdp~XajUs2d_X{os?E*4>(=`;C&n)s)9ObL`R_eihr4^ z6_TLG7Jko2=lUzDI-}0hY^Z-Sb217-d;Jrim{C+X>J!#rN2x=Fv%V-mX_jTgnSK6|D4TfR z43oJEAlg2J(c3!%H#WqJraS`F_Rg^JXPsel$IkE)^nDS4U-As!c(fa@_8Vf4Dd~9k zzR-_}Fop^^gUkqP&nV!tYo;6V>uKU8Q&uw2-i3($LKoTxB|g@Qpcdw_Z9ZVSYNHZ4 z^a9|^xEMCexX6mAK8!M9haZrrjO(x<4kNJ^#J+$&wKUj+W)_K)L1+eK%!RMFA{HHZ z9XeIUT%iVP+HvfNG=Y*CI~GF3d;osw$$`0ADCuD{`UoWT-QrVKHPMpc)vQ^JD(abh z+mCN=p^xA^4hoiX&`C~%>@|SY4`90tflG*d1>{x)ZUGp{0@az2C8hEw9yLP)C}ALC z9tO{EnW!g_ml4>b5=H%NRo&WtqQy*xm%;pnmy3v3qUNjW`XGXyc*XGF4Su%rvX2JD4i~eO_ZDz#nX4_a6_! z7(jdd?Sa=UZZhR~csjdhPT4{476LH*e_WB zcaZxSb>J`mZ?yh;GdvX&GmH!CH{tb1TYtS-Mnk_1<~IP!(Bv~W($E*1@-9@AGBl~y z=8lFYCA_7hp{S2nzLzRAmDZhd=Rr>DWN&~vhLu{t)=~22FIdW`al)007 zY=jXQP(~oxA(k-$DJ~z(CNn)xh4{%@85p64Mp&(zAYPhb1b)k;AE(mQW*O%d(VS^! zW(#vY#gzBJP?=_C&0?k)CNl2?s=KR9gV&5>k>MhpX{H;ARg`ur%D_OTB5h)sGSFsG zK01}*42U{ii^KT%9lORf)8QLFf7Jld#y>_k5?Gb{6ke{LLXN8#@=LQtl#cADCC9228epk(^g_mK4+0KfM4sU}KVWJ><6fJ!3m zg(db2h0X2_R|LSH8a8YARMR3AR?{Mmak0Gl%h5i>-$l3qwT?CgGt2^ueq;H&DBufJ zZ;0v!P)DP_5$fGRm7X7QDIbG`!Q?x_&xH{xm#7h^xcsYV@V7;HJegH4Jer_$d0XT& zm+8o%1)$7HKJz>1C;lYlSCF90NvdT4%d{O*l+aFX=X8iU6@Xv)WN^fh#DASc<)6#S zJVm%{s{CHg{03Q-wT>qe7a{I>EzE_`Mn%P^La7Jw|k6>u&;qpByY`U0{eY zqK2^{?Rg8j#}6ExQi}20-dEAQcFt0UbKt zW~N>uC+rBWP8iSyB2!sH%P8cB1Iid@;hYkD4;(zX9iz%$e`VMsvX2( zZf~Z~qrX8K`RkN{_YS47eGv2^pq}HvAJjPK_QgaD7;qAY3H;<7Hfg!BgkRB>-r#a#ik6`*?Pso?L+e)R)HSbRV) zyv9aVx7rN0O#s!c9zbt^p|h=o$>=VGKJ* zBSUx?!_L)+G2&Si!u~;&u=tW|gPQpVQOA^;`3EtEh%w?&WD|8n5w{a0*R_VAK0s(B zp$5UIh^wuJrZ*rq`fZ-%lI9>pd|N~MAhLfuLa~foL6*<75w??!k)Jg%h-UX8JiSqD zgnQBcl;_pP<&e>AXrg&E$8<4CHONoV z9>X*H1m`^YB zNB3v9AKi|(AH7)}Za;e1{Y$b1s@*>iyMHP-wfpCFzWwO_&$l1lY2{C%=je-%9xguq z2u$q+dbs$w1nL>;;-lB;;-lC3;-h<1RTRxrZzp=#P88 z4$7dJ#}zWH_XD#IPzZMt-(gA~-OW0rf`4X3?<8g&W*?DBIjxXkKjq;H(L0G*?H3dt)#2i!)$!tE7`un8dbN8P?sV}n-09+DxYNbQaHorp;Z7GH!<{ZZhC5w+40pQt z818iOG2H3mW4P1B$8e{MkKs-iAH$t4K88D8d<=KI_!#bZ@iE--;$s-QhudLHb@4IW z@#15+L>1b=Sgr!kr32xJhaPqhiRs-#54(rQL1XS7vQOI9 zvv}q_fQNtaQSBsh_t3-cAvYmP_vHT?L%jGw6cQ=mr5KSi z^L7LA`av!r$y2MM(9C9reL?6&;=P)fBylkavj7E4k3Y!}MP?QUhL=Ei7Lei=1#hJl z?u7y)nacv4PfMGzt6_v6!0|nx=nr_8co3Qib)K!e>F&TAbMf~+RWgb-Wv-5vJc981 zcojt|-YMAy)M&s2C3={N_xPT@4b*J7ig7^c)j+x%XmIm8Zpd_oQe374?*}6u}0Cy;3Q@?vqYoK&kj!!4kO zaGbN8r#MNmXMqyNan6dxJ|#*rrSmK5r6+@WGPGL-;IDi?GWY@USNR;D652xHxb>xklPAIlY^=Wjtb}ib;ER3XMj1`;H?Wv|Fh6mi8JUfcZvxu? zN5=D^;-{PP3WSvXw?WR$0A>F@2)qC=%K3F=@`rX(Wh5xg1LYIMeE{eh8)MktgTQXU zDZ{pSd9so{g6*yw{A9y5a-T!E`{iu5uZTFCEq@IF(e|E$9W>SUUlBvt_AC3N@d0%2 zcvbff-4R|Bsw2!n>Ks?C25p;cSZ=5@DtS7ftW+>Gd(0J;Oc zC%RCRhd|f~P`!X^iCyfh?}`Hc{TkH^NL4xqy?_MW3o3$}Rcz1tWb}I?t$M**5c(z{ z&0c_?G71$_MoAgDEO}2}-NO0Ra06_-0$if)UNMU&PBMxI7C#m_^FOTDpdo2 zM7DnbsBvgN0$);(BKCvpn4pyjo)q#@Cg2zpszXHs(&}@5s_m1}TIVyS+4j5M3X*Q~ zs!CZ2s!Ba0mvz^ZH}k~NBD@RB5v!1)XxkYJs`O}4!78mlCP_f=DeyhOJAn0c0toX- zyokgZqL731Gax((=)n(VfLO&3Wqb+3X8_$IW_D;1Pa~%y0kiKx=Hfd{nOM7$=YyJ# zfm+JH7En@;;Tcfc9mt~yJV2xn$X^lo6i~&4y@rF*FaQoVp!y&nX*3%LqIw++WD=+c z1Nv~-VQ!%&wyH0jS1@Pm5vR{^M7>2tS{U_p#Kv?~Iv2!qk??Fl>ES@GLEtJt`d}bN zQ7_o|OC$=nGLa=BeUK{6qqSYXAvp(3YAzv@P%Y$31ikLZ%-ErIdd*;W+Rv z1e87lQ~Ng_%zHV_U0oonomEz3Xme-1%i$#b(}m^QaR zHRpp04(I(<=lQTZ`%ayDQTj+t5A}8G1?kgVM%3p&%xNd72YX~QpewU^y@YdyW6k#Z12P)2|)O2Ii9LR{9^5nM}uA8dK)&Fy5xK)RhS~piy z6P@}Nm0FG75f)nwAq*+=IYutU)jUDj8?d;82(7_}I?C3%ZdBp^O7SE1+{Y)@kCtyYD z1mjf#tdFq9hvZ4N9vw5Jmr6c}>sjUs<+18(<{=)NZ{V@{5Rc6d;u4iyrWtCsq9AfX zp5$cFpQ}xBK^<$`7<>d{j?9xa&ym`kC;4WkS(*nZmIj`Q5A#fX1JA^Vc_zMrYhLP$ z>8YMrGJI z96DK_YIj_+M-2S{%7N0C@k6<1Zha=S#fJyWKyp~t@Z*t?Acxw+PeT%dY_x~-$QD5k zv5glnu2@EG8c%(l`yzBDE(sY>k-vF|z_J~#Y2(l8@}ndpm%eq;Pf@YELsLVEx6&$&O;ahV_*DKYN5ptByM%1=$bWQ~B zUdpVtM$Q0};od;xwon=omLWsLZkJ5vS@Om-u$Qz>hk-ei%;ih1oY@79saMWQgtLBy zEEAD__8v+;0tJoKIBf_U?tw)5M`$G*lEG)s&TC`(a}Z-M=yEH=zebMsDT%)!NzD2l zPt+j)a%h}kc{*b+%-K&)erd(NHpeD?3MJf-^Bn2#kmu%{y$o|b)4na|Lxx`^&-R>M z3_ruP59j>A@YXcKkLRAvy!3T{ITEZ+zG@iKML1fKItOpHG3!D+6IUYx`2ktF7Oe&$ z>vWVQ>D1fb0CBq`))|<~u#r9UI2=e)mbKiaj2Pse0lMv;fF#bKJR+W30}Y%(MMM(W zGglc-gZl%L=OM#3{BJM8!$TiZHHZ8zLE=iM(cS=>yAnllhC57*uX|C_2!&Yg>x^kq zi0$6Pn2`$c+=WzrltL12Um~LwlIPw}CC4Zv>3+eOu?i`67gNAEg`|y~k7z*Q9BRKx z9eCo(X^t|CFC4+<&LoG(+T2$tZ%ULUFhp(_mU^1PEMeGZ3OJ8oQ{^f%a$1q!M&Zn{ zd*Izx8**+A!-hMYa^{6uO3PhLF$$70OS(pjhY*e&SqWsh$a&EI4F>+~nXQKN+Z@H5J#(4iJftf% zd*(@o^RNorZj5R@;*3CI&%KYxqY8;-&z!TsaCT*nL0$>>Drn(6rqU$czfy~*qqIb^ z`vT?*=l3NQaJAF!_HrQ4l}rFq>E6cTznIkuq{=;wY4$i50IANNxpI!-yrM+c0D+pX zDm5E`K+o4yK8@~UOtUw8BhoavGpNBIvbO*kojr3g1iYmLjB)?TeEy`;OmJslR&w5r za;Rx`A7Xhv$aw|EnCf0bwokl`sMQuXN#xTQhmz^;H#FpDs+G($y0E&KB40gN$b5F} ze9~d&KOm}rpKGbIocX5Bawp%SfQ6<~!AO=c*TtsGzq4#5uhm!{F*|vZ#!88K$-&fO zscBPmBFV#l&he(rzZ9C6JdAP6w9K01XkyFF{A|n~^~qJtZ>5fFNFL0%lgwxZ;u@3t znZwDZ>dQ^Z-o#GPoMV!k(!kc}xbeyBDCJZ$uL|iVByT3?TFu#<982sB(_zV{CUeQT z)r|1t)h#Kuc;{9#myLY9NL5hn+sqs`;t3)(fqDGO^aexfTW0E?jJ?}TJd6UrYo^{M zdaoHBi`Xyl=gTcNRAusUVCZ0Sh59BJsIr(<1%@tp(1#7G11CvAcS; zWU^|ZD%?h1ENjf7;KDe{8*|18dnn#mlIO6d#)=#wxHHXU<1`jdMvKQiw zI~Z)fszrO8bIiyr*gx5ojOUrjb>J|PA5!ExGlvyrB|o5E=bH&KqOp zv%AoY(>&voMNIk|zH7ny~)5NEU`s~LB(na|#_DET#q#tj;4P2Nf=8#T5n zIg05nF%$grfoqd*F!xK%#9CnMlKG6gOy{>HIfHSVbh@p{X>10UYwXVCM8;jAv3rxf z8F!_|9!d@&wpn9OB;RE2SLqJ(9K3ia$gGu-QW|IG3Xf=fV}KVJMf;uW%*g#9Bxg|& zUL}bb$p&IKn#rett-=Z~s-Hn6ro^ymX*PWyFW&E*WlHwa5{?7T)uzjuDw)Ru-(2$q8M&zGz`RazlLgVS@aMfmirK6((aK!Z1-^paLc2;fJEIm z1yn=_0`c6(n5L_e7IV8ZO=UC(F$s4V)AY){6$Veb)0n2WLaN-K&^+8~g*3STpnyKv zZ-H&RdmhvD_4raX!F`4?H4c@=1zx^S^+ZsWNf>26f>j*mYy>O+m`ZN05pYpu1`R&A|S%k@=jZL8I`-oAWZYxVp8XZGwp`{V?$_4|E~ zo!^|9wPwwlHEZ6_V2u)kDa=7PAV_`h8a#0Dqa1yPVa{6(GjjbJoHY7THE~v#7>2nA6|cq9mVffVG&2=+iI$OVBQqKW;wwO$708qipzdA$rUPD;F95{~Wd zBYpy#qOogF!bZjV>5N;{J*#t?DzT1kiNrd3niQ*<67W-5vR^O297a)Dn3i(@ zSY0ib(&}#cJgr4I4vu;by^X@Jn$g`^uV!?&2Q#|QbfNC;<0gM{51ahvv&P9It2aW* zq?+v|)ilrMT*eMI_B&Bl?bga(piE~vyA@rDnvfd&D`?&&g9(g>jfXd82>p=H}f~qoCK$)p0 zt#V3Go4J8Jexdy8GFwLi+O435%nWETg&hG((v-QLe%J~i)STg+w$#%KYRz0rp3hk2 zUxr^>rj?Y>D!;jzhX_5VppMK8Lcdhdg3RNDo>x$3hA-Tter1(Y4LzB^(C-E1*P9v2 z_e>I9;`p2%b*ueCsxokQ;1)CD785mWATQY|x4 z55lKBN6KZ%(GtP0O zMMnVSfu#Ha5O=~<)q;W?6TPTM*4$*ax&xT%8AuFF^(+93;aEtZ1;EvCd=93D^g|Z| z^EjaUU}{$Y_$ycqYev7>3y1uXjh&UuzL*4u{o&Iw6I&!OL+1lH3yw2jgbZ{E1MvsR z6qZP42O+c!v;{Pm=|KMh#|{P>?tsPp473f>SVNK?Ul*w6t6O)${|=byJb<6V@dyJ9 zV}OSlfIl*ps-$9~o#wvgBnQXewwX=|IciSi(U0D?rPv&u@j}IXZB5NtS{9V;b{DfB}8h9txe0 zsC#oHQva8PEdSQ2K-J4q@t4GS=JiNf8vc^lpQWK*mV&<|4yVs3{PWkpCCY|CyoOd5 zu}Miw3_6Uit(Gioe?>ps2})~w8KfC}jPe@Iz-L3^kHL(7f<+WUS^S~fy2(oA-$O{5 z0%B#FhQmQjVw3fiDUdnd1)kN9G$@HIVl}bP+d7}cxk2(`~f2|67puL+}JO9w~GvumSeR{qY8gl zVzsb=4q)b^3~@=C_tyA* zb}0uiP}iH_Vq?f7uqe&;2mRjbKqb|pbTW85)cEfUSd?B#Iz3ED^;v+_Xa)ZeVa><; z=*NXw&lTIDpg`e>D7s$NLMZrqV0n#uIKBg{H%tyH!G7!OLm}P*~LGSf>)_5>zTwjdc6(pzYbHjMp)TI$)|v--XA?Cj41g} z^7w}p@=#@)`@o9FKdff*I0VEqV94X2mgKP$;5NzQ8S;3|a()IveyuRp_J4#tUbE`S z<0TM36Y%)6;xXp{6lWNb+X5a=kNXhZ{7?qKg8wL`d_4#5zp`rX1nOMSo&{4TNASP4 zDsBPt1|Y5_avcW=FIqmC+z-UPFx=1;?}E=7m^|9XE7v*-=_FCi6*6&#g|%UPJSYev zTuRB~3Oh*ic+82b@Pp*zK2KWC0t`5Y1A!Y@2upSBR!R9SB71=m9t{fqH-z5=5w_27 zAhV9y4Px-Afgc5F?~wS6xDca}tB`yeXRJ45Civ5&o6}W`FpJ5_}>UG2| zXzgrqd=q}vP=6_k1#8MPLm?Z!KODVJL;bzh8~e!WUa$-5jm>v@bc-I_vW1nb5gy$czVWyPB%5ITvjZ|m zlI=gpzcfI&+dzxm?Cir}a-NL@;44`G-`n9)%72^54_x_W?+4XpP?h~1j%(n^p%k*< zVY%&4;eLq>(uXx5z)4ao^ih2Z6@of~_XkfY{W7p$hge^QDVMcjbt!fDIN(QNWa1gr zdK@jPwYu_W2<*`<6aNNC{!B6^o}VpuWfxZ7K{C2!;zzY(>;@wfPclc61&JrZeFwBm zJb_r^bHx%*Dw%jX%*0pR!xS%pH%3B!fuyUpx zkCa2cnM5oYL#+sM6!jIhmnkKnJa$)$;&V@S4ieavLIK8aIXL@b7dodSgw2p9{q}B zAE;!m+zf}AE7f|d**(0ZhIeylypdM6hOcANc%v+LXf^amBmdDnUNIYs@}wH8UjWO- zYC|o2@zos|&vtJH3lI>$L6bUw_)STpnQ$jn z{TPsXK--<}{sw@26Wg6pLpRCqfh|wryK|~vC$T(*?;hd~g)S8Fyc)XIGuZB-65s>c zuP`J};k!qS+z$lm0d04&yD|*9q=xPwIufTM)PCMB}} zfVD}(7(mgl^b&7X!h01}sqEnHiP-c^l#`6mByN*Q$Obf7c&ioU0%{Z98UCoKp(ZICazp6XtDvS_ zo_?n*s5v)`e(My}nyaSYdIh!RM$xZNLDO?1=y!&KX6MGz?@R@?=SI`-ECtQYO`zWf z1$E@c)9-8rEyzuw-#H5E%uS@|8r~4DWgcxw-ub-Jqasj!%kvI}+v0 z<(v{^r-CYTv#|t<9g{%q%NXuP1=Z%hg35!P59wEzyOvybsc;Rs`?1jO-K6}Qa!W~h zvx1s)Z;|Jh71WyJ0v+}hW$bOaGRAO=@|&KkCiGPW&CWT5ZdFiw?&}P9n+i8KH=3#a zn)2(&oy7RQuAl|EGD6=_P-pILhPz#b>&YEKS^iV`Ez2z>^i2gV&;6Ouw-mG@cLt$5 zRJc{SrziInUv1+C4!Lg?EH>do;nVDD}ft}k~Q<@}EF+mPEp=pF@a%)LmS z-&N3sxy=lBuL^fb?ni{ar~EeMt|D}w^4pSoo$33&^1Gt!V654C_bX^?j!(9FKTy!N z+=-0g0R?Rr!+w*C3e4a?k@`n=}Ed(tE+EzYG(Gm*e8@&XR3d~yhR)V$yb5_2GU`m0N zR(>TxM}bvVem%jo0&A^&2_te9SZC!Qrmv^K283+{m{DMpl|P@LufS$2|9OI01-4rG z?-MLhV4Ib%q9#feINi#RAXuir*;alo!JGoyt^COZ%N01+%AZ59LV+DtemlXu0vA~M z;|W$Ou+z$)MsSbT z_Yxecz_nKX8iKV7?6vZb5FDn!J}Z9=!Ql$rVCByuI6{FNE!FJtfmqq>O~9Lj6r>I> zl{x5>*tulAz%wbF_drwJ<_$W&Ga)8C(k^G#wCR!L=|# zv&-?)~cT-3$2FuFNnDMy|R z%BDY^Bp6u*TT|I*W67!uZ5*ws3ARK;{YuT+UWas#= z>HPmukemAyYM}qFf;a*Z{y!9uD?8*+K<_E2GRLRs{C_H_D)$>g?<=S__g555{{scp zl^uLUkMIQ!!9c=>vd%>Z3*S;uQ`v#t;F(ZRb6GcfY(J@>*0O^M*@bBg|6f>p# z=H{{#Go7gBsH-FQ3S)PbUuSMKAx}X)WrvVUMnTJR-(fgkLCebyoCmdJ6|@4-B;l7R zXjR#f2v(|q)v}fM%M#VsK$&ZY%q7d5@_Rm+pUU{k75HLO)kG4?YWhpzFbB%N#)zvE z$x`G~LLEiU*epL)cJ`RmvmkM~^^{#+{WOpm1cE+$tOOD$5!s`%Z-b_KsmLB(&0y6X zB70o=PT+*Y&{a*@E^iZiw?-&GyGUiVw-8#(_DI0?eoUT=CE$8C$(BeU>&~CsEwW1` zkaLeDuuKA#-aTY+e2S4*c`q=`3h7hpH4`{N0(IVd1XfC*LD&xxIx+bk0@<^mosx$t zD6@Szn3X)L+^dOyw(4$(oUti*%_ksU+UAhD^bqGCzzls1G@`Z+{fi~ix7TLCHbKYp z-~)x-Ub_;G#+lyjqGm1wTcm*XE2+z&Xz#Z+eDvpBw%}I(dS$V zU{>%X*z5vdSq?1u>x&d)8rDZaHVvk)5@h(bNUHUi&aIR!^ ziaPn)_XCzwf9)tBF2a^e8J6QgbCETc&QbEInv1N-lgTGz4+gGb^uu)^J`WonybD9> z^Q@ML`aEm8q#izzLg5k@gCg+|;TF(0R*aPBkvowi{(;ak%$Qzq>9b}qms$Zg!!-S@ z4TNV}x!ZtT1Vk6i*xnOx4CE|J>f}@)PKFWV-bQ{{E3C{`kn&pwV{PXp4i()BYZRlq z8pKz^DAp?#>$`!t3r4X%QL%mkh@TQE*JoB)rR3}%B*X_m;*11JhczA=7(SPJ_AO^C z7Q47AfMX+wbqqG5d}|yZ+dBb)j)iI72?|!POW?oXnII^&WXVlj+1GL#{GJ4U0^U|^ zonUnVH^J(*CPEwA;YF5W+$Khqv<8vug}|cGMsy6eAGoEg(!T&fZd>3vro0%piF~`y zdc|o5MUM>53k8$btl9vqoW6ewqFw+q2`pckMBh6AeVrH_J$Yq+7Utgp`ZWv+1lIv4 zo=w4?cCuhvCz+lZGJRSy%^Zx%3PYw(H-7_Km;q=qF<|=iG%{TcXcdf@d<)s#n_+7B zH6RLW`z~}#ZAq&AfzondJ_FLtFl{#x_d|*MGT<-4OzAsKSP$kWp};4uT+#M5@_Sfn z=a=yPIm}eDzBkqz}PT49u)FdRcx+J`QHxIAR)KFWV13qhS#8?;!4k*Le*2 zI(|+AehvLv>EDAq4jqAW6gWh<*_}-P6(yDh^!`#AS2R&U2T%!DU>u8=MEJa-^?9j* zIcKm|UM1b_jD5~miMX+X?W8zx&O;4=Zc`BQmpVM?=+VGkRPq(%o5k1& z=+~QJjQp@{j?6k#i+C){=)^lobCN@UaEJfr;D7M@5<8M1KX(X}4l`G-jGiJh^E-gQ z4I`7P`23zpohbQSCi(Eb`XPO5kkE+}>9#uOT_$M`_%Z#v4$iZwpLisp-+Fi6yO zNXW$NsgqjCj8RzIM?qG_thUu;Rx8Uqn0=9~YmXqaW9Q-61dKS8Z4AEzjJ3UE2wJ$? zQ4bWz?3>T2v(-7jl~$KHOs_5;&Lv`+GFbv}L^{scRN!Cpt&CeE$2+PuHQ z;T_&Btf_LX8JiGrPm^xj>s|n0x&&MvZ?a}ccQ&(-Sus-rIbnAbnw9(tSoy=o!9Ay( z{8PzISmgHap>KQ1brQ1$nC#8OY<7SM4(DP1B1*nDjM#=7f&Dph;}MvW!Q{4iGP!*K z_#ZGR!&tqBip$WvPArpeJa?8^cugD`BC_Kc;*$=EpmF5lGa>I!j-$KrP|P=A6z@;Y zAnzrByI~~na|Ys_wm%2jTpOj|;4nS>SShUS2?RnriPi-Ukvo7=d_vMRb?C;uku)s; zB~8UyUZp~$hM0CN>Fi=3B$QI)acr^FoM+zSlKJZSQ4uxaem zoIHC38v9g7aJkeLKb(MHRSf?Mp;!#3L(%-cwg$ye?PAAQt&3l$>OqjA*Qw%HsO10I zz;1O1$Wu9MiuVIJoZG38sWo&9=cw@jrb)ndZlce03AoM@`pl3(7Q?->K%KmZ1T&Kt z!|gtVAMf)-99T|IuY&;&94*~TCB99%PnPbvbZ35npDHNBXJr8soPQD7-;?0j;l!7; zk;xyN3y~AdJ!^aEuj%_EXD%?aC1xw=%GRBLfz3lTbmz2J83$oMDf<}~P5n#R#W8>y93|6TphB>(& z0y8I91UdPsk9{-^3icqVqaqCob`rt{Med=dPOD(eaW2EptuXQ&pe0hQ77}^CMkYn- zcDREU$o&!A-b+abv(Nd^c80T02eVHyA~Z=jn0*qmeHL*Cvrj^<&)KJg*(afF2F;O! z*(ZTqhO0tJGKcF_Bvrh-JPx{UEIs0@l`=nMne9k@{%s%PY z>2vn!VD?Ffm-Dh4^*K#^ha?blPH zV?abyfBS6Qv4&L-e;lJ?JG5!;UV8eViQ*;Dw$)+AyuGGT%4$8^vSB1 z>30^M&tn3rsTM#J=qGA!A#@7-s-C24DO}7kMYS|BQBn4fYjh&z00Gmb?v9a@WBh&A4=LfejC1ktY`LVI{LqFH$&X0|qA3|A|70t%Z z4-f0*Ty82l`>|C?Jh5AV?EElXo6DUa{Bv~aH{IpVkByxlQqFcALfH8s z)b4WU$HvYN!_9R$e6_LjL%$A}J3lsdeh4jax$|RV=Z8?IdpE;j=ZE2XT<-kfku&MH z%;nAx{#&txmb={fv9a^Ra4THy{Mgv}VYpQ;cYbW_{LpW;%bgz^J3oZhy4?A(vGYTy z*X7QSjh!Ec>vOsDV`Jxsej8lw{Mgv}A+*uu&X0|qA3_(p-1))3a+l#Qak=wjW9Nr{ zn_TYv*x30Y&n+%@er)Xg(C>;gcYbW_{1Dpea_7g!&JUq&E_Z%x?EDbgj*S?C*!lS~ z!1y>iW2cTu)uDR&T%JoUm+h>`a(OOwtORfw$xa=WJpeSNOGWDFYRXdDAyUVsc@E7PD~z*KxH^GVL%#eVcJ&C|f3AkCGWV0lY^*^y{ov>z0AeS9QpZz6JnJp)9fCQ?9 zJ($p(vL-Hrqy@1rnH^J&nMT66neD0b{FM z0=?N;1QttROSX@|G6{S|*d2sUPA&!S93w0LF%ciDeHiXkau=>)Rq%0QJmKN=&DW#D zJReZD{^6_u26Jw-#SQ4ADo+M@1Zqv?sc;+v$Dnm^tcFA6f6oxFI=j(aZ;>IgKo8~* zQHhupUS}J;(k7LfzS2*e3}EFZ};<=9{4=b z&u4Dn^QS_e>dwF?CYKnr4Cp?HENbpzI&UelhVtEN>xPm$khnx4G!sIHb>c?Au)QZGt)_H8cuHbQkC`!*MS8^bkt?Au)QZS-sM*tfap+Xyv#?Au)QZG>7q z_H8cuHim2S*tfap+vqplW8dbYZzD9@W8a4V5GbK`kA0hqzK!ALdhFX=^lhZ<@YuJx z=-UV_@YuJx=-UW&dhFX=^lc2+h0ikar)a$WtbJ4dkT%X6j%|+ivzYQMyHWz&xp^YB< zHWz&xp$k3sZ7%vYhP%XL-{zukqu(ZveVdEEjec7^_H8cuHu_zWVc+JWZzHtTW8dbY zZzHtLW8dbYZzHrFn|TD$w^^8^%7VKLrAQWBOxrr(4m!N-OGytiw&zhr<&4dX&DcCW zWAiv;^Yo0(n`h40 zJUwHRmCeJ9?KLQKZ3$;=UN~bb<&4cMn6Z&i<_TtO%mJS>HV-qlRWgI*{kpQN_CquJ ztmWK~#u%wXuXLD$%Df)k?=P(^i^=Z}#Y#U+IbVRcp0{!&z}H@J7939@ob@6*qL;1I zohfu$p9!$ZSAoUMAerA0U{ZWR0snzAng3RRNxuV(ji($dzZYOUGK8Ds)a0O_!sT|L zjKRon@QJ`Y4-#Ac0B8N)O39ZaUIg}8hRmOaUqV)HIQ(D1WhJic5tHP7^u$WYbajA9 z^S(O%>tiy1OMvk%1jfeOl~N{#^7#W_ju@3pBOuQ(Efcm|{|5C|b8chpvYZW|rk}OF zW)4)AxDJz}n$uBZ5-aFB4V65xm@fXKWc-s#xcK5zk}p2h+~6V_zSC60cbaUz(^SK| z6ByRQ^*IW`cbbOsou(AuX=0LVSq}kCIv@UG82{%kO|sr{MHBpOar#vJs&4KD$i!~$ zVI-5^**F9GQFL?rxUAgT0#Rqm1}IH&Z-wDMzYs~B>9cW}_Bo^_!^^ZbF4MBT!NvJ{ zNRP|3glvB_AzY><yi9B3GA*Gd|9bl2GA*HIpOdQGAF4J-? ztIg+SS{s*X={MKsWm+4TX$f`sGYH`_EujTIFVot%OiQTKmoqk8rllHsd|sxtahaBW zy*@9~+PF+hs82}s{SjnReU8W`n2Z;#BvtBNV&kZ2H)^_Ucx@aNWhg?EgpH%3gls^Q zg^i=4gj_&vxFMk+>T)7hgcXzngm5@2O3KO%kBZtjDoUuzpVbInI4Y{NSxyK?Md?@P zmox#wQBgt-K97ppI4Vl0$>&i~8%IS6HTyg&YU8LVp;n(qMQt1vCDi8gsHokipy@u3 zirQx=XtqC=erGDE-RDtJ`z!^`^(WA8gMvEz@$@@eK?{5y6}8V%P^ZtMqV~B8>QUtb zM@6Z?WzfqMVV|e`E>wD72@k1vPh|hZpg1b}HuA!g3kJ8KAw(#-V88_6*avDj7Yqp5 zzFsgOjeWsvwaS#xL`o2-Pa2SlzOhO7Yyjv;p+thLJNGo zU_hwT*9!&=*W>F21NtrV^@0JR<-T4pAhg2Q3kD3g%GV19^jq!g1p`8BeZ62nsMpsE z1`OBd>jeY)ZSeJi0ilh)UN9hZp|2MV8152ZFBs5oldl&H=(ok!3kLL4gDNf<5ZdbN z1p`9ce7#^mXuHVj1%pqZXvG%{O4#mOV>0SoM=7T;*sQ8M4BHGSa=5!Y3XQ6@#(5Pz zkGfdHsbG#jL?vz?4)3+e@*F}82U1kDGr-gmD?bzt8fPT>ibq&T#SPQ&tNMzwC#b&S z)e#7U@6%^TeZ?j)9sb0S{eiaGey4)B@YJ4UN?kBa8E^277+-Y~>C{z+l)CPi3RdZO zyM^=AP@!o-yrh~6ZBL!qsoG>mxrH$;3;g5xWP)V~jRdK>y^w_&4Oji}s4P&S-b-3&Ks zwLJoMW*mNe{@`Y|{5RmE=2TAysBR35I>cQ8z~%pV95t_oZk(_H&ryd;zy=$#OKt<} z(Pvjsh?41z-?F>|A#&-~R_rp~X_Y??v?%SG4CpQe*`-?tUymi`L))p)mpvZdpY zM~&Z6P_C>KjdtTb3aTt+H`n-GtDI3)m9m3syjMZBrC%h^?Rbe6O>UYx{fVXO#ox2Ew^ zNtHH02y>}|)NO^vPbWFwuzDmU&9_>!W1e;wv-#(>*D=pj|2H$N{KS=F%(K0e~l;m79>5QAV! zHdOo@^YxKX=e2t`T0S#rVxk!;jA&AhtoUWEn|LsFd!j$mulYo`#Axc59BU zO5PoNt#=8+u(*u9sd_Pkm-D~5jlD(2ghw{tB>An$6W~=z(w#8!x^tNrd;|O@|H;Z< z0o;(ZbrM34TUPoQpsK^52~qVJeyyqt5PL!-TVah`p8OiTlQZc)Vc75CPSd@1+_~-= zlRrCb86K~?S+<-kd@_Ye!xAiG6Ds zhSi`!IB!5g&!#k<&BoKSe+x{_KPRBfu|3Dw3kD|}F+Uu7F68BxxEq~L=JUy?5UiGn z%i%E$M_n-Q8(tLEvKF;x)<}mc{K_Av5^qn|5eO8Ra2o;CaEO#y<&;B!5Th0}SdJn))5+j~*6(B=xDuawMj9oOwAE+WG0|p zAm&~KuO{XhcvTToPX{sU;WdPqJTOCuIR?dIFfl);1DKk-!N*%=Wg#1~1qFqraAXC* zn@|Ks)xhyhI7IzdA%e%C|4j#(odX_&ZiC}8Ux^L>6iz{W{j`)>xP5> z`eHwNT?|=Ti1`?Tw-9qWFcXM59W)bwscD46U1`0uK%XgNi61E-%l9c19t{kHdKvM! z40-}Xkl9&?a2PT3;WdnypFj(hbkqP-Nuj=nnDfM(fSB{d+zCeoF$dE@%$MLOpAE+t z1TUWo$E(2P_J?Bv;>r_G zB_<6opL`CX1DJ+iB8v1vYZDaED|3ad$mrg6B7H>mK_F&zpkIY)>_yocq5LL_bhqrt zE`rB7Fk;kS5nkn9ITIj zkDFu^Zy>x<)K3rvZi^@DzmF(vM)49b$%WQ!VHE6j7)4S=@pEwaDWl+v%eU$%k}8Tf z;qf;a#iL;qKY&`2jADBW9E^hh?Iy9%x+IK(Js_hOT1$$yfVW}ui0f!SA#fymIbR7lGo3glF z4v)<;ij83u{{&}?QGAJ<5yhB`5sIeuNO#Q7VAHf7;Wlx}o@U}yw57W}YzD(jIu5`L zP|%DYjuSQ+^y0SoJ9^<@GXE7_pGyQbmI!X7kIy9n8%qR)Y@bU6HkJqo zxjvT&Y%CEF%KBU)z`vi8P%g_Q0z7&kp-Q#Dh$Vt+Kv|`h4Y5Q(sMhBafsG{sa;fvV zL||iyfKY?aB?22u1caJ=E)m#RA|TZ4bBVyl5&@xB#D|q*ED;cD6D3?Cu(3pN55P>5 zePj~5ZAi^U%+9W)q!Ru@Lfs0o{i6siN|xtA==$#w>QR1G{*#0jE2!508=)l%s>?3I z33Yp^f*M3A1t5zulnH^2O2%9A3_~nSCa-~lg94lz4UgbRqs`e{YHRMx2vY8*uEi@0 z=+Q8HOWjnh4rFuomb#^exn<*B4U*rQoC~UQ+c_rn1(Nfaj}sj8$*;kH($U9!DsAVe z?46()v{X3in9raN;T)G<51f#p%bT1oZyqKBc^>m|f@3~;9`kX6V?KEv^KpV>K6xJV zae`w$6+GtS1jl?Tc+AHMj``$y%*Q!C#mKAjJm%vB$9(cU=Hmp%eDXZzxS!~Ja4MG)VynF#w8`o182OR39UK*|gWrnhNK zZ?^(BgdWz|Y$|yL4*yq;!OYb91u8KXhGDGjSKAn;{S3^kUyM5wq~Ah&`VE*Gc{}VE zGdBVE4B*``L;Fa#JIhl20pR~3?k$NMMBL=V(VD^xT_ka}#B~6k2UE+A^d>}x#U6~a zVTNub)eG{yo)bZF3`0(oILYm1z&FAS|Ea`DZbPR@cLO>90)9`xpzCRd0?1n`08d#n zQB`G3>K9~ESAfXc&KDn5Qok^kNqrN6{|2Lyx~+*xtyzEpC`|QE6b0*x&8Gr42kc;~L7Ys)byDEXfImf?OvH6k;5z`{PTXk}{JK#T{8xaVB~B*hI{8*&=|a?E z7^$XnAgD?dT{2NaM2_ND4JDhSWTKvuiDCjr$oxBM3=?H_R-Z+hqsMa3!5NZdD?fjds(z6bcb#L44j$4lIkfFCDLo-$h@ajyV= znK+9IKSAQ&2mCHf-O4q1j%}sH)gOV(hpAsy2V#fBC*`P*xj?qV)SWgFs2ZY91-Me8 zCIK~^sM`U4RifH}8cEd40DldGL3jz%e4w)lJsxM-cn=$|vdzsRHDcV6@P`4m+!|e) z_mibVOBW9*m;X~P(SSS02lz=BEmhzwMeQ0alBF~KS^f-veje?|6hX^rlvdid4)A?= zC<5LJ!#p`zT3do{Mml`tW!~~rAV|&dN#K_#h5+a1r^7!RLm+2MfN)g+PykVCS@ZJ@ zTUL)~Cm>8tI^b0vh$p$BT|``d6jwgR3E?X97Vb716oXoTZ@O#F{I59IWgqh2VwqSHB3y*cUyNK*7z8HvACX2~c@ zR}1kQkWgw~W_BR%=rBkSTM!7-;~=S;71pF%LPd?Kt^>m+{6{sG9;w_q8QIuWNJDAI znYzU|)8sHePae$y51AUGsF(eMU_r5L34%!)2Aa?!=2XwF{&=a5e$Qnd1zneU6#qS! z`LGBfGK%5`8)T)N$;)T3C2oOeC z!FW^+$@j03mAWDh3UXK06>0}XyiFV$RNh2YRL~`7BC2%FtJXEICM;b>g`pZytfN{R zl$}~L#sD`=*Q)+Sn{1-Ot*ZfAL!-vjhsDk;{3B71(4u2drWRH%QDlvwTyau<;mg`lvFCHU3CLstAY892Q#4Vy*&A z2m;9Lz_=Hn(G;U=jVddqYpfCT^KHo0$tuxT9|U)sS)o`nrUYDMDZudK!&Q_I(>cW3 zeO*mtyt10aiaFZyOspaR;p@_u)updQxBI2>Dp3}%Qn?r>B)UAzXy~$_O{_4r9OP@> zRCR!>j1(O88gX{0P%&mGHGAu6r0`E4yMeOO9$Y(yGIC3mYFVP@*g%)M`dI zKE7^zva0bZ2~{8~?)-eLxRwTT%iIYPTox$7#N~?A=<-N06}ocgiPL?!R-lh6TH&yk z4=P%Vp}eqX91>OBDx*F}9jtC1$gU=2r?DKD$?DD?Hf$M&DFEH7E?FaV$*NOcSFzEk zSAt@!6!MK48^$*(uFIGvaReWoDF#5J6-U%UcJJ8ciz+{x)-f^d0)f6V7>dY3Rggh! zeG0qgafRZcYi1W3;}a83H$k$vP>THmDHK8tZCYYD5KRclG4(Y`E2yxU$P)UQ~aAED# zZJyNb{|iHduv`od3sKNoDBTW;XIWKHQi2?;E({bad)NavYJe(gn8WqM)y7L3#?`EH z{fEcq;5e$D7(p1V2&878Umvwl`!rI}twW=39io8(G*r-k=z)ULol4i(Sly56sjfdv z(fBZ(f%L(^x*1`EIl(}+GinH8wKL*_!Ghuvv_2EfYNG4bqDn6?Pm!WLoqaG$eo$tkd2G-G#iU_94rpoU-g0yW#*c8<6n0QkI<(i`4R6)VipL)gV0TV0N zLv;N{XwFcB8g8=0dSYRPpKT(zMzDYXR^os_YeCO5$E48lvq;ET>wN za$UG9bm7j&CrbF&@liX4VR^jS8ElGJ&{n7hrm7wqW5Wv*6mn{8)jxLiHDo~gBmg-Rtr_9TUT^RZh!>?V~|iVn%Bh#19`J)9BqnKIG$Gm zj0tN16BI4tH5rZH*pNm~RaiO-MhLR*j8`qEIUI;4V<^@R(8mL2qAVG$ySUOC#;N7o&gQ3IpVCe8zV+NkM8WBT*@4wT& zg{>0!P{sd0jE~~YRKfTtR%~K3pP<-Oh2@Z>QGKsA8kcB<6wzRWWV8t89zd)L4-e~{ zY$OY+JEl3hh=xP!x=d;OYO)xmbKO#6{(Bpx%GyQBA_~YW`MiE*2ISnfI!3xhB|YjCTaFNtteIOp*X6 z2VSZNs?i!=Rs^+0Fu9AWVjtivW)HFzOlWLsC#sex{2KzSHPz-09U9eV$Z{VRK2;@}7_Z}n>ncIc1uXzZO9k^S-EvF~+SGt>TK{T3y|9{NaZ{H`HBOmn z4m*@|{TdWw$wfzvOBj)^ahkh-mEO}n26s=3&x^2Kl!{L=oOq>*t*Ky^q|1n>%Lq>o zaK9c(VHwn?2c(YVI*OX@d|2RSSi7gU9jF&tRnG^E+3g$yt%h%^7R6+bGWCr2jt z#yB%Wo@nz!(Ie)5n;+C>Y|jIeqY0M`FS?wj zbveb(oO!N<`*LC9C1>|$MiSx2>xDcUz%qs7J5kP3*3tkW%aqFMvQV`^8q3j`_=ouv z_n|t=1N})xgxTK{LMjKpYR#{d`A@Ie2uEM^@K%X1K-LX|{SMDs=wWgVA$)WatW_5j0hk77r zX}k~RIk>Pav&+;oFxfg2QRMWeane+e;}ki^aiP#aQRuy_cgNyK_^S9=lw&(1Yin?~ zwzTj#>`;C9u2woQr@&5Fl+P^N1dZx+Pz5BeV_rrrD`sP1u|t~FX~V)kEoi-^ z8qlY)YaAU9rHU$#s1&w9MJHiF3M9jzjTsvha}$Tdai*+BM@HS_q^If20* z?~Em!tJ^_aHNmKf!8SKL2lQh_2d`uo>(By`3zoDC#N#>;m0?zK_Hki`GdKFLZ8I{g z0EHvY>8cT$VKhQBqX}sLg={YP7~QwZ7|zZ@Sui^b3PsRT;BtiN+m2In5aK|#DOkwS zUeO2$YdIPjHePdMDN`$9hnUeYwew;DfjG294$gv8kGS9v8+ucU`Fg;8nArx!SE`YW zJvU1q=FFkdU7OQJJsg)RGl%0L!Z)?Z)MockoUmu#PlctiUb4(6qkHMGqyDENtP&6_ zHmuJ_>QiNM&l=nFggoAgaTz}>TvSau)S+teJySDkdoQD#?tzCox^fq$q;wBz)x#Z` zDQNEXVsp{%Hm2^_s3xvObE?X5PHgnDXEp9q!PBaUW`cp*pk|{piXU!;n)OPCY%!^K z?(&9B9+w@s%Nw@GL=+x+jjOVt0m~H~>{FG&rd7d!CD!l6&%dHJRE8rb(qZ}~2kV23 zmX4#u=8}cPuT51@ll%~RT<_IF7L&& zKlGtugV{T~#X+%r864)LIaCf0+cIqIxUH&XsKWvd2DGv?AYGfEHsIvh#=5{dS!7X+*y_GYGsPcu`8Rff7keTlRf=SaE zE)`oeyZ5wwyid(+|4t~av)k44Z5+ch*Wmi+ZY;Y{;1;Gw|Q<^Ac=?{CE zVEWR?nFTe6OLv6}GEFhmK9wnBFLn=FOGO3QiAkR>?7H3lB6u+rvSjZi-H<9?==Etk z-F~I@#)zwSW@O36nYwt3u$M6x1S*La;379qgMK<-9F*RQU>E^VUD?xX4J+*a*~TCGq5tSh2bY*h^t7 z(RCTDEy_uY^p@z>F0PS2#ZIm{}G}Sf`qq8;z>oYAbvkYTS;7D7t26Q zV~ItyR>9a}{i|XJU8gD!EsL-WYK^4USN03JIv&yyM0^~^)SG5eI}7iV!&st|)?3oK znAW>6ws_s@+(FmK*`!&lgV7!dw4z@UQ7kbj8bI+pj3t(mVz}h7)TC$(dGtg)!s0>7 z8$t0^$)mFp;T4Zg%_AeW!EZZEtgQIr4xsLVG1Kge!QfB>V`44Q0=gA2v4V;18K4Z+ zD2h-6%s@pmHHe1xQIbs;Ez-Xx+zb1}^ctGhmL^LK&bxK>{cwHa$cEPaly`f7@4W=+ z6G!OK8DV|s;O@T_W1m>4BM(crCyqecdthRjk`+e>YBAOCiB-U!+Ml=&sB>WA#4IK6 z7ZUq;^9%nutsrAP@c;-P?oa$%pk9SBIqL$&#%@sr;C<(r!o30ptpzRC7o(gc#Mgm* zT>6K_)Id5TqY0(OtAiLHe$+vYEtS zszJq6Lvh<6ON-tM@g+ibMBMz|2UdtMgQ5;-aG!)Av-=8}HK3Zdz~;yGYmkOX1ITH;9{=wXRaR1+Bn z&k8bpAI5Cx(joz6Ixw-u89gQl?}dq>O9Z2Tsep%825qA-6^suw)jpP*aM;Dt&|Reb zSt6Fb5qm>7VdAxKFGT+$48){$1B|K~bls>tXtA&;4;@m7%YfT1NkX=k*b2n8Fs4pf zD2O*AB1r*1jQQ$-ZGnL_v_3AKA#)L<=+YEqW7PK;QMV>Cm6KQnzH3!9IyIJ94g@_6 zN)s7Mkrk_h;ARrtQ1)2)$ci2i9}Qy?>qt%8C?|2fe0UF&EeqfPjgsMEhS)=V5!rx5 zByo}43K&E{iXf>4iW41st~B12J4q*2t#@O>Dw^rD4?G98#$QAZ02!h2z2 zXta<(y;Q)1`q!UPmj175A<>X5gV?KJAR;Z62eS~xD7qpFqY#~mAfK1PqH>eC7Kj@q z(#RB2(IeLWL`Wl3ZU};#nc{`A#|@)bL$Y}ZCDX_{M0Yq0 z*wX5RQEchzQ699GgdRGivKhWZl6BG2;k$Ow#Xv^OWr=w}EPyc+$PR~iLkg0_6K6)c z2Mt7I0}_$M27ea@gC1H;1EY&~)FdjEM2l#V2rZ-0?2({S5`-lK?_~BzI>nF_<2DQz z?~|j)PFf7wKZoRVv7}b?k4XA1T8}Y|>`$ME>t*FZ>-R8dSys{hD@+=0=S(qj2Z7gN zkSDa>HT?*Dp!{f27{y%0t$r!-J(8%KmXbKCEaLtUX;iD%fFyUUoS3|{n}g=H(z}z^ zMbgr#cKb%Vmv+NrR#zN)aAQ)q`eBk$uRQ5UV#rE?LjGhtNv7}IBz2dyok2VGlX z9MLPNJ-WXDgQ7(_Zj#PLv?xb^Wij$4lBk=O64p@Gst{=?>$5sbFP0%3Py4)9GmvFfDtm~{5gdIBaXdN=N% z<4G81ROb%5cEc2KBcIzPx{DSm`e&@xlA$4k4fhk&w~q?ngh;NIady(WQ95G{bwtsn zDdJN3f+U}ZDTwo0pmxHTnQU2oz-*Mny>?hol+0B?*T5icw1z3FwL9n-4x_ZEQ%IX3 z=!-_hBr-<%F`{mhDBj{moEDixiwZ<+Mr18A!cJNfVUWQcJLs4M(_ch}pf5x;iQ*zg zMBOG)T*QcIkx5i2Vml%`NJiL6>kt@-xON8}^I-am$Pn~}h$c~7MEoZ?kSvj8owSaE zk!h#nXqf(t8G^oGY!by8M?~EwQ9SK9SqDBxNM4<^x?p6o=;(&&A3Oea4)7~sP@rg? z1S2J;<7AlrVHkqGkk}-OODsekNE#(sC#|tCQW!eM!SrX$5cCCOlPJzOBI-7Y3RTet zJ~Jh+PFnlJ$YjxR08Ibb8G^ou-6V>~9uak$M1`@t;FFiUI%!qHs0isAtUSy*O8QcX z?xIBs(#PZ00gPs?e=2ZV;Xo|)!GLTytUBeSHAXta8LVZIv;xKv$=gm4bzwbDhCT%g z)LMjC?_6(`*2@Y@OU5Vrnutf_BDeb_aTl$h!C-V*Nt@hN?&x^v!-#$%sro8u zYxc$S@Lo{-1P1!0MYcE%#k7$p!^4NxZt3iN4%Q3GNlPW8D-u_^Ho)sLMMdjo7*#^( zxU~2dzn=2VJw3hh_&M zgYP3@=Siuuh$A*Y@q`w{9H1mb5rMQgY#PFX8A-`O`so;?hh%C`6X3u;1|$5}gy1uB zK%?$PtML;U*)pfQq_61M|!2TUw4LYWPR!75&dKD-CBDiJl>K_sRI3=!k# zu=E=gh5!bZTrK=XpOMEO##zWHc9J4o-7k)1*iF#xRZ{Jpw06S873hjnf$N2dr*$qc z^I(khMTueJLS@AH#ficSLZYY*6^8x|PDa^`Q-o}cSy@!&j9`(*4N4OkPG?MHn3Qyg zHF$akGNjbW+;R}MGhmPkT03A8Vx3KUCyde!T{prQNmbz)nxrTZW{9pGQHU_<2|?=5 z#@SG`&kw{z%c$fh*o{sq&!3 zlocyR4F5?<)FlD#CM}+|jv)1MFtI#RSwwVkMyvWU@&$3H!kAk8 zs0wD-oW!NqB|(>mP-j^9@6(+BDepmp6*ns1!+YdR16i7$)j>B6`Xh4g*P4 zxI&{zjixFg`j%@ngDeVT(OBBZzYs+Jg>KVfR7E3wQC7y3n+|nnh#Vmu@%Ps+HJC>} zMl2EQ!ctO&yc`7sIcQCV>96M|V8<&WS|l=Yb!Y;Tr!sdTUJ)# zL7|btmf=V%d53s7o&m$>VUX*zC@9vET$ALUgMph%lEtO4DwH#%z--}tP|%)Cua^~PlTH~8|6gVu)(fmM&`w`?p7*4_0qYI+@cQB}JZ@_v>IcdEIqp9eo zR5)=b`s$q89h9vKMiYbrO2>U5S4=y}Gy_7YR);aPj3a39CyjC1Oe82IjY(-B!}ixo zJ`5@vqaK;ah|+o#RJ$c*xZjr%F9Go?j9Hx1;x%CY4wE!TvI$!&es9_i2@Ubtg)ux; zQB^dzda+VMU*@L~Fs|5GTO2RcMwqxq*w%oRnNFj+n~@l;qv4DwjCdksOfG{WDtQLd zoX}2(PL{*Ko7QnKN_x7^QXaH6gdTK#N_o(_E%b<_*#g{`B}q6!QNxxifZPh>Vf?jD zjN}lE;0=;?F)bA=>_&V+0S}P=VRvp>2LW+d9GSEz@!B|2h)aQ>IA-zG90qJ?{ard2(|QvI^9MouEf}Rex+u9ZD2kkk>*az zU#;YKije3u4W{Tt{0maq9Hz2mz%(lf|0pHwq(w0j;>~7@V4H zK!4Dr`UG0zVB+n&C2D~iu1FZ7@H4a@4`~wqzeN-*nzkQ;8*VTVp4M?NDus0QDi2x> z^R!2pKl7g;nl4lpKoiLc?ZP;-$9AB6CrL?{&on4I9k?CAYixMQoS#CJOmDy^M z=QhLzYv8kP8A zQ2nHkGD3!wmh~j4c1ublb|Mtgh+SB30sW4oT}+FnAb}cfb{b*w>G&%ejnR@tC#^Q= z?4dPHVQEc=QMHP$naYFKZ0YQw#T-{OCjZ;O{tneH(YgnwKheFw-mi#gJy4YBQDFZ| z5z+c{QKBc~M6VVhdI~t=jn*!S(FV~kO7aYLA?Q+pGw-FtBF0kwJbYe&!5EDeiIQU7 z1rd$)98jb(A`li43Tb9ws40Wi82lx})+Ih{H}49EEDVRiEw1bj?_r9-7|hv&DaJr} zL#ojE;iVWb#78Nf5eZg%)`sF54ufQX3KgRWyL*bc01ejBFw_+7Q>A-iSa1-H^&oH$ z!GH^`ze{J>v{>TLK>ST2!v+J3zLrK~?er?U>nfWh44Wo=<=U&l^=&rmufpSxl1C@4 z0(MO7-YH1MFP2(EPUWj8LM0f)x^q)>hKJFK8Q6%OVi`7!1Cwt+(#}<>r1f>_G$gzc zh?^wR)Fjwz53NfQF?6U)0(VP9=T-K31()RGGrq;^PO!!E91o%;(M& zqbEo58a=krV#csxF(JRLhYI&P7-*H&?a~>RO%-D^kVh*TS}T>4)`ij;78SL8LGl&S zql;EA48||^4mwnTFzMt^@@^Q2ON%iu%c3El5+VzHPLg%fs!~o`WMCvPA|EL|x@a}Q zNFnG@0b)XsdnpWrpv4%92|?bDKmkWB^rNgpii9vK zThusN;#i&R2-#mTg^AZ;XvfNCL2*<5u~Mpo8cU&u)7;=ArGsg%sZS#^JX-QvX{b@d zL&rcHmoqhJI5moP|9pNu`KA~hGLC2qDI=e_%Mc}A@IFG1c#IMB5k@f}t0*?QjACUp zgiLs65gn2~tgR#})|`LzY%w@FtOJL;VIc3xus*Jwv}iD=*tmm^3l(0cF(q~Z@zp}) zCLpefBN^i1`8cEpgEZ2562?KZZWdrqJO%9YFiCUW-4`zc^?MkzPtXm^=NBM47-R{p z?Jzt|LHj>oQX*SvTkUkd29rje$%yx&Mo1^--(;htX+aEAVIT{w{b7Rm={yHU#{Xin zF#dlbAY*jEu@nZ~0rXI6cdD@uGEIabA0;BCnGX_~VKft@Xuu?Hd)?$J)J)f|4wd$(t79+$#(fLHi0A z=#ADjFgS2Qx~C zq7KgkMZH6OmsLs|IM$zSje?ApMlwG}2I#y0A3 zytd2IcN=ULC|+{b^c8@45qv6ael0M3cVK~pa2FExF2Ji`^A!ui{3?|t<>vbtM*!w) zE|i&{3L z)4i}6?(9!>X*c)JT7qJgEKc??Cz|2$qt%4&VD!!=06VLL-PnQF2>vWp_Jh?;vTG-5| zy1?dMqoyM-%8(E*-K@Db!=LGEhuubh*it^&)cd`#8UDe*=Cb`rz}#o(gw1V&mtb@M zM(b0xON=M_lzwjdhK`rZr#e3y1G3h@rX2iqK5BPD{QR3b{91%xAgPo-xrEmfz_c!b z!IPff*7U?_{+AKJ*PtxfE=U;d92ZpT0C25^{J8K_37hLpI=^+gmI2R?>9`&K8n12o zkw-%O{-;`wdqGD%y#%`o_Im-}Don6g-kM-DK7QKP`K@jGwZSCBX^&~Xvk{K*wg>iH z*wi_vc&0qmSwc*ET!*^^{wz0~D`s8d+h2bxCpAbuasl#o6e+O{)!=`@pr|90tM+HV zuMOX}rQw6N+MnZrV&*>J`EahLdkk(q+WQ8~I|iO^ZTP~Vrq?*y?-}$xaBF#VykX=TcCR~d?%EKkAv>X@I6({cTfDZA9-r}J@M6kCVxIjx;MioOErJK zRXUJ4=OdyUh>wY<`+S&p_rdTLRW1L%gxgp8z0nQiZ{p8~Y!|^~F`nF;$$>_vV_(AU zEB)T+2J$!YTabpWFnpwVZ-$Q;)7&wDxd(W@fXp|H_hg0}`e6K5!0at<{fM&G z)1`Or>?lTx+`E+ z+yxd_P*7B4QBlDUT|rb_>xwHXe8P$@sObOqd(JuYo_RYdiMW3+YvIkjzw?~SbIy6r zbDnd3c|)rWU(65PUdZ1>^sqZGV+}vNoYkh!J79-G4=;+f^e~OsrQZe|et2i_-FWW7 zW8(??A*Me|w_)yu8!s5O{_&S^dd(eKDlgyVPMv9Xj7j5g#AQlMQEo>Bh^3 zIe&K4q4V7>PtI$d=y@~fSL69>JiIy0=I`(D9)7NK=$4Pk{@Qpa^8Z3~8}9ezZ=!f6 za<}Q*-`|(NiQ=*DtV>>0#re)(}K1$a2$ zIf#ezot*3BW98unyxaV6zLRs77EknR`6|IY<%yms=ri$H{|oTG1W$F5r~dckZ=!fy z_i(-}f2Ek$oPoz))@9vUCY;0NWAobxe)+u2JeCY#a}Jl!Ie2XR z7veqq@IDki8OI;rb9fHp;oPnb&v*BEGw{_8-KHP@wc#gn=jAx|;UziNzjfoU>(0x3 z!q1nSaA6wpM0D5vLHM)J*PZw{-@Ogb4m>vhoa46n;XCKL`S9W(&Uf=>B1^ZwHeVJ` z^gEG1>(2S_V$UMD$MINyJk4vLddGdDbgjD$Z?XNIs=Eyr{(Yf%IDa30cKr|XZ~1#6 zzPNr+?AZr*USD9t^Kt_FTB^Bw7WRPQ!im~P!~MI3jShZh3y;ZgNs&l7NWpJ#w= zJT@$UKkc~Rjdwox;Nb-T)_<}1t=r>{KQ}%5?xt(q++QB3x9R)_?|i0VU68i|Sa+_2 zat)LZ>zB_>cx*ZGookoYzjdGNFT--plMfF}a@}(U9e>hu4!6-p8&oK&)s-_M(=XxS z`YF#kasAY$&2;#1O*EWun+|Cg7NJk%KL@`D@f>!ZEAjpYJa5MHHazdd^DaC*!*Cs* z8}Qil+;Eq`-TJfiE8tcvKE}nz`U~@C@$ZX%2h!qm7oK~~!?Q9r{=4yhk3*xM2l0Fj z&%<~g#d8eLf8yamnJ4h@{EZE7)L!C#?ythXzNbDUo$kWdGpEO;kx%hJk}pKM8D9(4b#^DF}zRoa0m78!{0>lxb9q^ zzuP=qgSYAOozIW)%*2|!>&`WLK3t2p?tFg*o>$@-a@@I2{{}o);dz_m&UO1+@Z5^$ z4#%DA_z&ZG1kW+Yo$LC)A;vmBHi+cY0w|@x0S<=eqwgJaIg0 z@L2v|fpJo6!se7HA(5BDbUS%imt zl8h`EJT^T`5C2*^_b2ErhI+nmB7d(a@jT@zY$=%*`PIp@aB@nvJHvaVy1EAc zkE932hm+Zw=AqR7)b5^|bhf5HJu;FWtr<&aa@m@u?#|}U!`=&dP9DzDYfW=!Pg_q- zb2^i(X&dby9v?`0HACrrcVRkHQ$Vgs=~!z<`uc}bqyJN}XOCnjmF1q^nx5Xy_4V5u z4xgZmGC9k(=k*Wuqi_-j&lsuh=L%c@NH&#Cpctx$Jk*jy9uhJ=_m4DaYGNQc*f&0$ zOXQA>C9`nDuGB;}H;@|50ide0xsmnt2O7=*J8-~;>i(f@|Ih&5qmUebf#*--P7n1< zFXF3M$uxhtd5zUXBP9_W8dIORJTlLp9hLW4``~4<`ik})@ynv>eV0ezrquVmc(o32 z8(b88T?MFOcp1QNg#`TP%jgsppAmKJ(#SLg3H0ZI4Q@73&jG+IL;qOiL%l|aYAjJv z^_vKjTddyoYQ|I&QO|pE9B!MZo|{33@|b^eG_qouucDRytZ1YfFEw<+?@Ip0$y8Kb zF&^<_aGh1}$Ckih7d`V=;(F^$#^w79{Axs@Lp+&{2<XL352l4SZ0&!g-?Cqn(gIgw3f zQurAj9)L;~9SrpkGDNyS9Yepm+8WvwsuvnrYuDp+GY;Pdx)~jN|B%(%WuljlWG$A2 zhEv&GK2@W%g^){)BsDjM+8ynV++RGN9GAWTSHK3jL4X1*xsx8o^P8Zyxc@`lJDvI@ z5)mcpfIULHsLuf)+|DwRq<5?wJ3H^)HR&%!+< z>YG%b>g0&OAgVI*8jtRL``e*!%b_&B=Cw~dMMt<5A-LN_ebFZu{?_PhKf2nVNnPGy zIN0FhFawm+=6#WNe~CA(#0P);Q7?fmmzO8lG4QQi{`fE9PgKpNb{7A0`J+ZnKq$Ra z#7D%RrhU$iYoACcN*Ab3A*|1PEG3Tn_-Z6#uG9&^_iExs|0Mp{K(z!B`J=gt`7?;% zU6@3tv_ND2jcX!wuDDfNns#o?JD(-wa_!ufkEt-`Et>tPq#kGWMXRG~X1mtv5%urr zRG{Xsy(I3JOQF{BS2{9Ny|gXWo5*U+l>a&YEcN#Y00(qNsF)h_TghK0q8>-IwvyWY zg(xP}b5vDFrun*_Yf;bKMWh~J4CQ`pbdEo_M#OBU#93JZz8JYgirDVFzpkq*my8QK!896zdy{mVg&zmr7x>eq-EGRSD#k&7DGnKl0ODwvY6 zvC733^`k3<{jUazO_lu*81|6>Q)(0?6N$z|D@E3^CGMe(7gJ9LiA*X-;VSXfw~*E< z6qO&39*`)15kxr!zG<_9t=~eFP(Dx$QB@I_@|bR_^WT5xcoEm%f=dWa9QLc46RC!> z*^Is$XXVkWPt6k5_J^@3%y{?D)z&(}INcjPnCjPq(&KCylx$$j&?*UrrLc-TzaC1S zTSL_*=#2$p8_a2s`=`*HsP&gXm{!F7CDlGU+xdfVtC3+!q*AO4e-%PY=krAwXhK6o zjOQirk7(-s`5d*?Yop^FJ&J6xp}r}hA{fS0AmIFJA7ZBeMzs0jue4sD&r-Vs8{^xxg;>yG#t>4`0e14 zyE4^Js2ensG9#loYbR&mYJ=s9scN=hLmbCRWQ9MMBbb`w`Po@c4y(WcE%Q7#4l6Za zg_}dwM>sFW>A9G?MMg(r$9?D$aV>#seoCn(s`{YwUgFPPvbqwb3Da|?S}GH6nD*+A zlp~|DbzrD6W=aYcDvK;q8E5YEn(;n>`7v7hRHCyhB@eo6YlIO!OpCTNR23NIj#c$t z6RN_miq7&&IV6G=go2TBFJ}?fNOBc4xhm~QLFVF66B6-tl0!X7h+01Os))~lL1dLb zV|lgiAXXqf3Y2;cj0tjCAu+93%n>8v)o>QZX3Hf-nvK)l6ls;Q?zc$Pr8Gt=eSakp z?1y6#PXr*ecW)z!=IG zj_r7Z`^PcxLMxIola$^*)s$$M!eV>ivItU&sPc%2-r2TNzs>l{{j)JJL=`f(S`g2s zs_;wKz^PtJhC^qu-O;w;xRPy*hM#{1qT&u|^?DeEi0X{!oN9m8PWY+e^oDT#QTUrg zB}Ur|DhV|iRd0|PkcfK7tLA(%#z*&~*nBi83aTGH-^eu^N`;JQbOEBmCF9qi;i5cQ zld6J~DKYiFfSW1Q)@LyKq@F`zj%dTWk1C#38m>=fckx#A0ZR_gOtQ=h#*Xvh~c8h7Xxl#2@B(iRmKYzJD zcZF4JZ@bLuwWlJHa!dti^^{j=A!G6(q!DPe*b0Ak6$QeElKkBzd}yUsZRPPv6R^TB zi(nWx%RgzgkG6?XoYb??YMghN8~2MUwgNRXlY%PB%%8b5u9@74w76woy$2Fv%A|5q z@(KStqpAkVg-Yh7jg18*Po44!lsW6LK&i}OgTc5I?y$kjU8N!&5U9nJN=2ThDM;y; z(2>Xf)NiAP2G(9!6W8!+-K&?#%Cn2?d%O$8c-of!{6sC!XLK{v7T4AV17cNB9g zal(#5cTIPSro%LlWabNC`nV3G*-Fh1S(D{{b6ATtPg;v@LR37jq2A6OvE1KOM2me0 z4fXe?#U4RLO{B%xvlnWy>vdP*YO%iwxS4`{d>K)nP>a2Hs?i#0_*WzOVJ&ub5Z~{r z#eRVaeC6Fc<)PuEMt&R>oNx^Jm!aXG|IqM7t!PcS&PmbI1J&afP>U{u{n;;p@ir7$ z0^?DtDJ_9<)WdUEih5HoiNtAsqKn%I5|3i3Gc2XiP-s=~a2A%nEXfAi3=+?4sX9xs zfi{4`6I|;4;{q)OiHEw>J$aH1wFM-e^HN{PlWbCJLE_ObwK<$rIVd~~Mk5!Qk})wd zvT)e&SL1ofIuG?Q?Ehao?kXxA%jb-5=@J#UttW-y%&)hnNCAA%p#AMsXHVZ z+Tx7iwSoC(6f###%z!Fbe_;f$wJ+665VuXEF|ipq4x2|QU^91Z22@<1`Nn0~;1si4 zKs3Ks1*wI5DJE-1MuiJB7^XaYsf<&QW*oVI)OhNFf?PbQ$3Zk=UmzFpsQRAC#rq@| zabTSb;Nx@gBy&-b&&AH2Qfge_p{=Msg2fn_>JJ#0O2(efXRHAkJId<$DKbWbsho?! z4KR79ul5@-x?T?>4uh?@e>#U=Skd50B_$GDhf9)fN)%dl{x-<;iAvqB-8r(c6>=0+ z_X>B@)m@h{eZ8)9T@dHw3KB!*sQL^HA~I{+Og_gVQJ6bm9bp3#EA54ZqzJozCG7r_ zVfP`2-8cSD>^>4=ce|~iha@`n#>*lcEUQzPEA_)jM2}Ufv5!G{d(Z$TKPe7-<lIArILWqw)jaFN&ienh?F#o3R@4KG6nxwQ};k6RPM{+D1) zDC+KSfy7|nVX(hNr>OTK%DdUpf_D$wI{axoRjYn0y%w5%OnpWg4fp7+&@*9p|mPXZAF46tizbI_|bCOxGHgF#F-P|qsvZQ%kIC%rk$X17fBaxXZU?)L7WsIu{n=!2#QwOy0q3BSg zrI_8r6btI8+03!S_a?;5tuQia&g02dW*hED?TQy)licQ%T{z_Z;3Uh2|mlQk8Zl$lPOl@#Rrvyht)dy+&l=~|K z9{%xi&BViAz{Lky!6#zlAxKJbKIWZ(3$}Y6)K_0ap=p^|%1tn<^tN5r4R#N&W65#G zb69%J_>|@fUZ}@3qeEBQPrT%u&T>DgU1ywdY&eBkg%fNWc(FSr{<8JeoUsUPebrs4 zIV2Dq8Zf5>P-=QlsC2Y={U6($Dn@`={*`xYM+h)IIp2dVA#(13SX&O*_?lnMmOx@s z;LVfe#}>9M8M1OP5tBc%2Q$J$1hTP8ebP7m5RVS=99zYlh~X z4dq*o?l7X>1H&2GK>at&UUr|eRrx;5j*n1VC`^FsPGHqWe+hh5VBIvPJ_)Udbu&c3 zUh(g!a`eW%pP_fgIR@Fd_tt6b#uuwMaz7$%d|xe#E1VLLMN2Fxo~{oEP<`?y3M>SQ zM@ISgaSS5SlC_;Pzl3oO##Hn7roYrJoqQ8VRUv8b&j#@c&g$$7To6!vPOc%xDE15_q5 zB6a*B9T5ZHCade_a0T*p11wGoMcO1)WPSq6UL^$!x=PJve_O6?CQiEWo)J6UF^$ZE zhqYJ&1`n(Z!$y8ZwSWy~?+c0{O0lbtCpIdDqXmwgN2!{4rz!2ch-_b-9$iu+fd>kg zh8h0&36Oj^osm{#L@a?DWCjA~SR&~2Dz#a|Tnh1{(nX_Kmnm8AiP2zVOlWWKzf?DD zE4%bXAy(gH1@H~N`J*>dvLVRSLR7ttO;3B^hHiM!;;&%gp}*y}t$StL)-S-t|LPIy zUpmqWJ5t;Kn33@02fsF;>2>W`*c=OC=a;eFa4@FLpSgoX_PfLG{BaQuox?}@FoF(- z(=vj#$3x`oNO(AHx>p#Pp3gB0FH}+A(48WWfWg$poLf}ZeN`+@JAWBmWqM7w>L!Fb z#!xcFt9uTNpz%KjMDt;4GZGB3+8Y}iv9krJ0$uIj(!`F*_Dv2zbyD_Ml{e-SLhuN? zbu;kTV%5Yqq@Pl=IP{F+I3J@!hn-ii97$u8PuGaQC8~Y`?Ts<9x@H=N&C^2a_6l&r z9bOy*GrBkG9voR$vh#)U8ui9_-JLJbL$TylHJ4sRiM>`j1T4MI_GhfXWTro(O7}h7 zV!}&ll*~Vt|%0`)5SeKVdr%*?))!{=)1x8Sb%NN(M(d6Q4twxZ8J5kO?~l z$?+OD+ese&Rk}nBBr#JUOv+Ko3CnnEl(Xi-$3nSR>rsZle{PIF0^{Di%lIoSx#)?-MjnELk z&v__QrhB@TfQQ)Yax0@Qo~v|ony8VT9#uc`#M)74-bKXpdBn}hDeSmh1hY|UHf92lBpxoQU>?oxUHU&DGLi32f!m{& zTTb6d8-Jf{5z)nV_lenE6ICC@Scu~O0QMqA{58Oud*I{6ANmqgTMN~L9HGp|Hlu~y zn~O$`OjMVYaw=0+)UbDk*7XM<95P)+W0qSazv1SZTW8RVz|NLTa|_L6tLQG2F{L`c z!{!br&mTH*o}003XshV9%c_rWGY^|ga&zPHnG~MCR1!gMW9r}7dgftnxO}R}{7mB;+gw{?DhAffL(arWn98?CAG z>xu~|CjUyxkhSlFUp=1{Q=|5kbINCN;!*ZnbLmm3ccbPhwEr;KETD<~076UiJQ$*W zM=B6Swa|>V5I4>!ei36F-=C{Keu*uy`|-tYv>M&BSIVgmbSBuhT`PfalA#&`FNvzJ zL2cThNQW({Uk3?s?3YJyq9s1x&{7@%KNlUkWCQ{;sd+HgT^%;ouXW0IW=rD<|8y;0O8y>aH z$5<@7i<<$h*?tx(@?Mgyqf zWF+U;A{S_F$PG|;Gr)IbgkhImdH91S!cz6brE$!&zIPfMAXi`)s<*I5oDY-E7!hdq z@cd(RPRgI5-gTL5k)H*Hz;WO_tX*N^Q#~r(C=3mpb1lV092eEf6&=J1~OBG}FV<+zZ&k*^j)8DOaty`xsD>}1uoP@f9w z^YKfC87PZ8;TM_$;b7}L4J{=-Javz{T1HAxbSW zxq?K0A&JUCm#F$YNHO(u=Cop-^efa^SPY{@YqoAz!w05Uk=12bU@F(Q+~_4uyR;en zo*Y&2h+nQ=iQPIV3B2n?Qru&JzK;{Ew;)Mm0S8j33VDtmraZbWtnrEp=KdAl_o#kW zP`#KCKq1ZpFW3=9eT)q}Z-b8t_0*;4yP^u_E~2SJ{o{5Udf-B=*NQs=@nZWf3!V>Kus45rZyuLMUo z%B~pb_^A3fxarZrN_8U`#Z2!b!WaYtK0hwJqsHJB)5lWd*GP@yU<&*#Mj9vA`HPn# z4K2F&F@z(!+Wr-p(DpAPTNzci%FuF#dQcw@0q<}z=EZuhDq``tp2ZA_z`2HvoXo2Wt)WgMdQAc7S(C$&?<$BsCR?d@0htW>f`ejxA z^hzlxEN^q+qf~}$G*4(x;o2W*m|{U5mpjrG_O(&;)ncT8qfy#&g8P2Gt$#YI8q+?M zD6SyL89rxcg+HSniz_M@k87z~0FHTUsQRUF4B?oAWtg?q{wZo6*Su9Lu2Vo-<>hGp zEBP{-lwEewyrpbu{`k@%OTCe4I zXz);0Xy%?r$;=d$35f|kZSH(tT9%yT`Xo3(BeOS51?SH&H6W*O^mP>+KNxF^`Yfh? z%b^aN63*l+DmADp6gFR0q9&zu-YTVIwWKSGs6A;jZ@8qooy`#EzzpguQHT=n!T>&3$u!0bH_rju*=D?El1Y1^+;F=8B2CZc`phMB)=TeFP1j;P!@}wcM)@H1T==y3YIbQa$+$8coR^B7Yi|Q{y=rhPr!bhe(%PF zVc@iiPVD+^jx@8sh25sS&%D@XGU0X(=K#U~_L~^J!QsB_kTJ-e4UOnJkF%!PVBAJh zc*z#(OdJy9xsLQ)5KCMla*^RZl7p%N`1B}$e)zL=qupnNq&O?DA`0N^{ z%9%kL_GQ?NiKUZUVOW%@2TNd~N7XG7DV#9^gU_!b+88#wFeN%l@P8|b)aD~tpx%e@ zhL4%*I|wif7r?=U!|zV>Tf;pEz%>*@`SrO^eSv!OtHGhp!x{;TG|L8UmgV}T__UL) zT(80{-s($;0dhduk1Xd+2W)M+S)z1w3X%2xOlh%8FiK`aW@g#(RQO9z$E(@p6U^}ZIao@?=Wl4l{ON8L^t_S2Vc0?w8(={n zf;!GyW3Z6I-A5KTA&(w_^(0-(Kq`$_*aYFLQz%;eh=5FR!;tVH?zj*wf|bOwl~{sG z<{dDUUpFmkMO;MgJ6hF6xctAMQg(GwsZvAgn>jFC-zoZl4zPV13xYo(!EKUMY&DHY z@@$i&c4+rwwQ+wY&bC*EQvcNp2Vc`Th^d2_tB()-X6m7Qc15Y4&Qxlk?^Z{v3Rr-) z-DP7~kD^xIvKQO5CbX`muvxZXlkSU^`QokV4}bf$o5|45)d|axhf(dWWN#=li5b)5 ztTohf(9+O?f=gAS0x+aOdx8}3YN9L$EcP&pcGkulzwV$Ol`#jTe~ym>gR|5ar`vEj z0%uC*sEede!VA{Le6F)Dl(U^hW=u-e1(=?y#jXO}RJc%$#q)2C*xNXpr~77L2acI7 z>_lG@#qJk0=5qFlFc)!Is*kgN%P}L8k>s_Ha%q|EfmJ3=`v(r!mEd(VSlmBpUT2Y4 z^mhwug{xJK#kj&$6ng!I%*+AuQ)`SLSQSwmmJqhgnFeYpei}pOZ_%mFP$T4p{UCNi z##pBR(*;{J3$#R_FpHEa^0tP>9~l>|wL;chUJjl82knvnfU^sKnyO?9>Ui>ZL{I6E zMX{6hy=|Qb>n1*zQ7p*zuD0E{%-l5-WF0AQLO_=trv<}f)<6r2N_6%LidGCh0T66Q z1fg)GD|*s73y6lHO&3fPL3iTLY7GyJiOFJapB^(8XUp|(VN-`HUW^;9bwz(IfquZK zZitim=hBj1&NmKsFrROnpgy=hu~aA_Yr0Ot&l-{?0}c>4VQYS&Y@oX_H?8J2$}8i+ za=|F>gA9CG-`a~NbQbfFj}9S{G`pyz1$KN_1R2 zD9OMa6RuOKMkb|DwXjfUsaHyJ3$ea5U0=rrZsY#CeeI3@S!$^sNzt%7SC!@bxy$`? zu~uJ&zgVQ=2p)0FL|70w3ghJ?5Q zyir7B4(v>zibzaPO_n1CT>81J(qB{UpC0$;p5@QF5YO$hZ48Es{CJY$epL)s zc?L6%WaZw$GK`-YJ!xCuEP%jL|q(@By#%k*r-Ux@&ndd5I8b`XPaGEx%v)XSuZHQg%wN00 zU%C$N_yN5hgL4MQW?Zai6bw`Hb4zm!gXOYOb%E3?;zG@DtoG|5#Aqp%`LO)7m3m$> zhXp*;kUg)FFOIKp0K;sUHSD&6f;h_LJW*Q$N)-s$$G9$~+sIJLD)}N}A5I zMf^dL9p~>gT8&i8jV!^U2^Tk@4z%;vAz;dDnpn&x{#X-MXEM3*vEnBn{$%B@Ka;DQ zq|AlqPq^&NkIbp7P{NS~TDNht^%gqvjyyS4RCuCJmaKT%m!G4PNoE`;L;YY``V^QW zC#^&`FeAE=i{wW>Azjl9?lY-@KA%=bS&bv?6%6}0Q{}-mM)@sKAMcq`z0y`Z;T;;q zj7e>O{;5{?g!2ZDv2VxqVk4snFvbr)Z88}lfSEwO?+oARBLoU+x>~kxA`d zaPkKoLjSQ&luds4S*%5y;o*c$gJJBH+Jv5*_A{ZMaN@yd?Y#ZzzRbWR)o|gMGC%!o z290QTNC#s=JuH5r5zk@{e_aXV^SoMCY`UUvvRPsh*kA1E_r$4#($7iSIj z1I+PHuH?{9J_=?p-RIW8n36kx^c75#`pixE$|awNWhEx=N|l`(KEB4NkGbGP9i6yu zbP6B5(YFD>59Pef+_n||7C6|eBlV^adgMs^f9{v} zM(Gp|oQQee@}w|i;Pm~giNp`-k;)jD)nd)l*lNid$ z9&f`hzu3p!@cfyrH~AsOd4!l_;18K@Fty$>L7Fm*bo_5@j|}3@Ety;5N0(>LrZzYS zN_YYrd6J!Z>%QK$sje`*;P+C0N2{T@rkb>YN!w&wOLNmMVZFYt7}Ib(kPi4%uY7?QL5S!Pv+;$0 zv;)@5#7mWoS};^#+E@NT(pHl8O@ENIOC;^D|3T9BCGB7TLDG)O4d$ylCXlsL^Rgzk zpIpzUWsOp%MB+lSCI?CK9#tcyu235P|M4*?nV$};6?cE|N|~!BDudI5GLW`~TWyf1 zVoh$Lv@qSCif8hV(Z{3;iaT#{706l?)Uzz8@0q!KqnGCKx<6hZfNNOF8bw`kqk*cD zwMg8;!6k;7oe22-!hpCr$wwIMLsa-~ZnWMF$`2RvY61jZ5(LGpd`xvw7m^lL>$tg3 zsb0C7p@w&$#Q4EEl2vWH%x>&5u&y1)|6;1sz0`L&~}Z^ zj+D#=+N06BNC`GSs9ue#A|=>vsCqQ2iIm`M1wN=DymaYXPI?XBr?;2P197h=o)#&Y z4|JYJr$ARv}K`VdDkM zQk>VqIcV$f4LGnS&3B?^;9Dg;6&h1=H8QK~n{eRG)gQ1yz6A%?=TYnMZ8)$-k&5II zpF5E{MHGU+4a{0a41%$dR_YZI2*v`u)T~Qq-MVzvF3zcnf3z5>-+wlknPv^&VldYI zrH*el7?(IpEyr~55naphtwsB*V4wB;5##+qcxO%Xpf;=QtH7*lH#yd}n;h%gCiivt zXN`Z%g#R)y>l~kgm-rq4X04;h1>+o~+#7U*VTcvaEVAa`cey??dn?8i${WGosm%wx z;EM31$6vpP(ficbrY3j1g?6D{ZVINsX)5YPA=rro!Nx<3c*KxQW>R9+VyK{3tYn!< zNZf`9XHPWdFf}H>(<9dl^WHUSV!Xx$N>i)qdUaa0Iu(bYxPwrwlf~c%k&yB}Kjk`e zOPZ4&rr2KWoX4NXdZ(QRD5z5za!-Bk*NBC#YdvT&>yD_iLYaYs=P!cty^6wcw@d=Z zc(w!!;n<8X7M$2|IAbONYj^y`4n_V{id!|V<^q`GpTZvql(=F6Z;Re%l)sLTC!?{j zcse%h&1^Ug(}{Xp*b<$!+CK%iBw)r9SF2#lCpXJstBJnDD2~H0@-=mCKEZy> zlW{Bvw`!cc)L*@l^N#o`C9g{1hu7h@rqiz~aKnuB>9~O5bllb8uU+TkOGEU%CW_l0 zPQ%73`7SOdX)Cz{4+k5Tt?*A@c8))99o+Qi6}W4UC-m0fD+T;SJ9lo%5p8hEcP!bk zAdykCXHcemi8=La@1nrw&A;NgLhJ{|!GgFGBX+;y)1OS`Bz&WxI1`v%MUrxIsor*@ z{1}MB@ydGr)lSR-<0@0!aE2|4_+Dx|O0%*YE4Q5D$L7}Z27fy)P+9KR*ZA1NiXFKs zx)ys|aWDf*UNJO%Z2FnQ4YswiVpeA$QstV zxP9&1r98w%uSEQkt)3CDlX+@<1_>pFn{ad)u8jH=If2h6X$= z8P_FxySFErcXuWT&PcR%zjV~w(XAn<~AjA zUQ1_F9j6~_U_!%rP1EKE!*{5Pg7f+ej6Oi_Gdhs*!E0%e9UX^ zYHQxr>$UYXb$k2!vPn<@y1I4pCa4T(4eWM z8U9#)EjyZ#)#3E$fw=rZIeFWb%H}CFlK>_9FwhwR0{83l z<=d4U9q2JR*}4tfxfN+jK3_bhoSU{GXS(XStIFFtp54|rKAg)ZU0<7MwtP2l)#8SD z>#cxn)IKvw%tjpq=im=PtPo0mx9{2$Z>~EdURPUPUtJfkudT1GtE*ifUzm#Rb28nf{^85W|CKB&$YF9~_HU9f()$&c>?-(<7-|)nKM?Bw00< zmY{I#8%9-C4UUiYvl_CSnZTfhRarSb<-MuErdA+*}g~3R;$ZcvgmGfHnHB%say4p;T zHcI>s6ul!2NTVSyY)x(5ko*>k4_ggqc1?@x?4;}=DwDynyvA_E#%Lz^?e?-msCa25 zx%AlX!LCfYzbDrRWzyE%O5qHv9?=FUYj01_*2E5@L{LB`S3O6vy{Z0-@)8VPW&{{& zv8w^i+sJI77F)IFz@bGuxPoqULG$@iQf|`<+Ucgm*6yA83PE9+3W2U{>1NZ|l;@@A zyk?wCK0X-}Su+dPP2Oa#t!b-7WT5l`ooQ5II-}!4xtk~`0LB|LUJD)6*;OZh>*eoy zlx=dfZ~t&Ik;+^Q^}466r9fwx3Ut}9LVMn}?mDl#wWqcFyw;Z5{9E8wpLbiIe{0CU zZ9vioAn@C|*bkv6(ah(SwN1IT6J)l7IxnwI`=Jqxnz21dk0|JEdv-PVw(Z_k>)E%u z{9CMf~;P6;B<8^eg6fHMSzi6Z(keDWugHA`* zsKTw-pzMvzGYwNk5Q4;xzR`ir8hrJ_X9`&o% z$sIj=*n&mI3bd~*BU_QkLTNZHdRx~X9U4u$E-$Up#|@;%As7WBXbR?~dzxAxWtvI$ zn<)!6{tnalO*L%m=+)k}M#)PI67gF0_Uf!6+}^&6l6%Iq*SzTxaN6A4 z-C^XD3fUEZV-CTobI-oS?(@2JBvQtvrcnc?w52Yrh_cp=f)w9Irs!+}CbT-vT%1Ub zrH0c7&;v}k4s4cI83LDcb$2#*9dC?j3e?M&G4 z5D2l6UAMH2&{9&QGq+7^y|<^itHs2i#m5u|3!pYogb)?m2D+Nins-pMZR@o5%!I?~ zrmoKF{-M^ojP_rXFd}Mzny}co^y{pH{2xJPqzvivmDtfRG)JC1~0w?rQ zl7;leyZ2v+F1?LCUr$S7&#tyzZM`1)?SmeB;bbP0&UnquZC%~dDH*<4Fk7#fkmngM-j8NIo-`gH_$p(Y$SYH_gv*j+)!hA9q2oHK>!cybumbr$!8o&9H!>Qqv>2~@Cf{+M#si;`AU#vb!@1g^f(#vxV*{w zf+^kxl|R-83(!knygiBiyDT)6x-LDu)El0g)1!l_1D;MM=+HLiyRkF!Z=3ROXQG@l z11#sn)^vKs~2 znrfxMY|W(muvnkXZDUEcfeECaV-2~r0q3?+s1itI3+!4H1nOK8=%&R;#8^>1X%sJ{ zsNNgN9$+LbiSE{>mh(wUjcN+KxRrpCPGiW{hU`qJ#W1%s(MQf}Wd2ViN9GUvy{#~! zP_()-3gixUBCQA!XIDtUKdY5YQoOQc$i8#1!p;Y6eS8eqDgG1no;>A^uL0;D#a z8ofxwL!?j(h`iRza(GKB%l_;q29>irT^tJq;CBDyzYXq4fIj_Lwq zaj_9dz!)5j5t}8A;b3h7sr+71KCPo7K)u*yfLiH+h(XK^0_cW*vM7PdOASLOB}S4M zgdIUC_6`lSU>pk7(KI~VR;c+PnqA{)G5NA~QZ<2gVIbUwRK#EU;=PzGNZQmS^5eNn zP-7a#kl^Uxk>jJ)yikQ9KFx-Wbs#-T7fFdJOPiLYUdmiJ&CAhv$bwNA(iYirbm4`= zu)H7);LE$JI?E>%b1VReNK3i$K1fHYdO2ND5Ka`@-t_qJz*dn*n#j2mibxDl{p4K{@6!&~Lg%=)&2HWHzT&33-Pb>cYto^$m19tNRGr7WRDUozk;1WQg5BeCw^i4J zR2?QlFtYYqY(bn@HN zJ;TZ5SdT_jb4NseL|;+p;UHmkZ4PKHXS1902BiF$9+Cz-2XoQ^=U^?2ruq{UP%Q{d zzfih#k{no!OZrw~M7hv+fsFQvRA)?ep*p~Y zl5R$8K$nq7n1YbWjzgS4_WFYZx{^<9Ll&wS1outz)Ab&p(Kk70jNS@V&xJ;>GVYiXmsRYYA}szPC~YnftIBP7^vNevE? z^}J2Oz6?zSc{9z@BtZq|yh_z*+>$P^z@!1?C@3ODeAE6ym7pD2hRI=J0mUtK1_ElZ zCGA!qYqke$p$cS`2PzU`pBPMK(9@Z^=uWanh1uF>>a&eq;TVQdxHTNTBf7ahXxdS> zWmWNXavr{g_Ecm->WuR=yjPZ+d_;tKda3igD04$g!X<@}_}-eG;+Q z4eWgTjubMJ9<#il9jC{1j*)qCslmYT5SgQ9W>m&L(8b~A3(Z*P3Z9bMIuDYD$pWfTNBc4dpujQ+)v(D+U_~Hv z0A2PGF9A76WnpFE5N=~gkL))=0keX$)`1ImebB9v2uS%4cWdnPzeLNHw+liDLvW3NL`jAJ;77;t_@QWkYA2@GK_0fa25=}~OmN2j9m*bhyW zVyf8#Tphv4W%wf9SD;hDC>OmzHqn0=3R|v)M&Ur5kf{t*6%^e?9^1iKYJjDOwvN*4 zKbR9PM)tEvP%ySgM)qfhlcN?5jv{WZ{T=ivH$4L!zCUgp(5tszq znV0C;-Mll=i#W(B#Dh?=fY7b4>|Uv4xf#zQBUBAg0%Tv>4g50U z#PG(3J}p}iI+Yl-QZj z{poZLE5-v*?qeBr&;2>Ct7T{F`Q6~YrDv-**oT(Ka$<5iO0!?Ub;1xhn;gRuNrA_U z(Bq;oS*^!KVG`3(db_)NB~AQd*2zgrPg+bxPYRE)4hmwSflW&n1Lz#>B$E?n4nsdo zg1Ej@S>{BtG=M4p?%iGWFt7J^chzHc7DIllGq0~N2m$ptIzGZss7GB$5;bejjkz$> z2&JQvH0pF3bx6YvqT7TMrx>+{ElkIfEi7!hL8z?B3WW)4OLc`*Dbd6MHX9KTVU(?m ze(MYUnwW&DnbaxPA;qcGN&YHnGn7HLWx^zv8G!b{{m@kQItz|OZ{vW+0}pJe8BVi7 zjW8g)7g{@HntzhntcM=Vyh6v&FNRD-!(zavLl*FYdQ0>rv1=lMIi%DBUSwsO!7=w?Wp^v1pSv zHU}p45`)mKs1B+R`q=<7rhSrqV?-P2nO2;rV>q1AE&0I^6$RRV5h@*e3w@Lu3n~>Z z7(Zpue;pG4gXofJ$!e~dY@S6#dS}l?9lD}K!Z?pVuk-x3-cboj(?%73qud8pk3!>RIj$Q;^ z5CsHLqTte_=q$ilT`eZnsdzakPq2*85gx!Gi8Qpw7A~Y845BVOcejAUrfpcx#kS)PXh@7(@_D8wRB;!+Yt*%;XGhnbj*i5x z-TYzrdSsqHQ0GL*?kJ^I+%{=9WVawNgYM}PAE;D@LLLz)H7d3ji$iM8tS)GA;7(VT z3)j$XZ$^s_+;fSMu^>au+zBHQj4(et#2q>8;r@s&tc;-U@fUTEzo>isMcv~s>K=bl z_xQX0cy?QM@7?9??mfQ?(K9KO2Y%Kg??Jil-Mv$2x+K=4Bv=n9%Z95 z#;odU)2nk@GQ6!l-IQe*O){onq9`m#9W8^cXtF`d*fX`Y!|U4Hoi7;FA}8@!lby7N z5L#CgQq`&GmB=8D9a!_GH*zf<@}WLh`RT#jAsAg88eo=sZyOeg+PDd^JBeY0^{1~+ zOl12oIBoeR|M{13=l|VHK9K|*{SRTZ3&#1Cg6pN>B7(U}3!@vyb&$%8(OcXUT0QLwW!lY`_InX^fYwV`KGID;hF zfU(7u8Lw-b{7~_Z99fz#fYWK16u>lxRgBciE)TJn{@iAlu zF2f$XdSqa$1Ql*f&iaOTW0#;ii+aNPr4^8^(YO2LFXz&0)cPL7e63@esbFQ9J2|#p zAZy6mF6geeFLmt1$Gn!U+dG@}9xQI?XlrRkmvv%Mb6Lq>G$gF>oJ~JBrAQu8i-8#G z`BCl~Zx&PB615qV7)Wy}L6;}^S}zmtW|xAT3l|5e6OilJjbSql|H2%>-!t3*X#A=A zAOK;Z2YqFL8w7FaG$n~`y*;piB$_(8yP>{b|C|}bs#nT4ZelTRbfc2UoydCj!b-sg zDE`!Y<}iE^k3Re#V24}bMX;0Zq@^UN8ZEK@rAmv5IdTk<@9uS?Rnuw?34 z!!3FTiEy`33J3Q^5H-}fkM;`l*^`YtS;=Ufj>h=Q*O(;Z)R&=?E!?o-M%^tEfmkI{G;YbN>qGLw8#r7?UD_?1 z#Jk*aAyi2hTog!ckR2h5TsTrJ*eOXJ^@e83-$d=frc+Rb@Ly0GWL(E>guLk1`j=Z zySO$?7JImT6-$@s^zlbn%9ns1I3kebFDMza^qVK}>~@iVxLc8DMS055^NvO2B2!>F z>g_$1$2iK0e?u>|bR00nzp*^#kyZSQcyazH-kQpP(=EnzV*Iz#VoBC}TOQ+^9r(9k zz|wI~OaaC87J=b!R;Lrd^ad2<4QH|5O@;qnlU zg>Xd(&kN!Cri8U~iTCtxb&hm-vf#+^GbI*lj(+Lj`A&=sPyY?G0{_c_Hvre-cN%iQ z_+RGG>3_h%>w%NNar|2US2%R~ztX|mfnN{&UdR8;lY{i>KkncQfGdHoa{OQ6(CPn1 z2mb@`hk-|pf6^D662$)y@Iv6XMFM`WogL66J_nb>--m%)OnfYl#~k`%(4T7X; z>6c0QKjF~n{}~4}{%0Lb`h9Z>(|^{%ry~C6fJ@PIZ22CG75euo0?ha}Ihg6~a4_k2 zJ2;8>?_K4^Daw}r#~nKT?^_VW&-ku$@TCZUJ@7iXTmJ5H=nQ|%w14^_zK!Y0zcTN7 zgePW8WcvRCd^a$D1Pi~8e`4X+g#Q9O-iz{g{Sylr@%;pZ&xPp3md^O@b8rkQiEb^iXY|fq!Bhf2rgr=Fq4Xk2tV%NeeiGp<;x=)@H+(h4hLUc#DA|tXL^r0_;p46KjqN>2K1+a z*%Pt-v3;Vy)Ey)62SJYjFL3A!9Qr3fUkE&4=uCgup)MW4@jdQf(n}W==4WOI&ko_ZgBiZj z!Hj>4gGt}vVA4ArO!`$0CjDv$lm1=@lYX6pNx#*>q~GRX((iRJ>Gy^3qapl62>;r_ zjPHuYh2?dngGqmzgPGp74krCZ2a|rQgGqldgdcbMi!R6`F~!#QPfs}XBnZT79eOMg z$p0wl#MCbge*}0PFq%(3K4L)p)9tO=!Ec0rVs<+We+}@0e0$aX)slSsEcHGJy8UO{ z*CS5;ZU_CFz_k3WK77of-v{~+LiDE`I?L~w5Pmj^7D8I|2l+ct_k9! z|8*hU8^ZU8@KYgt^d*Jy-4Vj~hww26v%G4Z_JgsWU>1OFzndI7{h#UJ5)82)+wGO= z_NKfAz>fo?+e3Ig-g^QV5dSoO#=+HQh)$|)f6qAlZ7M?lwL@q5#nuM#6K`^GYmx9b zIduBJJA@wz;m1OF!KsDmb%gL0A^cznmz-7@z9ob&3E>Arxb*ZOd~cEb#T`29YlnlG z-wp?po^vqihaF7%dmT*rbs>B-gl`Su+d}vb2Q&UgN1hTz@^e?n|6>k5P{jY^4t)&t z9|6A>=~(?eye=reOF$n19&_lAI`lVz{vF^I4&AE?!oL&r2ry?>Z2Sux`g=fM2)x9h zH#qbUgT4W{(V>qy^pAsnF|eEdJ01GhK)(if89GRt{%zF(e?J8McHmlv{#Z>wXMNVz z7UC@-jB6$G?U(*zb%prR5PmF#pLH=nPluVA6*jO!}1$CVlqV0e_?~aWLsyoc6X6>EA>Q`LOz4F#OTJY9jrWAv&@A z)Am+PAGf`kA?722WN`pr}W$)KFUKGF!{3iS>Vtw2Yn$hqp|e3 zL+=Nn64OFr~a96n=|C|7(PA3WeX{(BBDqTPS?5Lw`5udqUw~ z=Fr~<`pZM%haCD*&{M!LL-P3_cIYg(F$YtguXHf!SB3CV2h)GABd;GudcJ?3D>?`;X{oAeR~e+=>az?$lONGyFaWGyKaOOnT12q#t%L z={Gr;^xGUv`r{5J{Rsz??llGce+v9Q2|C-e<+sG4)BjorGrn~pyv4!7UtgdqozQjqt7x5Fbzp?yX;m}_RI-fsN4;INUzfx_} zWBU9+wTt;JYKvJOV-BW&es9{+Z!Qu)ziDjg^v~}ZyO>`mb}_#|>|%a0*y4{BiJu>5 zwe(Meemn3oIN1J{-&VDB^1rVwz~qM?Nwsuh+%S>H^ncXpukJ(q#P`CT_Rgb4_~8dE zZF~%Whl3eEKRs#bKPnQQ-vzXEhF{PgV20;M{#<&?&H(?cNPPTUjqCps2Qxl?QpTkp zbuj7t?2Ajk-@&Bw(=0Ci83&Wj&!@QbB^?1Kou4>y=~pF7b`6OZISZg{p^S+HpYzIYOK1ID7s79I+FN;%@JAj0l||^cI`q?v z&~J0-O#gld7wZombm;W|sDn2ZiT{{GZz@9fb_MxwFG6o|=uGbs2cKWW|Dz6le-Zi< z4xRBm<6x~F46I=0RBhd)o70chJJMfUqrHk&yq_0q;Tl*8g7v zzuqaIzXg68>r>>9;U5Hc)B6^1<>^8Gp9s-^0sOs}c%|IAMt=FJwQsJ~<;nP#0Y8WG zvgKP1%=H2k19rbDZwIj5e>WR}&jT*Qd<(~0jISTqpu&;8uC$Z;`=+`pF`f6f6C{VfpJw;zP*0~*p2TP@S9+tS^k~`e%i>_+-b`D z4e`K`p3P|m*&BmOyFnYSAF2gqOCq+bWj^|2j7NB&4hd$Ift0$Y7M+nerT zZ>0AD)VGy~Hv!Lyd!?^5@qGmNd86N${-=Ol{yz^qg8q{Ff%LD0=#PZ(e*kwn?fvJ# zb+v(hnt=}Kd6e%}CcZVmZhAGqm5`5(hQ1Ma1mm?1gPVan5q^uodx1Isav0^u@=5`F zS486Yi}of+fPYZqmA0Gk5p3CY=`%t&2K;Ww1Js119|t}Y^_MYtXUKnV2)`nPhk#k% zl&?8ZBU#{^F<(OYVg4=$E`$6sJ<98cfLpO%Xv_B=V3*(ffnNjuZ2LY2{FtK;e+K*` z$h&QiR5~9)dsu7In+xp5wSVTzwtYOzv%Q|w}isq1N<7ugVh&b2X^`Y_Yi&@cq!7` zW%zjt*t;CUhks1(H^6s7-*9}!@}Gj!=l;j0ACDwvF-Ioz&_fqjlTpLFGGJ|%VQ0&8($6Z?a-IDe9py7r(^G)4}7(; zuUYJaz?B$(HyZv90o(nKvr&In0=x15HSn9z9;`gQ7dWsd%DfwaThYH?Yw~|9@Cf=d zTb>^WcH_Gncp>nejf5~_2mbF7eYSR%<2E0kpC}-@Yh55-$NLC-Mt@SzYoMn;yVR` zQ5n_ko$)UL-t5TZT40yIdSF){w*uon=zM%H1^xiSmzwxr2K=crgZ7yQcH=t&%=uW> z2ixbHL-cn7-|p1!^}smtR{Efc|0BTHK>n)@{x)zY`fIEIo&dht;paJEH~nciAo4uQ z!KRz5m_zlZeWCjK7ai(%i}`n~|zjc)+h?Y}Pt{s8|LSjV>Z6u{4wAk;e5g!Xke_L&xgW42>d4KpD`2vH-Sg6U*WLv|3t|D&w=ef z+WWr({sHzsK&&PHnP~V=I^&&1z@KvLsT$zdLq4tlCej^wJ|CF#Gglx!)@Ks!tm?}5C&*M!Gj@NUV;Cy3KCqYA+XMNIoA|Jo z*n1xR36d4MLc>2F_GE{_*b7_`|H2Sn5yI<$xgV7MHSMd-z_TEaF#9C_Z6W`=f#2%r z+Y3VU0pK&8_I@Zte+}@>ZvPGZZq&EU&)uT?YY;)A6bn2p?^X%mi(6kyY;mM7{iV@{=y!b?yV2eHv>P7`o9fy z)?Y3}zYI9&KTh)A3e5Sa68LBNqbv&2y8(C>;up9r` zz|TAK)&krF`L*fw0tfXO^$vu>WACtcGvvv(hpT`$W4sC{Zz#oA= zVSO?`-vtiFtEH?9Zw}J8@%gCOUzOjl0T{l6`b zKH#52KUjJ?6#g*qLdZ9Mgug5K;>h#+fR~;P`@z^p9|Qg@{2w;{KMQ;@#@G9d{{Ic| zYLt()H%f5e(dGB#5MBW6>gzh-3gqYWCjK2E|J}g$A9eQm#1Y&6BHj@2E+ZeaQGS;I z{|NGA_3yiZ-Tv=Gz#l;Su=dHPfuBTu+VXn@cn{ix&F`1sajBzEPKp=iuL9VI{)Jdd zd9DEdCgQi{UjyvU4{iZ=%V!}J%}p5JZ}D`0))u1o0Ds5nuPzAD2SRu>gbxG19OHef zk8TDI<`16!?I(b_ey#OSnRh>Mg)^T18StHsJkO~__g-{;a+8RbcL4u;ueTV3(htgz(RS_rM;BoA|tg@`;gbb=q4s;H|=)ySVWP7f|<& zR`-wL=F$r~>elnpA71vE9vQFOhzma9R*%b*2KJ2di#u&CT|?R}%ew;y5AlMi0r6H} zi|=LO!oa5f{^VG$7ja~9@qgFOx&~a|jZXl!na`qh9m&;g(oWVBZ!8m+&sz2@EsDdVD`T>oG~VmXtkrw04_an&tyy(v9veRU`MleoNTU~(}sETTe| z&DD#zjxLRBHn(@|-ilk|64~+niMB-Ig1QY%koPiamIrjn)(Jwz@Lqf}vVk{0;Ikao z8y(lznlQN230LvRH#ZVFQ|7qcQ~Ycw;%8i{5kCIE#;zTR#P|J=GiPisy4HGqX6DR!{;zXp@_o)ugTk5(nM}gmdL41G3nk#iP0FaXiw3=OBCUy2mkMGkYA?9qBy!c4(Lst&8BF>O@kh!YOMwyt{5nwDY*0@?1 zNKVE1L=9U4n(pTr0E^#o`jI#D>^m=IH3u6 z(78t7JebTzHyqYvzjy#=6X4Y5kaXhg%HTZ!!<(FxD(#D&p05isCQjc2!~??zh+4;- z=9HA6wl`_gP@Gd>v69M5rmS+kMHU=_F#&W>07r_|oWL61*7gVhPnhmO@A_I>+OiS} z4Jx3wXhQR?K=ndWfMYd!d!@~yFc9uVjsOuW*;T|7g~ip&0PH(?-lZOTtw}=&2+kGp zUwfU%sZ<~y^-Nob1-Ex;C&$RHJ&JC&j?m^Iq9!y{)p2H_{>6V;iNZsAOPSVfS?92emaqI6A>v{BEXW2xM_U? zSUMb6DE)}pg;}9JQKT;dz@)p_zMQZuV&_6KaCjN*6w4D<60#6XL0SfX0FonPSSo9y zzVRBSUr=}P3ss#hHKE9i!bW%mHpxtoNtiAK(AZgRHaI>o zOrz_VDM5h!9YS8j7quw9mI|}zqd+9bXASB(;P)zYt=KqX(xMvGn-V2g?>2y02T4R- z6=+32AD|o!Xi$cZK!6T^_x};^G`r0y4|H~8*f5+$alBUI=0Ls_8$aJ0jmTb*!Koqz z1bjnl$1Oky9_2XA1pXm$Hvna^j>H0oKrIIA7c)?i@e5SC)dKo!2?eXqF3*xWMO4-2 zfT)YpWtx{@NeX+Ew(Sho#xy=^i70QX?wZRgwXDY-OfvB4E-Gkzj@3U3d?MLp!)&0y zH!1+M0AA+3oYY9?+cY{(j~hrEzVRuNq*rXG2EbFy}Uwh_b znP4zc(aZB?vJZG?;OT>lh@#rf)6sp7MuJ8`tdOCUGxwokWQKn2qhEl`S42-b2mo|e zh%j|-@Y1x*1)P$%hjniiC*kJVDnAjt-$EK8r4DE&6>mDLs3zr$EsF)|kop9?L>Z!1 zcZN0up?VG&TexiyntMaEg$Y3a&Q81GY<91qvFb2-2(=imp_2@y!t^M_^+A&j70Hz5 z#6olK1CQs*Ds>wydNy|CHo(|r891Eg-cc>H0x7=*0dCE2II>#;U^>K-hi19RYX>lM z_=0;_<30>7HG(`6s=L;_PFqFYsirn595*@1vP<#&RFqzRqxbeB_BU65E0=AhX0NT$|0EGNz zNzLr?j<^O(k@Ly&6}c$R5PNr@qX9`fmKcpA^4?3>KrJopKz@2;X{=za23BBO(Jy!B~|c7A}m(fK0Gy-+e~XXrixTC=ubsIK*;Jji(O2 zMFQ$VIeB8XFbl)MY8=Yz_;4j1cnPz8kX#1q0=UeO->p<|rXbRip-p9Veh%p(H%!$w zKd1Z4F2}cAtVC|Zz@0bTF#xRJ2s#^3vZ7R2L-a!A$Rc(YEfp?oqh{is z>$POxhfRduah?U>+j3~^N0PD9C2dMiOJKxB7Pk$0BbkGsP#n?;IiQT6TI3DW*)^$% zm9j~tOc};uUbfsyuo|0!lm^q09MT1YB~3Wyc9xPLLMEm*B54nhdM{(y6d_1joxz=6 z?@0DkL^y72OT;zpy<~H-3WH+PhKnrN;u2>~6l~nvkQVG7a5G^!ySloaaJgR%yRh*$ z0Iw%vEda06K&Z*F`IgdEp|W97xPthcp?HTFjWU_Xkmv|V-%9jqLc-v=>ULWor$PN8 zzD5YJLrbDqT$}`YrLK?lO6ajQe@P4cDclEGtaWiq4-n0z418H`ik3>`QBeX6aUjC6 zUc(!)4_oye!3PXiAQX6u&TObcg&si5N!?s`O0VNulI%+zTU8{@s*F<=)E6izY>dip ztwre|56370KA+*ZO-=-@j)2Q?;k7WK+MHuo>Vu_1Wax7rJf2@wsg5 zYr|=WD+EqMiEr~c7C)$_>s@Wg8f&Y`;yyM=tbK6kZ*`B7Tstxz!Kc`OSNB!DE&yL> z8F=GG{qS>;u3*bt{jhZzV^3FWIeN3X??(k6Ks`lQlLR|aF#ZJ>fd~Y}vLy}CVs2Vu z9qyv3V*>FG%u1rlvRdV4+V-4b0Jsw;JM0oxNH9>bLxa(WtErG_xE;cjV%%$|ll&B6 z#?>TvLLscv{vGTd@T4i5*6%J($l$?YH$XX^K}n4$*vnc%EccqpKlc zRWoGEC$ssGRsNQ>h@#p!l$3y0r%xR{)*{@stDS$)2_4>H)EfJwe*yvP0yKHZ5B9U$ zTkc1shbN$sM2yqD|Hk!3+?u9{=^?MaB-Aks!QWwsX500e2l!+M2F+Aqox>VK??S+v zT^`Uq%m7z)8Mh;R4Fi#l{TZ}k25Ol?79?8+xou9BfwL-K2fYM)BfR+S#2@>)%p<~b z$(8W84$~(K;3=Pc3V)^vjT&qn62y10Fq^p~FB?}P#v^XT;=5$4(NaHmvF^NkSm*IkE!pSLfH+`0nuBk0$cG6M z;+md8K6S8qxaxkS7{4CgKnJ=KBx7$M+gV^mz60;4-iGystpY*M(=e9Pb_`>(+kgD0bRSEq1`kUn9$w7%4XjgUZhhs#Rfvb4M8 zPRFwG^#WRk=W%0~$~uG|&zbH>J1Uwau8sRVVgqGkJrY9?CS*Fqw&6hxEj8n34FN!k ziV#2+sxZ?KMmz&PcGho^nr_j<&LNBdX3aU>IeS~f(l`KG{RsOIH1^}ytL4S|6nj*| z*&34?%o5NprXR)=X!F7`tvy>6C&H$rp2^J=HaKh$v5RPYg?1ur04CUXwR2o{?r0xe zEyNnDPP7kGRHS+T?7W^G`k{}huLw$hMt>N*BaYVB7p8nxmm&6+7|sPFX@Rs`mb$`I zsGGR4@>VQ9ERMFn(bnx8k#9YPJ#`mV-6DyiThZ_A@=zS(+qX9uAfr*J68cokhHX%U zkW7|8!hEY0o``ideQ#kS6i-5WFGqN~z2UTC`@^t1WdWb!@ zu$x~fDI~VDjBK7`R9RPG3)4joj~}!#3ARgJhvgY+`R0c`NCn_(@sKYEL%s{M(BErI zi8`<5HF?`)v7)MiLrlWyz2pAro;)JZ6WF^0vGZPzTM$j}&Fz==xVDGYZ*5Nugs7Dy zYkS=IukD2mBqZ@T3|qZ5DH~*ui|G*Rfm_Fu;R@WkC;|LodLVm?mL3d;+<&nn_*+j7 z@IDAxSnW#nNSI#j3i^vTgg)5^ucYD?vhdm#UX^>qzex=rzN^H~`}p!bNxTXzf3y0R zM~`ZJZ^=JQ^ION)%Oke>Kag+z+H>(eD*YU2zWMj?%fHdOeuK|>-^9>4tC ztslPs7^(8Z^ZdNRH{8#6;vS(Sdl!w@J>Nx~kMCMCpS|N1=lho3BkkckX{eX)o%zSx zH^d3Amw&tRG4g%?sprybJi5Y{ZQz&h{BHKc%j3*_l6-t8((7e;Ul>2G{f&eT_VLtr zqkQ~Zwg0?%TzhZNb7@^X`T@SYhx5k|+jpu-`QO|0BOGB&=6>nV$k*@#{Cd4VjncgQ z*{?jVJ>`pd_VNDy9LIhSFTH9%#&LPZQ@`HGciMhZE56D*^yB${k3&!M%5RrXYJ*7r z{MsKk@_qStPilXP@GCsuUpMmo`wvfQKlu_)+Q;+#6UUzBr62$ON$ratTc+sRe=Y6f z{HyQ0S9{v3=3_bh#*+T8U%prS#n Date: Wed, 23 Feb 2022 19:32:54 +1100 Subject: [PATCH 0140/3101] hwdef: added HolybroG4_GPS --- .../hwdef/HolybroG4_GPS/hwdef-bl.dat | 86 ++++++++++ .../hwdef/HolybroG4_GPS/hwdef.dat | 154 ++++++++++++++++++ 2 files changed, 240 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef-bl.dat new file mode 100644 index 00000000000..ced540c3e47 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef-bl.dat @@ -0,0 +1,86 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32G4xx STM32G474xx + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 32 + +# reserve some space for params +APP_START_OFFSET_KB 4 + +# assume 512k flash part +FLASH_SIZE_KB 512 + +# board ID for firmware load +APJ_BOARD_ID 1053 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# debug on USART2 +STDOUT_SERIAL SD2 +STDOUT_BAUDRATE 57600 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +define CH_CFG_ST_FREQUENCY 1000000 + +# order of UARTs +SERIAL_ORDER USART2 + +# blue LED +PC10 LED_BOOTLOADER OUTPUT HIGH +define HAL_LED_ON 0 + +PB15 LED_RED OUTPUT HIGH +PC6 LED_GREEN OUTPUT HIGH + +# USART2 +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define HAL_USE_SERIAL TRUE + +define STM32_SERIAL_USE_USART1 FALSE +define STM32_SERIAL_USE_USART2 TRUE +define STM32_SERIAL_USE_USART3 FALSE + +define HAL_NO_GPIO_IRQ +define HAL_USE_EMPTY_IO TRUE + +# avoid timer and RCIN threads to save memory +define HAL_NO_TIMER_THREAD +define HAL_NO_RCIN_THREAD + +define DMA_RESERVE_SIZE 0 + +define HAL_DISABLE_LOOP_DELAY + +# enable CAN support + +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 +PB14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +define CAN_APP_NODE_NAME "org.ardupilot.HolybroG4_GPS" + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a smaller bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 2500 + +# Add CS pins to ensure they are high in bootloader +PB1 BARO_CS CS +PC4 GYRO_CS CS +PC14 ICM_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat new file mode 100644 index 00000000000..a440f9cd196 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat @@ -0,0 +1,154 @@ +# hw definition file for processing by chibios_pins.py +# MCU class and specific type + +# MCU class and specific type +MCU STM32G4xx STM32G474xx + +FLASH_RESERVE_START_KB 36 + +STORAGE_FLASH_PAGE 16 +define HAL_STORAGE_SIZE 800 + +# board ID for firmware load +APJ_BOARD_ID 1053 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +define CH_CFG_ST_FREQUENCY 1000000 + +# assume 512k flash part +FLASH_SIZE_KB 512 + +# debug on USART2 +STDOUT_SERIAL SD2 +STDOUT_BAUDRATE 57600 + +# order of UARTs +SERIAL_ORDER USART2 USART3 + +# sensor power control +PC11 VDD_3V3_SENSORS_EN OUTPUT HIGH + +# LEDs +PC10 LED OUTPUT HIGH GPIO(2) # blue +PB15 LED_R OUTPUT HIGH GPIO(0) +PC6 LED_G OUTPUT HIGH GPIO(1) + +define HAL_GPIO_A_LED_PIN 0 +define HAL_GPIO_B_LED_PIN 1 +define HAL_GPIO_C_LED_PIN 2 + +define HAL_GPIO_LED_ON 0 +define HAL_GPIO_LED_OFF 1 + +define HAL_HAVE_PIXRACER_LED + +# USART3, GPS +PB10 USART3_TX USART3 +PB11 USART3_RX USART3 + +# USART2, debug +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# I2C1 bus +PB7 I2C1_SDA I2C1 +PB8 I2C1_SCL I2C1 + +# I2C2 bus +PA8 I2C2_SDA I2C2 +PA9 I2C2_SCL I2C2 + +define HAL_I2C_INTERNAL_MASK 3 + +# I2C buses +I2C_ORDER I2C1 I2C2 + +# one SPI bus +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI CS +PC4 GYR_CS CS +PB1 ACC_CS CS +PC14 ICM_CS CS + +# GPS PPS +PA15 GPS_PPS_IN INPUT + +# SPI devices +SPIDEV bmi088_a SPI1 DEVID1 ACC_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_g SPI1 DEVID2 GYR_CS MODE3 10*MHZ 10*MHZ + +# compass +COMPASS BMM150 I2C:0:0x10 false ROTATION_YAW_90 + +# baro +BARO BMP388 I2C:0:0x77 + +# IMU +IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_ROLL_180_YAW_90 + +define HAL_BARO_ALLOW_INIT_NO_BARO + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE + +define HAL_NO_GPIO_IRQ + +# avoid RCIN thread to save memory +define HAL_NO_RCIN_THREAD + +define HAL_USE_RTC FALSE +define DISABLE_SERIAL_ESC_COMM TRUE + +define DMA_RESERVE_SIZE 0 + +define HAL_DISABLE_LOOP_DELAY + +# enable CAN support + +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 +PB14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +define CAN_APP_NODE_NAME "org.ardupilot.HolybroG4_GPS" + +define HAL_NO_LOGGING +define HAL_NO_MONITOR_THREAD + +define HAL_MINIMIZE_FEATURES 0 + +define HAL_DEVICE_THREAD_STACK 768 + +# we setup a small defaults.parm +define AP_PARAM_MAX_EMBEDDED_PARAM 256 + +# disable dual GPS and GPS blending to save flash space +define GPS_MAX_RECEIVERS 1 +define GPS_MAX_INSTANCES 1 +define HAL_COMPASS_MAX_SENSORS 1 + +# GPS+MAG+BARO+LEDs +define HAL_PERIPH_ENABLE_GPS +define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_NOTIFY +define HAL_PERIPH_ENABLE_RC_OUT + +# GPS on 2nd port +define HAL_PERIPH_GPS_PORT_DEFAULT 1 From 18589696972466743697fd60bdfcc68c4a799ab6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 14:03:52 +1100 Subject: [PATCH 0141/3101] AP_Notify: fixed DroneCAN LEDs on AP_Periph override needs to be 0 for LEDs to work on peripherals from DroneCAN notification packets --- libraries/AP_Notify/AP_Notify.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 07a4b1253e0..5b0e3efd321 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -112,11 +112,7 @@ AP_Notify *AP_Notify::_singleton; #endif #ifndef NOTIFY_LED_OVERRIDE_DEFAULT -#ifdef HAL_BUILD_AP_PERIPH - #define NOTIFY_LED_OVERRIDE_DEFAULT 1 // rgb_source_t::mavlink -#else - #define NOTIFY_LED_OVERRIDE_DEFAULT 0 // rgb_source_t::standard -#endif +#define NOTIFY_LED_OVERRIDE_DEFAULT 0 // rgb_source_t::standard #endif #ifndef NOTIFY_LED_LEN_DEFAULT From 0853accea1f91992654e4bf30e45ec812a6ecbfb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 16:25:53 +1100 Subject: [PATCH 0142/3101] AP_Periph: fixed moving baseline yaw for single CAN peripherals the AP_GPS_UAVCAN driver requires this param for auto-config of MB yaw on DroneCAN GPS --- Tools/AP_Periph/Parameters.cpp | 2 +- Tools/AP_Periph/Parameters.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 09943209406..2fe378093c9 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -173,7 +173,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @RebootRequired: True GSCALAR(gps_port, "GPS_PORT", HAL_PERIPH_GPS_PORT_DEFAULT), -#if HAL_NUM_CAN_IFACES >= 2 +#if GPS_MOVING_BASELINE // @Param: MB_CAN_PORT // @DisplayName: Moving Baseline CAN Port option // @Description: Autoselect dedicated CAN port on which moving baseline data will be transmitted. diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 14a2714bfce..2320297fd1f 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -95,7 +95,7 @@ class Parameters { #ifdef HAL_PERIPH_ENABLE_GPS AP_Int8 gps_port; -#if HAL_NUM_CAN_IFACES >= 2 +#if GPS_MOVING_BASELINE AP_Int8 gps_mb_only_can_port; #endif #endif From 74f1a8007d1ec3fc313277136f2b9bc55ad508fe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 16:27:11 +1100 Subject: [PATCH 0143/3101] hwdef: enable GPS_MOVING_BASELINE on FreeflyRTK and f303-GPS this allows for F9P based dual-GPS yaw on DroneCAN peripherals with auto-config --- libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat | 1 + libraries/AP_HAL_ChibiOS/hwdef/f303-GPS/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/f303-Universal/hwdef.dat | 3 +++ 3 files changed, 7 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat index 5a889940e8c..54d9928d96e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat @@ -130,6 +130,7 @@ define AP_PARAM_MAX_EMBEDDED_PARAM 256 # disable dual GPS and GPS blending to save flash space define GPS_MAX_RECEIVERS 1 define GPS_MAX_INSTANCES 1 +define GPS_MOVING_BASELINE 1 define HAL_COMPASS_MAX_SENSORS 1 # GPS+MAG+BARO+Buzzer+NeoPixels diff --git a/libraries/AP_HAL_ChibiOS/hwdef/f303-GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/f303-GPS/hwdef.dat index bfe5edf0a42..67e512fa463 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/f303-GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/f303-GPS/hwdef.dat @@ -14,3 +14,6 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_ENABLE_MAG define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY + +# allow for F9P GPS modules with moving baseline +define GPS_MOVING_BASELINE 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/f303-Universal/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/f303-Universal/hwdef.dat index 507a90719f1..b9bbb9c0bf5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/f303-Universal/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/f303-Universal/hwdef.dat @@ -16,6 +16,9 @@ define HAL_PERIPH_ENABLE_ADSB define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY define HAL_PERIPH_ENABLE_RANGEFINDER +# allow for F9P GPS modules with moving baseline +define GPS_MOVING_BASELINE 1 + define HAL_PERIPH_ADSB_PORT_DEFAULT 3 define HAL_AIRSPEED_BUS_DEFAULT 0 From 60eb6ff1b72152763224fedf420c6ba66ad7ae67 Mon Sep 17 00:00:00 2001 From: murata Date: Sun, 6 Mar 2022 21:54:00 +0900 Subject: [PATCH 0144/3101] hwdef: Maximum number of battery monitors is 1 --- libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat index b47ea2582ae..7b98f93828c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat @@ -157,6 +157,7 @@ define HAL_WITH_DSP FALSE # --------------------- save flash ---------------------- define AP_BATTMON_SMBUS_ENABLE 0 define AP_BATTMON_FUEL_ENABLE 0 +define AP_BATT_MONITOR_MAX_INSTANCES 1 define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 define HAL_GENERATOR_ENABLED 0 From eb78a0a4a63f9110d1cd5e8283020f8686e25ac9 Mon Sep 17 00:00:00 2001 From: murata Date: Sun, 6 Mar 2022 22:24:25 +0900 Subject: [PATCH 0145/3101] hwdef: Set the maximum number of barometric pressure sensors to 1 --- libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat index 7b98f93828c..2c478226582 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat @@ -170,3 +170,4 @@ define HAL_NMEA_OUTPUT_ENABLED 0 define HAL_BUTTON_ENABLED 0 define HAL_OREO_LED_ENABLED 0 define HAL_PICCOLO_CAN_ENABLE 0 +define BARO_MAX_INSTANCES 1 From ea23c1ef0965739fda7845f62e3437c4f27d90a5 Mon Sep 17 00:00:00 2001 From: Rijo Joseph C Date: Sun, 6 Mar 2022 13:44:16 +0530 Subject: [PATCH 0146/3101] README: Fix broken links to CI tests --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 10db35f4269..8e1ec94d77f 100644 --- a/README.md +++ b/README.md @@ -2,11 +2,11 @@ Discord -[![Test Copter](https://github.com/ArduPilot/ardupilot/workflows/test%20copter/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_copter.yml) [![Test Plane](https://github.com/ArduPilot/ardupilot/workflows/test%20plane/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_plane.yml) [![Test Rover](https://github.com/ArduPilot/ardupilot/workflows/test%20rover/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_rover.yml) [![Test Sub](https://github.com/ArduPilot/ardupilot/workflows/test%20sub/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_sub.yml) [![Test Tracker](https://github.com/ArduPilot/ardupilot/workflows/test%20tracker/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_tracker.yml) +[![Test Copter](https://github.com/ArduPilot/ardupilot/workflows/test%20copter/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_copter.yml) [![Test Plane](https://github.com/ArduPilot/ardupilot/workflows/test%20plane/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_plane.yml) [![Test Rover](https://github.com/ArduPilot/ardupilot/workflows/test%20rover/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_rover.yml) [![Test Sub](https://github.com/ArduPilot/ardupilot/workflows/test%20sub/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_sub.yml) [![Test Tracker](https://github.com/ArduPilot/ardupilot/workflows/test%20tracker/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_tracker.yml) -[![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_replay.yml) +[![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_replay.yml) -[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) +[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) [![Test Environment Setup](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml) From d8085f71af8a3dd1e1c24fe70a4d6aee4fc41001 Mon Sep 17 00:00:00 2001 From: RuffaloVM Date: Fri, 4 Mar 2022 18:43:50 +0900 Subject: [PATCH 0147/3101] ArduPlane : case label indentation --- ArduPlane/RC_Channel.cpp | 12 ++++++------ ArduPlane/reverse_thrust.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index dcf71e5085b..59f34fc1fae 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -318,10 +318,10 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit break; #endif -case AUX_FUNC::ARSPD_CALIBRATE: + case AUX_FUNC::ARSPD_CALIBRATE: #if AP_AIRSPEED_AUTOCAL_ENABLE - switch (ch_flag) { - case AuxSwitchPos::HIGH: + switch (ch_flag) { + case AuxSwitchPos::HIGH: plane.airspeed.set_calibration_enabled(true); break; case AuxSwitchPos::MIDDLE: @@ -333,9 +333,9 @@ case AUX_FUNC::ARSPD_CALIBRATE: #endif break; - case AUX_FUNC::LANDING_FLARE: - do_aux_function_flare(ch_flag); - break; + case AUX_FUNC::LANDING_FLARE: + do_aux_function_flare(ch_flag); + break; case AUX_FUNC::PARACHUTE_RELEASE: #if PARACHUTE == ENABLED diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index b44c36feed0..04033d00f66 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -90,16 +90,16 @@ bool Plane::allow_reverse_thrust(void) const case Mode::Number::TAKEOFF: allow = false; break; -case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::FLY_BY_WIRE_A: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_FBWA); break; -case Mode::Number::ACRO: + case Mode::Number::ACRO: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_ACRO); break; -case Mode::Number::STABILIZE: + case Mode::Number::STABILIZE: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_STABILIZE); break; -case Mode::Number::THERMAL: + case Mode::Number::THERMAL: allow |= (g.use_reverse_thrust & USE_REVERSE_THRUST_THERMAL); break; default: From 0ce3f91f636da095ad4e8c1ad68198312c365ea4 Mon Sep 17 00:00:00 2001 From: RuffaloVM Date: Fri, 4 Mar 2022 18:51:06 +0900 Subject: [PATCH 0148/3101] ArduCopter : case label indentation --- ArduCopter/system.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index dd5213b88b3..8375c440093 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -475,7 +475,7 @@ void Copter::allocate_motors(void) motors_var_info = AP_MotorsMatrix_6DoF_Scripting::var_info; #endif // AP_SCRIPTING_ENABLED break; -case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX: + case AP_Motors::MOTOR_FRAME_DYNAMIC_SCRIPTING_MATRIX: #if AP_SCRIPTING_ENABLED motors = new AP_MotorsMatrix_Scripting_Dynamic(copter.scheduler.get_loop_rate_hz()); motors_var_info = AP_MotorsMatrix_Scripting_Dynamic::var_info; From 962d1d112b2de4a021b39bbb771478f7ccbbd729 Mon Sep 17 00:00:00 2001 From: RuffaloLavoisier Date: Sat, 5 Mar 2022 09:20:06 +0900 Subject: [PATCH 0149/3101] ArduPlane : Indentation correction --- ArduPlane/RC_Channel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 59f34fc1fae..ec183484c72 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -321,7 +321,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::ARSPD_CALIBRATE: #if AP_AIRSPEED_AUTOCAL_ENABLE switch (ch_flag) { - case AuxSwitchPos::HIGH: + case AuxSwitchPos::HIGH: plane.airspeed.set_calibration_enabled(true); break; case AuxSwitchPos::MIDDLE: From a7a903e110b4eaca2f34d3949602d2337b2b5fa6 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Tue, 22 Feb 2022 08:05:49 +1100 Subject: [PATCH 0150/3101] CPUInfo: add DSP --- Tools/CPUInfo/CPUInfo.cpp | 47 ++++++++++++++++++++++++--------------- Tools/CPUInfo/Makefile | 2 +- 2 files changed, 30 insertions(+), 19 deletions(-) diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index 9ddca1b1d69..73d1c6a45a9 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -13,6 +13,10 @@ #include #include "EKF_Maths.h" +#if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include +#endif + void setup(); void loop(); @@ -132,24 +136,31 @@ static void show_timings(void) TIMEIT("dmul", v_out_d *= v_d, 100); TIMEIT("ddiv", v_out_d /= v_d, 100); - TIMEIT("sinf()", v_out = sinf(v_f), 20); - TIMEIT("cosf()", v_out = cosf(v_f), 20); - TIMEIT("tanf()", v_out = tanf(v_f), 20); - TIMEIT("acosf()", v_out = acosf(v_f * 0.2), 20); - TIMEIT("asinf()", v_out = asinf(v_f * 0.2), 20); - TIMEIT("atan2f()", v_out = atan2f(v_f * 0.2, v_f * 0.3), 20); - TIMEIT("sqrtf()",v_out = sqrtf(v_f), 20); - - TIMEIT("sin()", v_out = sin(v_f), 20); - TIMEIT("cos()", v_out = cos(v_f), 20); - TIMEIT("tan()", v_out = tan(v_f), 20); - TIMEIT("acos()", v_out = acos(v_f * 0.2), 20); - TIMEIT("asin()", v_out = asin(v_f * 0.2), 20); - TIMEIT("atan2()", v_out = atan2(v_f * 0.2, v_f * 0.3), 20); - TIMEIT("sqrt()",v_out = sqrt(v_f), 20); - TIMEIT("sq()",v_out = sq(v_f), 20); - TIMEIT("powf(v,2)",v_out = powf(v_f, 2), 20); - TIMEIT("powf(v,3.1)",v_out = powf(v_f, 3.1), 20); + TIMEIT("sinf()", v_out = sinf(v_f), 100); + TIMEIT("cosf()", v_out = cosf(v_f), 100); + #if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + TIMEIT("arm_sin_f32()", v_out = arm_sin_f32(v_f), 100); + TIMEIT("arm_cos_f32()", v_out = arm_cos_f32(v_f), 100); + #endif + TIMEIT("tanf()", v_out = tanf(v_f), 100); + TIMEIT("acosf()", v_out = acosf(v_f * 0.2), 100); + TIMEIT("asinf()", v_out = asinf(v_f * 0.2), 100); + TIMEIT("atan2f()", v_out = atan2f(v_f * 0.2, v_f * 0.3), 100); + TIMEIT("sqrtf()",v_out = sqrtf(v_f), 100); + + TIMEIT("sin()", v_out = sin(v_f), 100); + TIMEIT("cos()", v_out = cos(v_f), 100); + TIMEIT("tan()", v_out = tan(v_f), 100); + TIMEIT("acos()", v_out = acos(v_f * 0.2), 100); + TIMEIT("asin()", v_out = asin(v_f * 0.2), 100); + TIMEIT("atan2()", v_out = atan2(v_f * 0.2, v_f * 0.3), 100); + TIMEIT("sqrt()",v_out = sqrt(v_f), 100); + #if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + TIMEIT("arm_sqrt_f32()", arm_sqrt_f32(v_f, (float32_t*)&v_out), 100); + #endif + TIMEIT("sq()",v_out = sq(v_f), 100); + TIMEIT("powf(v,2)",v_out = powf(v_f, 2), 100); + TIMEIT("powf(v,3.1)",v_out = powf(v_f, 3.1), 100); TIMEIT("EKF",v_out = ekf.test(), 5); TIMEIT("iadd8", v_out_8 += v_8, 100); diff --git a/Tools/CPUInfo/Makefile b/Tools/CPUInfo/Makefile index 90eda1cb2a2..e5398174624 100644 --- a/Tools/CPUInfo/Makefile +++ b/Tools/CPUInfo/Makefile @@ -2,7 +2,7 @@ all: @cd ../../ && modules/waf/waf-light configure --board linux --debug - @cd ../../ && modules/waf/waf-light tools + @cd ../../ && modules/waf/waf-light tool @cp ../../build/linux/tool/CPUInfo CPUInfo.elf @echo Built CPUInfo.elf From b4d4ed8a50642ad1fdea545b6998a91d045c2f3f Mon Sep 17 00:00:00 2001 From: murata Date: Sun, 27 Feb 2022 13:33:56 +0900 Subject: [PATCH 0151/3101] AP_Scripting: Change to no message notification in DISARMED state --- .../examples/copter-fast-descent.lua | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Scripting/examples/copter-fast-descent.lua b/libraries/AP_Scripting/examples/copter-fast-descent.lua index 620fdd92133..49752b87276 100644 --- a/libraries/AP_Scripting/examples/copter-fast-descent.lua +++ b/libraries/AP_Scripting/examples/copter-fast-descent.lua @@ -81,8 +81,8 @@ function update() -- activate_type 0: reset stage when disarmed or not in Guided mode if not arming:is_armed() or (vehicle:get_mode() ~= copter_guided_mode_num) then stage = 0 - if (update_user) then - gcs:send_text(0, "Fast Descent: waiting for Guided") + if (update_user and arming:is_armed()) then + gcs:send_text(6, "Fast Descent: waiting for Guided") end return update, interval_ms end @@ -91,8 +91,8 @@ function update() auto_last_id, cmd, arg1, arg2 = vehicle:nav_script_time() if not arming:is_armed() or not auto_last_id then stage = 0 - if (update_user) then - gcs:send_text(0, "Fast Descent: waiting for NAV_SCRIPT_TIME") + if (update_user and arming:is_armed()) then + gcs:send_text(6, "Fast Descent: waiting for NAV_SCRIPT_TIME") end return update, interval_ms end @@ -120,7 +120,7 @@ function update() speed_xy = 0 speed_z = 0 stage = stage + 1 -- advance to next stage - gcs:send_text(0, "Fast Descent: starting") + gcs:send_text(5, "Fast Descent: starting") end elseif (stage == 1) then -- Stage1: descend @@ -211,16 +211,16 @@ function update() stage = stage + 1 end if (update_user) then - gcs:send_text(0, string.format("Fast Descent: alt:%d target:%d", math.floor(-rel_pos_home_NED:z()), math.floor(alt_above_home_min:get()))) + gcs:send_text(5, string.format("Fast Descent: alt:%d target:%d", math.floor(-rel_pos_home_NED:z()), math.floor(alt_above_home_min:get()))) end else - gcs:send_text(0, "Fast Descent: lost position estimate, aborting") + gcs:send_text(5, "Fast Descent: lost position estimate, aborting") stage = stage + 1 end elseif (stage == 2) then -- Stage2: done! stage = stage + 1 - gcs:send_text(0, "Fast Descent: done!") + gcs:send_text(5, "Fast Descent: done!") if (activate_type:get() == 0) then -- if activated from Guided change to RTL mode vehicle:set_mode(copter_rtl_mode_num) From 1ab7eeb25f97bc86b395606d19b0f194eac6de40 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 28 Feb 2022 12:16:52 -0300 Subject: [PATCH 0152/3101] Tools: util.py: fix comment for reltopdir() --- Tools/autotest/pysim/util.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index a906a9096b8..061129f2281 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -62,7 +62,7 @@ def relcurdir(path): return os.path.relpath(path, os.getcwd()) def reltopdir(path): - """Return a path relative to topdir().""" + """Returns the normalized ABSOLUTE path for 'path', where path is a path relative to topdir""" return os.path.normpath(os.path.join(topdir(), path)) From 3875715f676358459528de6b055703b1e0ae7101 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 23:39:59 +0000 Subject: [PATCH 0153/3101] AP_Scripting: applets: add motor failure testing sctipt --- .../applets/motor_failure_test.lua | 67 +++++++++++++++++++ .../applets/motor_failure_test.md | 7 ++ 2 files changed, 74 insertions(+) create mode 100644 libraries/AP_Scripting/applets/motor_failure_test.lua create mode 100644 libraries/AP_Scripting/applets/motor_failure_test.md diff --git a/libraries/AP_Scripting/applets/motor_failure_test.lua b/libraries/AP_Scripting/applets/motor_failure_test.lua new file mode 100644 index 00000000000..64f64440e60 --- /dev/null +++ b/libraries/AP_Scripting/applets/motor_failure_test.lua @@ -0,0 +1,67 @@ +-- This is a script that stops motors in flight, for use testing motor failure handling + +-- add new param MOT_STOP_BITMASK +local PARAM_TABLE_KEY = 75 +assert(param:add_table(PARAM_TABLE_KEY, "MOT_", 1), "could not add param table") +assert(param:add_param(PARAM_TABLE_KEY, 1, "STOP_BITMASK", 0), "could not add param") + +local stop_motor_bitmask = Parameter() +assert(stop_motor_bitmask:init("MOT_STOP_BITMASK"), "could not find param") + +-- find rc switch with option 300 +local switch = assert(rc:find_channel_for_option(300),"Lua: Could not find switch") + +-- read spin min param, we set motors to this PWM to stop them +local pwm_min +if quadplane then + pwm_min = assert(param:get("Q_M_PWM_MIN"),"Lua: Could not read Q_M_PWM_MIN") +else + pwm_min = assert(param:get("MOT_PWM_MIN"),"Lua: Could not read MOT_PWM_MIN") +end + +local stop_motor_chan +local last_motor_bitmask + +-- find any motors enabled, populate channels numbers to stop +local function update_stop_motors(new_bitmask) + if last_motor_bitmask == new_bitmask then + return + end + stop_motor_chan = {} + for i = 1, 12 do + if ((1 << (i-1)) & new_bitmask) ~= 0 then + -- convert motor number to output function number + local output_function + if i <= 8 then + output_function = i+32 + else + output_function = i+81-8 + end + + -- get channel number for output function + local temp_chan = SRV_Channels:find_channel(output_function) + if temp_chan then + table.insert(stop_motor_chan, temp_chan) + end + end + end + last_motor_bitmask = new_bitmask +end + +function update() + + update_stop_motors(stop_motor_bitmask:get()) + + if switch:get_aux_switch_pos() == 2 then + for i = 1, #stop_motor_chan do + -- override for 15ms, called every 10ms + -- using timeout means if the script dies the timeout will expire and all motors will come back + -- we cant leave the vehicle in a un-flyable state + SRV_Channels:set_output_pwm_chan_timeout(stop_motor_chan[i],pwm_min,15) + end + end + + return update, 10 -- reschedule at 100hz +end + +return update() -- run immediately before starting to reschedule diff --git a/libraries/AP_Scripting/applets/motor_failure_test.md b/libraries/AP_Scripting/applets/motor_failure_test.md new file mode 100644 index 00000000000..8a749b7506a --- /dev/null +++ b/libraries/AP_Scripting/applets/motor_failure_test.md @@ -0,0 +1,7 @@ +# Motor Failure testing Lua script + +This script allows testing failure of motors on copter and quadplane (VTOL only). Vehicles with eight or more motors should be able to fly easily with a single failed motor. Hexacopters can also cope with motor failure if they have sufficient thrust. + +Motor failure is triggered by a RC switch configured to option 300 (Scripting1). Switch low all motors will run, switch high will stop motors. + +Configure which motors stop with the param MOT_STOP_BITMASK, this is added by the script so will only show up once the script is loaded on the SD card. The parameters is a bitmask of motors to stop. A value of 1 will stop motor 1, value of 2 stop motor 2, a value of 3 stops both motors 1 and 2. From e07e22c0fe8246ca52c0006b5b28cf0f951dfcf3 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 23:40:48 +0000 Subject: [PATCH 0154/3101] AP_Scripting: applets: add forward flight motor shutdown readme --- .../applets/forward_flight_motor_shutdown.md | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md diff --git a/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md b/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md new file mode 100644 index 00000000000..4055be44bd5 --- /dev/null +++ b/libraries/AP_Scripting/applets/forward_flight_motor_shutdown.md @@ -0,0 +1,9 @@ +# Forward flight motor shutdown script for tailsitters and tiltrotors + +This allows to shutdown selected motors to be stopped once in forward flight for efficiency. + +Set the motors to shutdown in with the `stop_motors` variable. Enable and disable the functionality with a RC switch with options 300 (Scripting1). + +Motors will automatically be shutdown if forward throttle is lower than the value set in `throttle_off_threshold` (50% by default) the motors will then be re-enabled if the throttle goes above the value set in `throttle_on_threshold` (75% by default). + +Time for stopped motors to go from throttle value to 0 and 0 back to throttle can be set with `slew_down_time` and `slew_up_time`. From 8ab92f42cd8a5529cd6723619cf187762810bf50 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 7 Mar 2022 22:04:15 -0600 Subject: [PATCH 0155/3101] AP_RangeFinder: add build option for Rangefinders --- libraries/AP_RangeFinder/AP_RangeFinder.cpp | 5 ++++- libraries/AP_RangeFinder/AP_RangeFinder.h | 12 ++++++++++-- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index fe93be6f3a3..f81fb07d952 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -339,6 +339,7 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance */ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) { +#if AP_RANGEFINDER_ENABLED const Type _type = (Type)params[instance].type.get(); switch (_type) { case Type::PLI2C: @@ -583,7 +584,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) case Type::Benewake_CAN: _add_backend(new AP_RangeFinder_Benewake_CAN(state[instance], params[instance]), instance); break; -#endif +#endif //HAL_MAX_CAN_PROTOCOL_DRIVERS case Type::NONE: default: break; @@ -597,6 +598,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) // param count could have changed AP_Param::invalidate_count(); } +#endif //AP_RANGEFINDER_ENABLED } AP_RangeFinder_Backend *RangeFinder::get_backend(uint8_t id) const { @@ -841,3 +843,4 @@ RangeFinder *rangefinder() } } + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 4eba3c2cc1d..72f832a3a67 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -21,9 +21,17 @@ #include #include "AP_RangeFinder_Params.h" +#ifndef AP_RANGEFINDER_ENABLED +#define AP_RANGEFINDER_ENABLED 1 +#endif + // Maximum number of range finder instances available on this platform -#ifndef RANGEFINDER_MAX_INSTANCES -#define RANGEFINDER_MAX_INSTANCES 10 +#ifndef RANGEFINDER_MAX_INSTANCES + #if AP_RANGEFINDER_ENABLED + #define RANGEFINDER_MAX_INSTANCES 10 + #else + #define RANGEFINDER_MAX_INSTANCES 1 + #endif #endif #define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10 From ca9e3e95a8004fe61aca83f0a95e2de11a431e62 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 7 Mar 2022 22:04:15 -0600 Subject: [PATCH 0156/3101] Tools: add build option for Rangefinders --- Tools/scripts/build_options.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index b77346cf031..5f46164a02c 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -50,7 +50,7 @@ def __init__(self, category, label, define, description, default, dependency): Feature('MSP', 'MSP_BARO', 'HAL_MSP_BARO_ENABLED', 'Enable MSP Baro', 0, 'MSP,OSD'), Feature('MSP', 'MSP_AIRSPEED', 'HAL_MSP_AIRSPEED_ENABLED', 'Enable MSP AirSpeed', 0, 'MSP,OSD'), Feature('MSP', 'MSP_OPTICALFLOW', 'HAL_MSP_OPTICALFLOW_ENABLED', 'Enable MSP OpticalFlow', 0, 'MSP,OSD'), # also OPTFLOW dep - Feature('MSP', 'MSP_RANGEFINDER', 'HAL_MSP_RANGEFINDER_ENABLED', 'Enable MSP Rangefinder', 0, 'MSP,OSD'), + Feature('MSP', 'MSP_RANGEFINDER', 'HAL_MSP_RANGEFINDER_ENABLED', 'Enable MSP Rangefinder', 0, 'MSP,OSD,RANGEFINDERS'), Feature('MSP', 'MSP_DISPLAYPORT', 'HAL_WITH_MSP_DISPLAYPORT', 'Enable MSP DisplayPort OSD (aka CANVAS MODE)', 0, 'MSP,OSD'), Feature('ICE', 'EFI', 'HAL_EFI_ENABLED', 'Enable EFI Monitoring', 0, None), @@ -98,6 +98,7 @@ def __init__(self, category, label, define, description, default, dependency): Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users Feature('Sensors', 'BEACON', 'BEACON_ENABLED', 'Enable Beacon', 0, None), Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None), + Feature('Rangefinders', 'RANGEFINDERS', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders', 0, None), Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None), Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"), From 42d1ce635b8756eddbf738888a3ac1ebdc512280 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 28 Feb 2022 16:06:36 -0500 Subject: [PATCH 0157/3101] AP_Airspeed: improve description of ARSPD_TUBE_ORDR --- libraries/AP_Airspeed/AP_Airspeed.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 12a03c47203..7efb651be10 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -150,8 +150,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: _TUBE_ORDER // @DisplayName: Control pitot tube order - // @Description: Changes the pitot tube order to specify the dynamic pressure side of the sensor. Accepts either if set to 2. Accepts only one side if set to 0 or 1 and can help detect excessive pressure on the static port without indicating positive airspeed. + // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed. // @User: Advanced + // @Values: 0:Normal,1:Swapped,2:Auto Detect AP_GROUPINFO("_TUBE_ORDER", 6, AP_Airspeed, param[0].tube_order, 2), #ifndef HAL_BUILD_AP_PERIPH @@ -253,8 +254,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: 2_TUBE_ORDR // @DisplayName: Control pitot tube order of 2nd airspeed sensor - // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed. + // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed. // @User: Advanced + // @Values: 0:Normal,1:Swapped,2:Auto Detect AP_GROUPINFO("2_TUBE_ORDR", 17, AP_Airspeed, param[1].tube_order, 2), // @Param: 2_SKIP_CAL From 926558a808c8f86465fd38fbf402ae94852a8d69 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 20:09:42 +0900 Subject: [PATCH 0158/3101] Copter: Change the process for errors --- ArduCopter/GCS_Mavlink.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index e4136c439ad..ab86cf7f1d7 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1401,17 +1401,19 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) mavlink_set_home_position_t packet; mavlink_msg_set_home_position_decode(&msg, &packet); if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { - if (!copter.set_home_to_current_location(true)) { - // silently ignored - } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" + copter.set_home_to_current_location(true); +#pragma GCC diagnostic pop } else { Location new_home_loc; new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; - if (!copter.set_home(new_home_loc, true)) { - // silently ignored - } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" + copter.set_home(new_home_loc, true); +#pragma GCC diagnostic pop } break; } From 8037eaf4cfc411d127bcf034ed5a3118488fdb15 Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Thu, 10 Feb 2022 22:40:03 +0900 Subject: [PATCH 0159/3101] Update ArduCopter/GCS_Mavlink.cpp Co-authored-by: Peter Hall <33176108+IamPete1@users.noreply.github.com> --- ArduCopter/GCS_Mavlink.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index ab86cf7f1d7..73b05c077a0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1401,10 +1401,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) mavlink_set_home_position_t packet; mavlink_msg_set_home_position_decode(&msg, &packet); if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-result" - copter.set_home_to_current_location(true); -#pragma GCC diagnostic pop + IGNORE_RETURN(copter.set_home_to_current_location(true)); } else { Location new_home_loc; new_home_loc.lat = packet.latitude; From 77d898c951341e61e889647d8d50967319e798d6 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 22:43:53 +0900 Subject: [PATCH 0160/3101] Copter: Change the process for errors --- ArduCopter/GCS_Mavlink.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 73b05c077a0..ff3c53e159d 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1407,10 +1407,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-result" - copter.set_home(new_home_loc, true); -#pragma GCC diagnostic pop + IGNORE_RETURN(copter.set_home(new_home_loc, true)); } break; } From ff27fb776ddc70b183a87d89ec63c65b78e3f065 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 23:04:14 +0900 Subject: [PATCH 0161/3101] Rover: Change the process for errors --- Rover/commands.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Rover/commands.cpp b/Rover/commands.cpp index 1cd2b7bf763..4298c091859 100644 --- a/Rover/commands.cpp +++ b/Rover/commands.cpp @@ -76,7 +76,5 @@ void Rover::update_home() return; } - if (!ahrs.set_home(loc)) { - // silently ignored... - } + IGNORE_RETURN(ahrs.set_home(loc)); } From e52931c6921cfd71022cdad917838dd79779b757 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 23:04:33 +0900 Subject: [PATCH 0162/3101] Sub: Change the process for errors --- ArduSub/GCS_Mavlink.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0f645a8ca9b..26e10bdfd72 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -734,9 +734,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) if (sub.far_from_EKF_origin(new_home_loc)) { break; } - if (!sub.set_home(new_home_loc, true)) { - // silently ignored - } + IGNORE_RETURN(sub.set_home(new_home_loc, true)); } break; } From b76d048a5e88c3033b117c71cfc374ed5b57a605 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 23:04:54 +0900 Subject: [PATCH 0163/3101] Blimp: Change the process for errors --- Blimp/GCS_Mavlink.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 0f29d59ca57..32396aebf01 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -542,17 +542,13 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) mavlink_set_home_position_t packet; mavlink_msg_set_home_position_decode(&msg, &packet); if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { - if (!blimp.set_home_to_current_location(true)) { - // silently ignored - } + IGNORE_RETURN(blimp.set_home_to_current_location(true)); } else { Location new_home_loc; new_home_loc.lat = packet.latitude; new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; - if (!blimp.set_home(new_home_loc, true)) { - // silently ignored - } + IGNORE_RETURN(blimp.set_home(new_home_loc, true)); } break; } From d9ef916a826ab0662a5a29caf7c7341a4edd5066 Mon Sep 17 00:00:00 2001 From: murata Date: Thu, 10 Feb 2022 23:05:15 +0900 Subject: [PATCH 0164/3101] Tracker: Change the process for errors --- AntennaTracker/sensors.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 1a63346aa05..55946944d94 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -51,9 +51,7 @@ void Tracker::update_GPS(void) // Now have an initial GPS position // use it as the HOME position in future startups current_loc = gps.location(); - if (!set_home(current_loc)) { - // silently ignored - } + IGNORE_RETURN(set_home(current_loc)); ground_start_count = 0; } } From 693022d34457ed17384160c6d8e7b692b0ff7c82 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Feb 2022 21:17:00 +1100 Subject: [PATCH 0165/3101] Tools: add AntennaTracker to builds list for completeness, and time monitoring --- Tools/scripts/build_sizes/build_sizes.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/build_sizes/build_sizes.py b/Tools/scripts/build_sizes/build_sizes.py index 02042a65c38..d5cce220ad3 100755 --- a/Tools/scripts/build_sizes/build_sizes.py +++ b/Tools/scripts/build_sizes/build_sizes.py @@ -16,7 +16,7 @@ parser.add_argument('--outfile', default="builds.html", help='output file') build_dirs = ['latest', 'beta', 'stable'] -builds = ['Plane', 'Copter', 'Rover', 'Sub', 'Blimp', 'AP_Periph'] +builds = ['Plane', 'Copter', 'Rover', 'Sub', 'Blimp', 'AntennaTracker', 'AP_Periph'] args = parser.parse_args() From d3b7bbf994e1eee36509ef3850dee75769babe66 Mon Sep 17 00:00:00 2001 From: murata Date: Fri, 4 Mar 2022 06:51:27 +0900 Subject: [PATCH 0166/3101] SITL: Change build errors --- libraries/SITL/SIM_Buzzer.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/SITL/SIM_Buzzer.h b/libraries/SITL/SIM_Buzzer.h index 1af1635b771..4153cc18050 100644 --- a/libraries/SITL/SIM_Buzzer.h +++ b/libraries/SITL/SIM_Buzzer.h @@ -40,6 +40,13 @@ class Buzzer { AP_Int8 _enable; // enable buzzer sim AP_Int8 _pin; +#ifdef WITH_SITL_TONEALARM + bool was_on; + + uint32_t on_time; + + bool prep_done; +#endif }; } From bb1dc2e9a1a2f4105793515ec118c2f1703f049f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 8 Mar 2022 15:23:59 +0000 Subject: [PATCH 0167/3101] AP_Motors: update no motor found warning message --- libraries/AP_Motors/AP_Motors_Class.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 7f51eea1cc7..4c5d746f6f4 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -199,7 +199,7 @@ void AP_Motors::add_motor_num(int8_t motor_num) SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num); SRV_Channels::set_aux_channel_default(function, motor_num); if (!SRV_Channels::find_channel(function, chan)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num); + gcs().send_text(MAV_SEVERITY_ERROR, "Motors: no SERVOx_FUNCTION set to Motor%u", motor_num + 1); } } } From eee2b00d8cd3e351cd4c69289023126a502948bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 08:53:58 +1100 Subject: [PATCH 0168/3101] AP_Math: fixed build error on cygwin ensure variables are always initialised --- libraries/AP_Math/SCurve.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index 42a14ca797b..5d60eacd033 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -622,11 +622,12 @@ void SCurve::advance_time(float dt) // calculate the jerk, acceleration, velocity and position at the provided time void SCurve::get_jerk_accel_vel_pos_at_time(float time_now, float &Jt_out, float &At_out, float &Vt_out, float &Pt_out) const { + // start with zeros as function is void and we want to guarantee all outputs are initialised + Jt_out = 0; + At_out = 0; + Vt_out = 0; + Pt_out = 0; if (num_segs != segments_max) { - Jt_out = 0; - At_out = 0; - Vt_out = 0; - Pt_out = 0; return; } From cf8d1473b3f80d38fd381ed0f0c9a4346e731234 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 08:42:17 +1100 Subject: [PATCH 0169/3101] CI: fail cygwin build if we don't get the binaries --- .github/workflows/cygwin_build.yml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index d737fa6297d..2febbe38afe 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -27,6 +27,13 @@ jobs: run: | C:\Cygwin\bin\bash --login -c "Tools/scripts/cygwin_build.sh" + - name: Check build files + id: check_files + uses: andstor/file-existence-action@v1 + with: + files: "artifacts/ArduPlane.elf.exe, artifacts/ArduCopter.elf.exe, artifacts/ArduHeli.elf.exe, artifacts/ArduRover.elf.exe, artifacts/ArduSub.elf.exe" + allow_failure: true + - name: Archive build uses: actions/upload-artifact@v2 with: From 8ccf84d9a33e748df28a7cbb78da6b922ebde167 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Mar 2022 12:39:46 +1100 Subject: [PATCH 0170/3101] Plane: disallow mavlink disarm while flying this relies on is_flying(), and we will need to watch for reports of the heuristics failing --- ArduPlane/AP_Arming.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 1abe179c28c..a38289b448a 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -275,12 +275,15 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks) { if (do_disarm_checks && - method == AP_Arming::Method::RUDDER) { - // don't allow rudder-disarming in flight: + (method == AP_Arming::Method::MAVLINK || + method == AP_Arming::Method::RUDDER)) { if (plane.is_flying()) { - // obviously this could happen in-flight so we can't warn about it + // don't allow mavlink or rudder disarm while flying return false; } + } + + if (do_disarm_checks && method == AP_Arming::Method::RUDDER) { // option must be enabled: if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) { gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled"); From e6bbd7f548d4eb227ac661156ab5a0c766b7abe8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 08:12:47 +1100 Subject: [PATCH 0171/3101] autotest: adjust for arming change in plane expect disarm to fail when airmode on --- Tools/autotest/quadplane.py | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 999689a9ad2..4ede8602c3f 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -248,13 +248,25 @@ def test_airmode(self): self.progress("Waiting for Motor1 to speed up") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) - self.progress("Disarm/rearm with GCS") - self.disarm_vehicle() + self.progress("Disarm/rearm with GCS - expect to fail") + self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, # DISARM + 0, + 0, + 0, + 0, + 0, + 0, + timeout=10, + want_result=mavutil.mavlink.MAV_RESULT_FAILED) + + self.progress("Disarm/rearm with GCS - forced") + self.disarm_vehicle(force=True) self.arm_vehicle() self.progress("Verify that airmode is still on") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) - self.disarm_vehicle() + self.disarm_vehicle(force=True) self.wait_ready_to_arm() def test_motor_mask(self): From a98e913a2a084263ff20ff6402cc671d8b1f32e3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 09:45:59 +1100 Subject: [PATCH 0172/3101] autotest: flake8 warning fixes --- Tools/autotest/arducopter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 5ca36c518c3..d04be75bb95 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -3961,7 +3961,7 @@ def test_guided_local_position_target(self, x, y, z_up): if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask): raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" % - ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask)) + ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask)) if x - m.x > 0.1: raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x)) @@ -4012,7 +4012,7 @@ def test_guided_local_velocity_target(self, vx, vy, vz_up, timeout=3): # Check the last received message if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask): raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" % - ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask)) + ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask)) if vx - m.vx > 0.1: raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx)) From 6fcf85edb838bfe0b5695071de02cc5a0790c886 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 08:15:15 +1100 Subject: [PATCH 0173/3101] autotest: more changes for plane disarm disallow --- Tools/autotest/arduplane.py | 8 ++++---- Tools/autotest/common.py | 18 +++++++++++++++++- Tools/autotest/quadplane.py | 15 +-------------- 3 files changed, 22 insertions(+), 19 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 1668fc401bc..570fa365a6e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1038,7 +1038,7 @@ def ThrottleFailsafe(self): "SIM_RC_FAIL": 0, # fix receiver }) self.zero_throttle() - self.disarm_vehicle() + self.disarm_vehicle(force=True) self.context_pop() self.reboot_sitl() @@ -2268,7 +2268,7 @@ def fly_soaring(self): self.wait_waypoint(4, 4, timeout=1200, max_dist=120) # Disarm - self.disarm_vehicle() + self.disarm_vehicle_expect_fail() self.progress("Mission OK") @@ -2365,7 +2365,7 @@ def fly_soaring_speed_to_fly(self): raise NotAchievedException("Airspeed did not reduce with lower SOAR_VSPEED") # Disarm - self.disarm_vehicle() + self.disarm_vehicle_expect_fail() self.progress("Mission OK") @@ -2872,7 +2872,7 @@ def fail_speed(): self.context_clear_collection("STATUSTEXT") ################################################################### - self.disarm_vehicle() + self.disarm_vehicle(force=True) except Exception as e: self.print_exception_caught(e) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 7efa598cc7f..8da34d53dc4 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4361,6 +4361,22 @@ def disarm_vehicle(self, timeout=60, force=False): timeout=timeout) return self.wait_disarmed() + def disarm_vehicle_expect_fail(self): + '''disarm, checking first that non-forced disarm fails, then doing a forced disarm''' + self.progress("Disarm - expect to fail") + self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, # DISARM + 0, + 0, + 0, + 0, + 0, + 0, + timeout=10, + want_result=mavutil.mavlink.MAV_RESULT_FAILED) + self.progress("Disarm - forced") + self.disarm_vehicle(force=True) + def wait_disarmed_default_wait_time(self): return 30 @@ -11191,7 +11207,7 @@ def test_frsky_sport(self): # ok done, stop the engine if self.is_plane(): self.zero_throttle() - if not self.disarm_vehicle(): + if not self.disarm_vehicle(force=True): raise NotAchievedException("Failed to DISARM") def test_frsky_d(self): diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 4ede8602c3f..923e9b4ed9e 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -248,20 +248,7 @@ def test_airmode(self): self.progress("Waiting for Motor1 to speed up") self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge) - self.progress("Disarm/rearm with GCS - expect to fail") - self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # DISARM - 0, - 0, - 0, - 0, - 0, - 0, - timeout=10, - want_result=mavutil.mavlink.MAV_RESULT_FAILED) - - self.progress("Disarm/rearm with GCS - forced") - self.disarm_vehicle(force=True) + self.disarm_vehicle_expect_fail() self.arm_vehicle() self.progress("Verify that airmode is still on") From 4386d748de5b0acef8249e33ad79738f5859da97 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 8 Mar 2022 21:47:15 +0000 Subject: [PATCH 0174/3101] AC_AttitudeControl: WeatherVane: defualt to 0 gain on plane and early return --- libraries/AC_AttitudeControl/AC_WeatherVane.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp index 3db207935ee..e1978c80112 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp @@ -10,10 +10,12 @@ #define WVANE_PARAM_ENABLED 1 #define WVANE_PARAM_SPD_MAX_DEFAULT 0 #define WVANE_PARAM_VELZ_MAX_DEFAULT 0 + #define WVANE_PARAM_GAIN_DEFAULT 0 #else #define WVANE_PARAM_ENABLED 0 #define WVANE_PARAM_SPD_MAX_DEFAULT 2 #define WVANE_PARAM_VELZ_MAX_DEFAULT 1 + #define WVANE_PARAM_GAIN_DEFAULT 1 #endif @@ -32,7 +34,7 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = { // @Range: 0.5 4 // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, 1.0), + AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, WVANE_PARAM_GAIN_DEFAULT), // @Param: ANG_MIN // @DisplayName: Weathervaning min angle @@ -98,8 +100,8 @@ AC_WeatherVane::AC_WeatherVane(void) bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing) { Direction dir = (Direction)_direction.get(); - if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0)) { - // parameter disabled + if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) { + // parameter disabled, or 0 gain // disabled temporarily // dont't override pilot reset(); From c46a19bf89e02ffccd72e5207c4ed1c090be38e0 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 8 Mar 2022 14:02:14 +0000 Subject: [PATCH 0175/3101] ArduPlane: tailsitter: keep attitude controll throttle level upto date for smoother controller handover --- ArduPlane/tailsitter.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index 7ee5b999201..0d9fdc1317e 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -312,6 +312,11 @@ void Tailsitter::output(void) // in assisted flight this is done in the normal motor output path if (!quadplane.assisted_flight) { + + // keep attitude control throttle level upto date, this value should never be output to motors + // it is used to re-set the accel Z integrator term allowing for a smooth transfer of control + quadplane.attitude_control->set_throttle_out(throttle, false, 0); + // convert the hover throttle to the same output that would result if used via AP_Motors // apply expo, battery scaling and SPIN min/max. throttle = motors->thrust_to_actuator(throttle); From bb3bedb90dbba4eb5b782e02d8bd8719ad1eddef Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 7 Mar 2022 20:01:53 +0000 Subject: [PATCH 0176/3101] AP_HAL: always choose high for dshot prescaler calculation --- libraries/AP_HAL/RCOutput.cpp | 24 +++++++++++++++-------- libraries/AP_HAL/tests/test_prescaler.cpp | 2 +- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL/RCOutput.cpp b/libraries/AP_HAL/RCOutput.cpp index 0b36cf1a2c7..f0a4a44724b 100644 --- a/libraries/AP_HAL/RCOutput.cpp +++ b/libraries/AP_HAL/RCOutput.cpp @@ -86,15 +86,23 @@ uint32_t AP_HAL::RCOutput::calculate_bitrate_prescaler(uint32_t timer_clock, uin } } - // find the closest value const uint32_t freq = timer_clock / prescaler; - const float delta = fabsf(float(freq) - target_frequency); - if (freq > target_frequency - && delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) { - prescaler++; - } else if (freq < target_frequency - && delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) { - prescaler--; + // if using dshot then always pick the high value. choosing low seems to not agree with some + // ESCs despite the fact that BLHeli32 is supposed not to care what the bitrate is + if (is_dshot) { + if (freq < target_frequency) { + prescaler--; + } + } else { + // find the closest value + const float delta = fabsf(float(freq) - target_frequency); + if (freq > target_frequency + && delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) { + prescaler++; + } else if (freq < target_frequency + && delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) { + prescaler--; + } } return prescaler; diff --git a/libraries/AP_HAL/tests/test_prescaler.cpp b/libraries/AP_HAL/tests/test_prescaler.cpp index f741fb1c378..1afaa9f1bff 100644 --- a/libraries/AP_HAL/tests/test_prescaler.cpp +++ b/libraries/AP_HAL/tests/test_prescaler.cpp @@ -11,7 +11,7 @@ class PrescalerParameterizedTestFixture :public ::testing::TestWithParam Date: Tue, 8 Mar 2022 15:05:19 +0000 Subject: [PATCH 0177/3101] Plane: never stick mix without valid RC input --- ArduPlane/Attitude.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index b76d8ec2768..23230d3b37c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -61,6 +61,10 @@ float Plane::calc_speed_scaler(void) */ bool Plane::stick_mixing_enabled(void) { + if (!rc().has_valid_input()) { + // never stick mix without valid RC + return false; + } #if AC_FENCE == ENABLED const bool stickmixing = fence_stickmixing(); #else @@ -81,9 +85,7 @@ bool Plane::stick_mixing_enabled(void) // we're in an auto mode. Check the stick mixing flag if (g.stick_mixing != StickMixing::NONE && g.stick_mixing != StickMixing::VTOL_YAW && - stickmixing && - !rc_failsafe_active()) { - // we're in an auto mode, and haven't triggered rc failsafe + stickmixing) { return true; } else { return false; From 5560dc24b5572412103fc98b86d55df6f726d089 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0178/3101] AP_Common: removed terrain home correction --- libraries/AP_Common/Location.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 4ae650312f0..0ba5e4a9a58 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -140,7 +140,7 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const if (terrain == nullptr) { return false; } - if (!terrain->height_amsl(*this, alt_terr_cm, true)) { + if (!terrain->height_amsl(*this, alt_terr_cm)) { return false; } // convert terrain alt to cm From 535d7f0d858a11c5e791b29f612e6ef8147f8b0e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0179/3101] AP_HAL_SITL: removed terrain home correction --- libraries/AP_HAL_SITL/SITL_State.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 491b330a237..be35e182c31 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -906,7 +906,7 @@ void SITL_State::set_height_agl(void) AP_Terrain *_terrain = AP_Terrain::get_singleton(); if (_terrain != nullptr && - _terrain->height_amsl(location, terrain_height_amsl, false)) { + _terrain->height_amsl(location, terrain_height_amsl)) { _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; return; } From 0de118562552b067003aa0c260650759ec8f850f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0180/3101] AP_Scripting: removed terrain home correction --- libraries/AP_Scripting/docs/docs.lua | 3 +-- libraries/AP_Scripting/generator/description/bindings.desc | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index f74195d5ebb..073c2c291b5 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1457,9 +1457,8 @@ function terrain:height_terrain_difference_home(extrapolate) end -- desc ---@param loc Location_ud ----@param corrected boolean ---@return number|nil -function terrain:height_amsl(loc, corrected) end +function terrain:height_amsl(loc) end -- desc ---@return integer diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index fba015b46e7..0bfb3ec4f3a 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -173,7 +173,7 @@ singleton AP_Terrain alias terrain singleton AP_Terrain method enabled boolean singleton AP_Terrain enum TerrainStatusDisabled TerrainStatusUnhealthy TerrainStatusOK singleton AP_Terrain method status uint8_t -singleton AP_Terrain method height_amsl boolean Location float'Null boolean +singleton AP_Terrain method height_amsl boolean Location float'Null singleton AP_Terrain method height_terrain_difference_home boolean float'Null boolean singleton AP_Terrain method height_above_terrain boolean float'Null boolean From 5d3a0a78cbfa98ec24a5d68155bb8d67219c1617 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0181/3101] AP_Terrain: removed terrain home correction --- libraries/AP_Terrain/AP_Terrain.cpp | 25 ++++++++----------------- libraries/AP_Terrain/AP_Terrain.h | 6 +----- libraries/AP_Terrain/TerrainGCS.cpp | 6 +++--- libraries/AP_Terrain/TerrainMission.cpp | 4 ++-- 4 files changed, 14 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 3f2cf781666..a6146937aeb 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -95,7 +95,7 @@ AP_Terrain::AP_Terrain() : This function costs about 20 microseconds on Pixhawk */ -bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) +bool AP_Terrain::height_amsl(const Location &loc, float &height) { if (!allocate()) { return false; @@ -107,10 +107,6 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) if (loc.lat == home_loc.lat && loc.lng == home_loc.lng) { height = home_height; - // apply correction which assumes home altitude is at terrain altitude - if (corrected) { - height += (ahrs.get_home().alt * 0.01f) - home_height; - } return true; } @@ -161,11 +157,6 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) home_loc = loc; } - // apply correction which assumes home altitude is at terrain altitude - if (corrected) { - height += (ahrs.get_home().alt * 0.01f) - home_height; - } - return true; } @@ -186,7 +177,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool const AP_AHRS &ahrs = AP::ahrs(); float height_home, height_loc; - if (!height_amsl(ahrs.get_home(), height_home, false)) { + if (!height_amsl(ahrs.get_home(), height_home)) { // we don't know the height of home return false; } @@ -197,7 +188,7 @@ bool AP_Terrain::height_terrain_difference_home(float &terrain_difference, bool return false; } - if (!height_amsl(loc, height_loc, false)) { + if (!height_amsl(loc, height_loc)) { if (!extrapolate || !have_current_loc_height) { // we don't know the height of the given location return false; @@ -283,7 +274,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio) return 0; } float base_height; - if (!height_amsl(loc, base_height, false)) { + if (!height_amsl(loc, base_height)) { // we don't know our current terrain height return 0; } @@ -297,7 +288,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio) climb += climb_ratio * grid_spacing; distance -= grid_spacing; float height; - if (height_amsl(loc, height, false)) { + if (height_amsl(loc, height)) { float rise = (height - base_height) - climb; if (rise > lookahead_estimate) { lookahead_estimate = rise; @@ -324,12 +315,12 @@ void AP_Terrain::update(void) // try to ensure the home location is populated float height; - height_amsl(ahrs.get_home(), height, false); + height_amsl(ahrs.get_home(), height); // update the cached current location height Location loc; bool pos_valid = ahrs.get_location(loc); - bool terrain_valid = pos_valid && height_amsl(loc, height, false); + bool terrain_valid = pos_valid && height_amsl(loc, height); if (pos_valid && terrain_valid) { last_current_loc_height = height; have_current_loc_height = true; @@ -372,7 +363,7 @@ void AP_Terrain::log_terrain_data() float current_height = 0; uint16_t pending, loaded; - height_amsl(loc, terrain_height, false); + height_amsl(loc, terrain_height); height_above_terrain(current_height, true); get_statistics(pending, loaded); diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index aeac19c4bc8..ae47f726641 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -121,12 +121,8 @@ class AP_Terrain { find the terrain height in meters above sea level for a location return false if not available - - if corrected is true then terrain alt is adjusted so that - the terrain altitude matches the home altitude at the home location - (i.e. we assume home is at the terrain altitude) */ - bool height_amsl(const Location &loc, float &height, bool corrected); + bool height_amsl(const Location &loc, float &height); /* find difference between home terrain height and the terrain diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index 17621f1a72d..4eed5931ab5 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -228,8 +228,8 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc Location current_loc; const AP_AHRS &ahrs = AP::ahrs(); if (ahrs.get_location(current_loc) && - height_amsl(ahrs.get_home(), home_terrain_height, false) && - height_amsl(loc, terrain_height, false)) { + height_amsl(ahrs.get_home(), home_terrain_height) && + height_amsl(loc, terrain_height)) { // non-zero spacing indicates we have data spacing = grid_spacing; } else if (extrapolate && have_current_loc_height) { @@ -238,7 +238,7 @@ void AP_Terrain::send_terrain_report(mavlink_channel_t chan, const Location &loc terrain_height = last_current_loc_height; } else { // report terrain height if we can, but can't give current_height - height_amsl(loc, terrain_height, false); + height_amsl(loc, terrain_height); } uint16_t pending, loaded; get_statistics(pending, loaded); diff --git a/libraries/AP_Terrain/TerrainMission.cpp b/libraries/AP_Terrain/TerrainMission.cpp index ab6b8b79cea..40a23f865a9 100644 --- a/libraries/AP_Terrain/TerrainMission.cpp +++ b/libraries/AP_Terrain/TerrainMission.cpp @@ -95,7 +95,7 @@ void AP_Terrain::update_mission_data(void) // we have a mission command to check float height; - if (!height_amsl(cmd.content.location, height, false)) { + if (!height_amsl(cmd.content.location, height)) { // if we can't get data for a mission item then return and // check again next time return; @@ -157,7 +157,7 @@ void AP_Terrain::update_rally_data(void) loc.lat = rp.lat; loc.lng = rp.lng; float height; - if (!height_amsl(loc, height, false)) { + if (!height_amsl(loc, height)) { // if we can't get data for a rally item then return and // check again next time return; From 07b78ab9a634ad6e82092a52ee37395324c5453a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0182/3101] SITL: removed terrain home correction --- libraries/SITL/SIM_Aircraft.cpp | 4 ++-- libraries/SITL/SIM_Ship.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index b1bdf65988c..d99a39a14d4 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -108,8 +108,8 @@ float Aircraft::ground_height_difference() const if (sitl && terrain != nullptr && sitl->terrain_enable && - terrain->height_amsl(home, h1, false) && - terrain->height_amsl(location, h2, false)) { + terrain->height_amsl(home, h1) && + terrain->height_amsl(location, h2)) { h2 += local_ground_level; return h2 - h1; } diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index 5500c3135cb..3e097f16a8b 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -183,7 +183,7 @@ void ShipSim::send_report(void) #if AP_TERRAIN_AVAILABLE auto terrain = AP::terrain(); float height; - if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height, true)) { + if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height)) { alt_mm = height * 1000; } #endif From 72087335f7205d705dee01c4ad2d93153193dc64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 31 Jan 2022 15:35:06 +1100 Subject: [PATCH 0183/3101] Plane: removed terrain home correction --- ArduPlane/altitude.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index ce717a12b67..3ad2b2dfa39 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -628,8 +628,8 @@ void Plane::rangefinder_terrain_correction(float &height) return; } float terrain_amsl1, terrain_amsl2; - if (!terrain.height_amsl(current_loc, terrain_amsl1, false) || - !terrain.height_amsl(next_WP_loc, terrain_amsl2, false)) { + if (!terrain.height_amsl(current_loc, terrain_amsl1) || + !terrain.height_amsl(next_WP_loc, terrain_amsl2)) { return; } float correction = (terrain_amsl1 - terrain_amsl2); From 431330c651f55676add82e17bc3fb14c4e8506df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jan 2022 20:20:33 +1100 Subject: [PATCH 0184/3101] Tools: added SFO_Bay location --- Tools/autotest/locations.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index 1722ce3ed82..f32c8c042a1 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -92,3 +92,5 @@ RF_AirStadium=36.893328,-2.720371,1434,0 RF_BuenaVista=37.093686,-2.890969,2390,0 RF_Castle=37.090662,-3.074557,2736,0 RF_Garage=37.621798,-2.646596,788,0 +SFO_Bay=37.62561973,-122.33235387,0.0,0 + From ab64744ccd820da56f422553f9212475da25cbe2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jan 2022 15:52:47 +1100 Subject: [PATCH 0185/3101] AP_Follow: added APIs for plane ship landing --- libraries/AP_Follow/AP_Follow.cpp | 48 ++++++++++++++++++++++++++++++- libraries/AP_Follow/AP_Follow.h | 16 +++++++++++ 2 files changed, 63 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index a71d8eefa35..ff5d52d65eb 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -33,6 +33,14 @@ extern const AP_HAL::HAL& hal; #define AP_FOLLOW_POS_P_DEFAULT 0.1f // position error gain default +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) +#define AP_FOLLOW_ALT_TYPE_DEFAULT 0 +#else +#define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE +#endif + +AP_Follow *AP_Follow::_singleton; + // table of user settable parameters const AP_Param::GroupInfo AP_Follow::var_info[] = { @@ -117,7 +125,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { // @Description: Follow altitude type // @Values: 0:absolute, 1:relative // @User: Standard - AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALTITUDE_TYPE_RELATIVE), + AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT), #endif AP_GROUPEND @@ -131,6 +139,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = { AP_Follow::AP_Follow() : _p_pos(AP_FOLLOW_POS_P_DEFAULT) { + _singleton = this; AP_Param::setup_object_defaults(this, var_info); } @@ -479,3 +488,40 @@ void AP_Follow::clear_dist_and_bearing_to_target() _dist_to_target = 0.0f; _bearing_to_target = 0.0f; } + +// get target's estimated location and velocity (in NED), with offsets added +bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const +{ + Vector3f ofs; + if (!get_offsets_ned(ofs) || + !get_target_location_and_velocity(loc, vel_ned)) { + return false; + } + // apply offsets + loc.offset(ofs.x, ofs.y); + loc.alt -= ofs.z*100; + return true; +} + +// return true if we have a target +bool AP_Follow::have_target(void) const +{ + if (!_enabled) { + return false; + } + + // check for timeout + if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) { + return false; + } + return true; +} + +namespace AP { + +AP_Follow &follow() +{ + return *AP_Follow::get_singleton(); +} + +} diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h index a7b7f0d6701..353fe3fdcfa 100644 --- a/libraries/AP_Follow/AP_Follow.h +++ b/libraries/AP_Follow/AP_Follow.h @@ -39,6 +39,11 @@ class AP_Follow // constructor AP_Follow(); + // enable as singleton + static AP_Follow *get_singleton(void) { + return _singleton; + } + // returns true if library is enabled bool enabled() const { return _enabled; } @@ -58,6 +63,9 @@ class AP_Follow // get target's estimated location and velocity (in NED) bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const; + // get target's estimated location and velocity (in NED), with offsets added + bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const; + // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned); @@ -87,10 +95,14 @@ class AP_Follow // get bearing to target (including offset) in degrees (for reporting purposes) float get_bearing_to_target() const { return _bearing_to_target; } + // get system time of last position update + uint32_t get_last_update_ms() const { return _last_location_update_ms; } + // parameter list static const struct AP_Param::GroupInfo var_info[]; private: + static AP_Follow *_singleton; // get velocity estimate in m/s in NED frame using dt since last update bool get_velocity_ned(Vector3f &vel_ned, float dt) const; @@ -133,3 +145,7 @@ class AP_Follow // setup jitter correction with max transport lag of 3s JitterCorrection _jitter{3000}; }; + +namespace AP { + AP_Follow &follow(); +}; From c54fb0f51b03953210d98f84a39e598f52ce383f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jan 2022 15:53:18 +1100 Subject: [PATCH 0186/3101] Plane: link in AP_Follow ready for ship landing --- ArduPlane/GCS_Mavlink.cpp | 24 ++++++++++++++++++++++++ ArduPlane/Parameters.cpp | 6 ++++++ ArduPlane/Parameters.h | 3 +++ ArduPlane/Plane.h | 1 + ArduPlane/wscript | 1 + 5 files changed, 35 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 948b0b49483..7b899741b3a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -652,6 +652,10 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, { #if HAL_ADSB_ENABLED plane.avoidance_adsb.handle_msg(msg); +#endif +#if SHIP_LANDING_ENABLED + // pass message to follow library + plane.g2.follow.handle_msg(msg); #endif GCS_MAVLINK::packetReceived(status, msg); } @@ -884,6 +888,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_GUIDED_CHANGE_HEADING: return handle_command_int_guided_slew_commands(packet); + case MAV_CMD_DO_FOLLOW: +#if SHIP_LANDING_ENABLED + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + plane.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } +#endif + return MAV_RESULT_FAILED; + default: return GCS_MAVLINK::handle_command_int_packet(packet); } @@ -1085,6 +1099,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } return MAV_RESULT_ACCEPTED; +#if SHIP_LANDING_ENABLED + case MAV_CMD_DO_FOLLOW: + // param1: sysid of target to follow + if ((packet.param1 > 0) && (packet.param1 <= 255)) { + plane.g2.follow.set_target_sysid((uint8_t)packet.param1); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; +#endif + default: return GCS_MAVLINK::handle_command_long_packet(packet); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 1b0c5139404..c09f77752ad 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1229,6 +1229,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15 AP_GROUPINFO("ONESHOT_MASK", 32, ParametersG2, oneshot_mask, 0), + +#if SHIP_LANDING_ENABLED + // @Group: FOLL + // @Path: ../libraries/AP_Follow/AP_Follow.cpp + AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow), +#endif AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index d22e435a47b..59f8b08e2c8 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -545,6 +545,9 @@ class ParametersG2 { AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; #endif +#if SHIP_LANDING_ENABLED + AP_Follow follow; +#endif AP_Float fs_ekf_thresh; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cdf85eb5a91..4f6030b78c7 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -82,6 +82,7 @@ #include #include #include // Landing Gear library +#include #include "GCS_Mavlink.h" #include "GCS_Plane.h" diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 4ce4e0f02c8..1386bd7b7c1 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -29,6 +29,7 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_KDECAN', + 'AP_Follow', ], ) From 3fe9a5526934358ea455f82e2bd15cc259a0d171 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jan 2022 15:53:45 +1100 Subject: [PATCH 0187/3101] Plane: added APIs for lua ship landing --- ArduPlane/ArduPlane.cpp | 34 ++++++++ ArduPlane/GCS_Mavlink.cpp | 6 +- ArduPlane/Log.cpp | 2 +- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 2 + ArduPlane/quadplane.cpp | 162 +++++++++++++++++++++++--------------- ArduPlane/quadplane.h | 9 +++ 8 files changed, 148 insertions(+), 71 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 52f8f324119..7d67005f27f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -738,6 +738,7 @@ bool Plane::get_target_location(Location& target_loc) case Mode::Number::GUIDED: case Mode::Number::AUTO: case Mode::Number::LOITER: + case Mode::Number::TAKEOFF: #if HAL_QUADPLANE_ENABLED case Mode::Number::QLOITER: case Mode::Number::QLAND: @@ -751,6 +752,39 @@ bool Plane::get_target_location(Location& target_loc) } return false; } + +/* + update_target_location() works in all auto navigation modes + */ +bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) +{ + if (!old_loc.same_latlon_as(next_WP_loc)) { + return false; + } + ftype alt_diff; + if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) || + !is_zero(alt_diff)) { + return false; + } + next_WP_loc = new_loc; + next_WP_loc.change_alt_frame(old_loc.get_alt_frame()); + + return true; +} + +// allow for velocity matching in VTOL +bool Plane::set_velocity_match(const Vector2f &velocity) +{ +#if HAL_QUADPLANE_ENABLED + if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) { + quadplane.poscontrol.velocity_match = velocity; + quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis(); + return true; + } +#endif + return false; +} + #endif // AP_SCRIPTING_ENABLED #if OSD_ENABLED diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 7b899741b3a..b7328a1364d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -653,7 +653,7 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, #if HAL_ADSB_ENABLED plane.avoidance_adsb.handle_msg(msg); #endif -#if SHIP_LANDING_ENABLED +#if AP_SCRIPTING_ENABLED // pass message to follow library plane.g2.follow.handle_msg(msg); #endif @@ -889,7 +889,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in return handle_command_int_guided_slew_commands(packet); case MAV_CMD_DO_FOLLOW: -#if SHIP_LANDING_ENABLED +#if AP_SCRIPTING_ENABLED // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { plane.g2.follow.set_target_sysid((uint8_t)packet.param1); @@ -1099,7 +1099,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } return MAV_RESULT_ACCEPTED; -#if SHIP_LANDING_ENABLED +#if AP_SCRIPTING_ENABLED case MAV_CMD_DO_FOLLOW: // param1: sysid of target to follow if ((packet.param1 > 0) && (packet.param1 <= 255)) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index ad3ba064f66..7904d674003 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -23,7 +23,7 @@ void Plane::Log_Write_Attitude(void) } else { ahrs.Write_Attitude(targets); } - if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) { + if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight() || quadplane.in_vtol_airbrake()) { // log quadplane PIDs separately from fixed wing PIDs logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index c09f77752ad..c38483f0b1c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1230,7 +1230,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15 AP_GROUPINFO("ONESHOT_MASK", 32, ParametersG2, oneshot_mask, 0), -#if SHIP_LANDING_ENABLED +#if AP_SCRIPTING_ENABLED // @Group: FOLL // @Path: ../libraries/AP_Follow/AP_Follow.cpp AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 59f8b08e2c8..bae6c1a3deb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -545,7 +545,7 @@ class ParametersG2 { AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2}; #endif -#if SHIP_LANDING_ENABLED +#if AP_SCRIPTING_ENABLED AP_Follow follow; #endif diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 4f6030b78c7..8a74bb1e3c4 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1210,6 +1210,8 @@ class Plane : public AP_Vehicle { #if AP_SCRIPTING_ENABLED bool set_target_location(const Location& target_loc) override; bool get_target_location(Location& target_loc) override; + bool update_target_location(const Location &old_loc, const Location &new_loc) override; + bool set_velocity_match(const Vector2f &velocity) override; #endif // AP_SCRIPTING_ENABLED }; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 07ccdcec810..7cf1cd3539d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1714,7 +1714,7 @@ void QuadPlane::update(void) } const uint32_t now = AP_HAL::millis(); - if (!in_vtol_mode()) { + if (!in_vtol_mode() && !in_vtol_airbrake()) { // we're in a fixed wing mode, cope with transitions and check // for assistance needed if (plane.control_mode == &plane.mode_manual || @@ -1733,7 +1733,7 @@ void QuadPlane::update(void) } else { - assisted_flight = false; + assisted_flight = in_vtol_airbrake(); // output to motors motors_output(); @@ -2011,6 +2011,9 @@ bool QuadPlane::in_vtol_auto(void) const /* are we in a VTOL mode? This is used to decide if we run the transition handling code or not + + note that AIRBRAKE is not considered in_vtol_mode even though the + VTOL motors are running */ bool QuadPlane::in_vtol_mode(void) const { @@ -2018,7 +2021,7 @@ bool QuadPlane::in_vtol_mode(void) const return false; } if (in_vtol_land_sequence()) { - return poscontrol.get_state() != QPOS_APPROACH; + return poscontrol.get_state() != QPOS_APPROACH && poscontrol.get_state() != QPOS_AIRBRAKE; } if (plane.control_mode->is_vtol_mode()) { return true; @@ -2033,7 +2036,7 @@ bool QuadPlane::in_vtol_mode(void) const return true; } if (in_vtol_auto()) { - if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_APPROACH) { + if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_AIRBRAKE) { return true; } } @@ -2082,7 +2085,8 @@ void QuadPlane::update_land_positioning(void) poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms; poscontrol.target_vel_cms.rotate_xy(ahrs_view->yaw); - poscontrol.target_cm += (poscontrol.target_vel_cms * dt).topostype(); + // integrate our corrected position + poscontrol.xy_correction += poscontrol.target_vel_cms.xy() * dt * 0.01; poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in)); if (poscontrol.pilot_correction_active) { @@ -2095,9 +2099,16 @@ void QuadPlane::update_land_positioning(void) */ void QuadPlane::run_xy_controller(void) { + float accel_cmss = wp_nav->get_wp_acceleration(); + if (in_vtol_land_sequence() && + (poscontrol.get_state() == QPOS_POSITION1 || + poscontrol.get_state() == QPOS_POSITION2)) { + accel_cmss = MAX(accel_cmss, transition_decel*100); + } + const float speed_cms = wp_nav->get_default_speed_xy(); + pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss); + pos_control->set_correction_speed_accel_xy(speed_cms, accel_cmss); if (!pos_control->is_active_xy()) { - pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); - pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->init_xy_controller(); } pos_control->update_xy_controller(); @@ -2130,6 +2141,8 @@ void QuadPlane::poscontrol_init_approach(void) } poscontrol.thrust_loss_start_ms = 0; } + poscontrol.pilot_correction_done = false; + poscontrol.xy_correction.zero(); } /* @@ -2159,13 +2172,6 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // if it is active then the rate control should not be reset at all qp.attitude_control->reset_yaw_target_and_rate(false); pos1_start_speed = plane.ahrs.groundspeed_vector().length(); - } else if (s == QPOS_POSITION2) { - // POSITION2 changes target speed, so we need to change it - // back to normal - qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), - qp.wp_nav->get_wp_acceleration()); - qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), - qp.wp_nav->get_wp_acceleration()); } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle qp.pos_control->get_accel_z_pid().set_integrator(0); @@ -2212,6 +2218,11 @@ void QuadPlane::vtol_position_controller(void) // and tilt is more than tilt max bool suppress_z_controller = false; + Vector2f landing_velocity; + if (now_ms - poscontrol.last_velocity_match_ms < 1000) { + landing_velocity = poscontrol.velocity_match; + } + // horizontal position control switch (poscontrol.get_state()) { @@ -2423,19 +2434,11 @@ void QuadPlane::vtol_position_controller(void) if (distance > 0.1) { target_speed_xy_cms = diff_wp.normalized() * target_speed * 100; } - if (!tailsitter.enabled()) { - // this method ignores pos-control velocity/accel limtis - pos_control->set_vel_desired_xy_cms(target_speed_xy_cms); - - // reset position controller xy target to current position - // because we only want velocity control (no position control) - const Vector2f& curr_pos = inertial_nav.get_position_xy_cm(); - pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y); - pos_control->set_accel_desired_xy_cmss(Vector2f()); - } else { - // tailsitters use input shaping and abide by velocity limits - pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f()); - } + + target_speed_xy_cms += landing_velocity * 100; + + // use input shaping and abide by accel and jerk limits + pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f()); // run horizontal velocity controller run_xy_controller(); @@ -2465,35 +2468,19 @@ void QuadPlane::vtol_position_controller(void) case QPOS_POSITION2: case QPOS_LAND_DESCEND: { + setup_target_position(); /* for final land repositioning and descent we run the position controller */ - if (poscontrol.pilot_correction_done) { - // if the pilot has repositioned the vehicle then we switch to velocity control. This prevents the vehicle - // shifting position in the event of GPS glitches. - Vector2f zero; - pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero); - } else { - Vector2f zero; - pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), zero, zero); - } + Vector2f zero; + Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100; + pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero); // also run fixed wing navigation plane.nav_controller->update_waypoint(plane.current_loc, loc); update_land_positioning(); - /* - apply the same asymmetric speed limits from POSITION1, so we - don't suddenly speed up when we change to POSITION2 and - LAND_DESCEND - */ - const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc); - const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); - - pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); - pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); - run_xy_controller(); // nav roll and pitch are controlled by position controller @@ -2520,7 +2507,8 @@ void QuadPlane::vtol_position_controller(void) } else { // we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling Vector2f zero; - pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero); + Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100; + pos_control->input_vel_accel_xy(vel_cms, zero); } run_xy_controller(); @@ -2681,20 +2669,12 @@ void QuadPlane::setup_target_position(void) set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } - const Vector2f diff2d = origin.get_distance_NE(loc); + Vector2f diff2d = origin.get_distance_NE(loc); + diff2d += poscontrol.xy_correction; poscontrol.target_cm.x = diff2d.x * 100; poscontrol.target_cm.y = diff2d.y * 100; poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt; - const uint32_t now = AP_HAL::millis(); - if (!loc.same_latlon_as(last_auto_target) || - plane.next_WP_loc.alt != last_auto_target.alt || - now - last_loiter_ms > 500) { - wp_nav->set_wp_destination(poscontrol.target_cm.tofloat()); - last_auto_target = loc; - } - last_loiter_ms = now; - // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); @@ -2705,18 +2685,24 @@ void QuadPlane::setup_target_position(void) */ void QuadPlane::takeoff_controller(void) { + if (!hal.util->get_soft_armed()) { + return; + } /* for takeoff we use the position controller */ setup_target_position(); - // set position controller desired velocity and acceleration to zero - pos_control->set_vel_desired_xy_cms(Vector2f()); - pos_control->set_accel_desired_xy_cmss(Vector2f()); - // set position control target and update - Vector2f zero; - pos_control->input_vel_accel_xy(zero, zero); + + Vector2f vel, zero; + if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) { + vel = poscontrol.velocity_match * 100; + } + + pos_control->set_accel_desired_xy_cmss(zero); + pos_control->set_vel_desired_xy_cms(vel); + pos_control->input_vel_accel_xy(vel, zero); run_xy_controller(); @@ -2757,6 +2743,16 @@ void QuadPlane::waypoint_controller(void) { setup_target_position(); + const Location &loc = plane.next_WP_loc; + const uint32_t now = AP_HAL::millis(); + if (!loc.same_latlon_as(last_auto_target) || + plane.next_WP_loc.alt != last_auto_target.alt || + now - last_loiter_ms > 500) { + wp_nav->set_wp_destination(poscontrol.target_cm.tofloat()); + last_auto_target = loc; + } + last_loiter_ms = now; + /* this is full copter control of auto flight */ @@ -3086,10 +3082,18 @@ bool QuadPlane::verify_vtol_land(void) const float dist = (inertial_nav.get_position_neu_cm().topostype() - poscontrol.target_cm).xy().length() * 0.01; reached_position = dist < descend_dist_threshold; } + Vector2f target_vel; + if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) { + target_vel = poscontrol.velocity_match; + } + Vector3f vel_ned; + UNUSED_RESULT(plane.ahrs.get_velocity_NED(vel_ned)); + if (reached_position && - plane.ahrs.groundspeed() < descend_speed_threshold) { + (vel_ned.xy() - target_vel).length() < descend_speed_threshold) { poscontrol.set_state(QPOS_LAND_DESCEND); poscontrol.pilot_correction_done = false; + poscontrol.xy_correction.zero(); #if AC_FENCE == ENABLED plane.fence.auto_disable_fence_for_landing(); #endif @@ -3642,6 +3646,23 @@ bool QuadPlane::in_vtol_land_poscontrol(void) const return false; } +/* + see if we are in the airbrake phase of a VTOL landing + */ +bool QuadPlane::in_vtol_airbrake(void) const +{ + if (plane.control_mode == &plane.mode_qrtl && + poscontrol.get_state() == QPOS_AIRBRAKE) { + return true; + } + if (plane.control_mode == &plane.mode_auto && + is_vtol_land(plane.mission.get_current_nav_cmd().id) && + poscontrol.get_state() == QPOS_AIRBRAKE) { + return true; + } + return false; +} + // return true if we should show VTOL view bool QuadPlane::show_vtol_view() const { @@ -3877,4 +3898,15 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const return MAV_VTOL_STATE_UNDEFINED; } +/* + see if we are in a VTOL takeoff + */ +bool QuadPlane::in_vtol_takeoff(void) const +{ + if (in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) { + return true; + } + return false; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 7b184afc005..284c6f99dab 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -108,6 +108,7 @@ class QuadPlane bool verify_vtol_land(void); bool in_vtol_auto(void) const; bool in_vtol_mode(void) const; + bool in_vtol_takeoff(void) const; bool in_vtol_posvel_mode(void) const; void update_throttle_hover(); bool show_vtol_view() const; @@ -433,6 +434,7 @@ class QuadPlane return AP_HAL::millis() - last_state_change_ms; } Vector3p target_cm; + Vector2f xy_correction; Vector3f target_vel_cms; bool slow_descent:1; bool pilot_correction_active; @@ -442,6 +444,8 @@ class QuadPlane bool reached_wp_speed; uint32_t last_run_ms; float pos1_start_speed; + Vector2f velocity_match; + uint32_t last_velocity_match_ms; private: uint32_t last_state_change_ms; enum position_control_state state; @@ -564,6 +568,11 @@ class QuadPlane see if we are in the VTOL position control phase of a landing */ bool in_vtol_land_poscontrol(void) const; + + /* + are we in the airbrake phase of a VTOL landing? + */ + bool in_vtol_airbrake(void) const; // Q assist state, can be enabled, disabled or force. Default to enabled Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED; From 33bc067edf30f51161fee5b606e4597ae106b366 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Jan 2022 07:49:00 +1100 Subject: [PATCH 0188/3101] Plane: fix NAV_CONTROLLER_OUTPUT in Q modes Q modes don't always use wp_nav, but do use pos_control AUTO and QRTL setup the plane navigation as well, so can use the L1 data --- ArduPlane/GCS_Mavlink.cpp | 13 ++++++++----- ArduPlane/quadplane.cpp | 6 ------ 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b7328a1364d..e913678acb9 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -172,20 +172,23 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const } #if HAL_QUADPLANE_ENABLED const QuadPlane &quadplane = plane.quadplane; - if (quadplane.show_vtol_view()) { + if (quadplane.show_vtol_view() && quadplane.using_wp_nav()) { const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd(); - bool wp_nav_valid = quadplane.using_wp_nav(); + + const Vector2f& curr_pos = quadplane.inertial_nav.get_position_xy_cm(); + const Vector2f& target_pos = quadplane.pos_control->get_pos_target_cm().xy().tofloat(); + const Vector2f error = (target_pos - curr_pos) * 0.01; mavlink_msg_nav_controller_output_send( chan, targets.x * 0.01, targets.y * 0.01, targets.z * 0.01, - wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0, - wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination() * 0.01, UINT16_MAX) : 0, + degrees(error.angle()), + error.length(), (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0, plane.airspeed_error * 100, // incorrect units; see PR#7933 - wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0); + quadplane.wp_nav->crosstrack_error()); return; } #endif diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 7cf1cd3539d..e20f3f76b1d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3436,12 +3436,6 @@ bool QuadPlane::using_wp_nav(void) const if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) { return true; } - if (plane.control_mode == &plane.mode_qrtl && poscontrol.get_state() >= QPOS_POSITION2) { - return true; - } - if (plane.control_mode == &plane.mode_auto && poscontrol.get_state() > QPOS_APPROACH) { - return true; - } return false; } From 18782c499042f0b2ed89fafc15854be7c131e82f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 9 Jan 2022 11:20:46 +1100 Subject: [PATCH 0189/3101] SITL: added ship offset and ATTITUDE --- libraries/SITL/SIM_Ship.cpp | 18 ++++++++++++++++++ libraries/SITL/SIM_Ship.h | 1 + 2 files changed, 19 insertions(+) diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index 3e097f16a8b..acd4bd376e1 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -41,6 +41,7 @@ const AP_Param::GroupInfo ShipSim::var_info[] = { AP_GROUPINFO("PSIZE", 3, ShipSim, path_size, 1000), AP_GROUPINFO("SYSID", 4, ShipSim, sys_id, 17), AP_GROUPINFO("DSIZE", 5, ShipSim, deck_size, 10), + AP_GROUPINFO("OFS", 7, ShipSim, offset, 0), AP_GROUPEND }; @@ -117,6 +118,10 @@ void ShipSim::update(void) if (home.lat == 0 && home.lng == 0) { return; } + const Vector3f &ofs = offset.get(); + home.offset(ofs.x, ofs.y); + home.alt -= ofs.z*100; + initialised = true; ::printf("ShipSim home %f %f\n", home.lat*1.0e-7, home.lng*1.0e-7); ship.sim = this; @@ -208,6 +213,19 @@ void ShipSim::send_report(void) if (len > 0) { mav_socket.send(buf, len); } + + // also set ATTITUDE so MissionPlanner can display ship orientation + mavlink_msg_attitude_pack_chan(sys_id, + component_id, + mavlink_ch, + &msg, + now, + 0, 0, radians(ship.heading_deg), + 0, 0, ship.yaw_rate); + len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } } #endif diff --git a/libraries/SITL/SIM_Ship.h b/libraries/SITL/SIM_Ship.h index 16e97152a65..2989ffd5972 100644 --- a/libraries/SITL/SIM_Ship.h +++ b/libraries/SITL/SIM_Ship.h @@ -71,6 +71,7 @@ class ShipSim { AP_Float path_size; AP_Float deck_size; AP_Int8 sys_id; + AP_Vector3f offset; Location home; const char *target_address = "127.0.0.1"; From 711ecb45ab620c6e7ff2867fec88a4a186f939c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Jan 2022 14:37:05 +1100 Subject: [PATCH 0190/3101] AP_Vehicle: added update_target_location() --- libraries/AP_Vehicle/AP_Vehicle.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 4df022eb8b3..8cc1a3ed7a1 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -210,6 +210,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // get target location (for use by scripting) virtual bool get_target_location(Location& target_loc) { return false; } + virtual bool update_target_location(const Location &old_loc, const Location &new_loc) { return false; } // circle mode controls (only used by scripting with Copter) virtual bool get_circle_radius(float &radius_m) { return false; } @@ -225,6 +226,9 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) { return false; } virtual void nav_script_time_done(uint16_t id) {} + // allow for VTOL velocity matching of a target + virtual bool set_velocity_match(const Vector2f &velocity) { return false; } + // control outputs enumeration enum class ControlOutput { From 197ccda4083a6c2a4e02a1e6702a6fbebf1375f2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Jan 2022 14:46:16 +1100 Subject: [PATCH 0191/3101] AP_Scripting: added follow API --- .../generator/description/bindings.desc | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 0bfb3ec4f3a..b8a2321a500 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -123,6 +123,7 @@ userdata Vector3f method dot float Vector3f userdata Vector3f method cross Vector3f Vector3f userdata Vector3f method scale Vector3f float'skip_check userdata Vector3f method copy Vector3f +userdata Vector3f method xy Vector2f userdata Vector2f field x float'skip_check read write userdata Vector2f field y float'skip_check read write @@ -131,6 +132,7 @@ userdata Vector2f method normalize void userdata Vector2f method is_nan boolean userdata Vector2f method is_inf boolean userdata Vector2f method is_zero boolean +userdata Vector2f method angle float userdata Vector2f method rotate void float'skip_check userdata Vector2f operator + userdata Vector2f operator - @@ -211,6 +213,7 @@ singleton AP_Vehicle method get_control_output boolean AP_Vehicle::ControlOutput singleton AP_Vehicle method start_takeoff boolean float (-LOCATION_ALT_MAX_M*100+1) (LOCATION_ALT_MAX_M*100-1) singleton AP_Vehicle method set_target_location boolean Location singleton AP_Vehicle method get_target_location boolean Location'Null +singleton AP_Vehicle method update_target_location boolean Location Location singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -360 +360 boolean float'skip_check boolean boolean singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean @@ -229,7 +232,7 @@ singleton AP_Vehicle method nav_script_time_done void uint16_t'skip_check singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 float'skip_check float'skip_check float'skip_check singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check singleton AP_Vehicle method nav_scripting_enable boolean uint8_t 0 UINT8_MAX - +singleton AP_Vehicle method set_velocity_match boolean Vector2f include AP_SerialLED/AP_SerialLED.h singleton AP_SerialLED alias serialLED @@ -489,3 +492,12 @@ singleton AP::fwversion() field patch uint8_t read singleton AP::fwversion() field fw_hash_str string read singleton AP::fwversion() field fw_hash_str alias hash +include AP_Follow/AP_Follow.h +singleton AP_Follow depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI +singleton AP_Follow alias follow +singleton AP_Follow method have_target boolean +singleton AP_Follow method get_last_update_ms uint32_t +singleton AP_Follow method get_target_location_and_velocity boolean Location'Null Vector3f'Null +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 + From 5c94be1d727819fc8a8bd48257fbcc69efea2238 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 9 Jan 2022 11:02:23 +1100 Subject: [PATCH 0192/3101] AP_Scripting: added rotate_xy for Vector3f --- libraries/AP_Scripting/generator/description/bindings.desc | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index b8a2321a500..dfa2c4d283f 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -124,6 +124,7 @@ userdata Vector3f method cross Vector3f Vector3f userdata Vector3f method scale Vector3f float'skip_check userdata Vector3f method copy Vector3f userdata Vector3f method xy Vector2f +userdata Vector3f method rotate_xy void float'skip_check userdata Vector2f field x float'skip_check read write userdata Vector2f field y float'skip_check read write From 3dca7f08b78c79916f43882d30691d5ce080a062 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Jan 2022 17:32:03 +1100 Subject: [PATCH 0193/3101] AP_Scripting: plane ship landing script --- .../examples/plane_ship_landing.lua | 439 ++++++++++++++++++ 1 file changed, 439 insertions(+) create mode 100644 libraries/AP_Scripting/examples/plane_ship_landing.lua diff --git a/libraries/AP_Scripting/examples/plane_ship_landing.lua b/libraries/AP_Scripting/examples/plane_ship_landing.lua new file mode 100644 index 00000000000..88209772776 --- /dev/null +++ b/libraries/AP_Scripting/examples/plane_ship_landing.lua @@ -0,0 +1,439 @@ +-- support takeoff and landing on moving platforms for VTOL planes + +local PARAM_TABLE_KEY = 7 +local PARAM_TABLE_PREFIX = "SHIP_" + +local MODE_MANUAL = 0 +local MODE_RTL = 11 +local MODE_QRTL = 21 +local MODE_AUTO = 10 +local MODE_QLOITER = 19 + +local NAV_TAKEOFF = 22 +local NAV_VTOL_TAKEOFF = 84 + +local ALT_FRAME_ABSOLUTE = 0 + +-- 3 throttle position +local THROTTLE_LOW = 0 +local THROTTLE_MID = 1 +local THROTTLE_HIGH = 2 + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +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 SHIP specific parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), 'could not add param table') +SHIP_ENABLE = bind_add_param('ENABLE', 1, 0) +SHIP_LAND_ANGLE = bind_add_param('LAND_ANGLE', 2, 0) +SHIP_AUTO_OFS = bind_add_param('AUTO_OFS', 3, 0) + +-- other parameters +RCMAP_THROTTLE = bind_param("RCMAP_THROTTLE") +ALT_HOLD_RTL = bind_param("ALT_HOLD_RTL") +Q_RTL_ALT = bind_param("Q_RTL_ALT") +TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM") +TECS_LAND_ARSPD = bind_param("TECS_LAND_ARSPD") +Q_TRANS_DECEL = bind_param("Q_TRANS_DECEL") +WP_LOITER_RAD = bind_param("WP_LOITER_RAD") +FOLL_OFS_X = bind_param("FOLL_OFS_X") +FOLL_OFS_Y = bind_param("FOLL_OFS_Y") +FOLL_OFS_Z = bind_param("FOLL_OFS_Z") + +-- an auth ID to disallow arming when we don't have the beacon +local auth_id = arming:get_aux_auth_id() +arming:set_aux_auth_failed(auth_id, "Ship: no beacon") + +-- current target +local target_pos = Location() +local current_pos = Location() +local target_velocity = Vector3f() +local target_heading = 0.0 + +-- landing stages +local STAGE_HOLDOFF = 0 +local STAGE_DESCEND = 1 +local STAGE_APPROACH = 2 +local STAGE_IDLE = 2 +local landing_stage = STAGE_HOLDOFF + +-- other state +local vehicle_mode = MODE_MANUAL +local reached_alt = false +local throttle_pos = THROTTLE_HIGH +local have_target = false + +-- square a variable +function sq(v) + return v*v +end + +-- return time since boot in seconds +function time_seconds() + return millis():tofloat() * 0.001 +end + +--[[ + parameter values which are auto-set on startup +--]] +local key_params = { + FOLL_ENABLE = 1, + FOLL_OFS_TYPE = 1, + FOLL_ALT_TYPE = 0, +} + +-- check key parameters +function check_parameters() + for p, v in pairs(key_params) do + local current = param:get(p) + if not current then + gcs:send_text(0,string.format("Parameter %s not found", p)) + end + 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)) + end + end +end + +-- update the pilots throttle position +function update_throttle_pos() + local tpos + if not rc:has_valid_input() then + tpos = THROTTLE_LOW + else + local tchan = rc:get_channel(RCMAP_THROTTLE:get()) + local tval = (tchan:norm_input_ignore_trim()+1.0)*0.5 + if tval >= 0.40 then + tpos = THROTTLE_HIGH + elseif tval >= 0.1 then + tpos = THROTTLE_MID + else + tpos = THROTTLE_LOW + end + end + if tpos ~= throttle_pos then + 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)", + 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") + landing_stage = STAGE_HOLDOFF + end + end + throttle_pos = tpos +end + +-- get landing airspeed +function get_land_airspeed() + if TECS_LAND_ARSPD:get() < 0 then + return TRIM_ARSPD_CM:get() * 0.01 + end + return TECS_LAND_ARSPD:get() +end + +--[[ + calculate stopping distance assuming we are flying at + TECS_LAND_ARSPD and are approaching the landing target from + behind. Take account of the wind estimate to get approach + groundspeed +--]] +function stopping_distance() + -- get the target true airspeed for approach + local tas = get_land_airspeed() * ahrs:get_EAS2TAS() + + -- add in wind in direction of flight + local wind = ahrs:wind_estimate():xy() + + -- rotate wind to be in approach frame + wind:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get())) + + -- ship velocity rotated to the approach frame + local ship2d = target_velocity:xy() + ship2d:rotate(-math.rad(target_heading + SHIP_LAND_ANGLE:get())) + + -- calculate closing speed + -- use pythagoras theorem to solve for the wind triangle + local tas_sq = sq(tas) + local y_sq = sq(wind:y()) + local closing_speed + if tas_sq >= y_sq then + closing_speed = math.sqrt(tas_sq - y_sq) + else + -- min 1 m/s + closing_speed = 1.0 + end + + -- include the wind in the direction of the ship + closing_speed = closing_speed + wind:x() + + -- account for the ship velocity + closing_speed = closing_speed - ship2d:x() + + -- calculate stopping distance + return sq(closing_speed) / (2.0 * Q_TRANS_DECEL:get()) +end + +-- get holdoff distance +function get_holdoff_distance() + local radius = WP_LOITER_RAD:get() + local holdoff_dist = math.abs(radius*1.5) + local stop_distance = stopping_distance() + + -- increase holdoff distance by up to 50% to ensure we can stop + holdoff_dist = math.max(holdoff_dist, math.min(holdoff_dist*2.5, stop_distance*2)) + return holdoff_dist +end + +-- get the holdoff position +function get_holdoff_position() + local radius = WP_LOITER_RAD:get() + local heading_deg = target_heading + SHIP_LAND_ANGLE:get() + local holdoff_dist = get_holdoff_distance() + + local ofs = Vector2f() + ofs:x(-holdoff_dist) + ofs:y(radius) + ofs:rotate(math.rad(heading_deg)) + local target = target_pos:copy() + target:offset(ofs:x(), ofs:y()) + return target +end + +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +--[[ + check if we have reached the tangent to the landing location +--]] +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)) + landing_stage = STAGE_DESCEND + end + if reached_alt and landing_stage == STAGE_DESCEND then + -- go to approach stage when throttle is low, we are + -- pointing at the ship and have reached target alt. + -- Also require we are within 2.5 radius of the ship, and our heading is within 20 + -- degrees of the target heading + 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) + if (error1 < margin and + distance < 2.5*holdoff_dist and + distance > 0.7*holdoff_dist and + error2 < 2*margin) then + -- we are on the tangent, switch to QRTL + gcs:send_text(0, "Starting approach") + landing_stage = STAGE_APPROACH + vehicle:set_mode(MODE_QRTL) + end + end +end + +--[[ + check if we should abort a QRTL landing +--]] +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") + landing_stage = STAGE_HOLDOFF + vehicle:set_mode(MODE_RTL) + end +end + +-- update state based on vehicle mode +function update_mode() + local mode = vehicle:get_mode() + if mode == vehicle_mode then + return + end + vehicle_mode = mode + if mode == MODE_RTL then + landing_stage = STAGE_HOLDOFF + reached_alt = false + elseif mode ~= MODE_QRTL then + landing_stage = STAGE_IDLE + reached_alt = false + end +end + +-- update target state +function update_target() + if not follow:have_target() then + if have_target then + gcs:send_text(0,"Lost beacon") + arming:set_aux_auth_failed(auth_id, "Ship: no beacon") + end + have_target = false + end + if not have_target then + gcs:send_text(0,"Have beacon") + arming:set_aux_auth_passed(auth_id) + end + have_target = true + + target_pos, target_velocity = follow:get_target_location_and_velocity_ofs() + target_heading = follow:get_target_heading_deg() + -- zero vertical velocity to reduce impact of ship movement + target_velocity:z(0) +end + +-- get the alt target for holdoff, AMSL +function get_target_alt() + local base_alt = target_pos:alt() * 0.01 + if landing_stage == STAGE_HOLDOFF then + return base_alt + ALT_HOLD_RTL:get() * 0.01 + end + return base_alt + Q_RTL_ALT:get() +end + +function update_alt() + local alt = current_pos:alt() * 0.01 + local target_alt = get_target_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") + end + reached_alt = true + end + end +end + +--[[ + update automatic beacon offsets +--]] + +function update_auto_offset() + if arming:is_armed() or math.floor(SHIP_AUTO_OFS:get()) ~= 1 then + return + end + + -- get target without offsets applied + target_no_ofs, vel = follow:get_target_location_and_velocity() + + -- setup offsets so target location will be current location + 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())) + FOLL_OFS_X:set_and_save(new:x()) + FOLL_OFS_Y:set_and_save(new:y()) + FOLL_OFS_Z:set_and_save(new:z()) + + SHIP_AUTO_OFS:set_and_save(0) +end + +-- main update function +function update() + if SHIP_ENABLE:get() < 1 then + return + end + if not follow:have_target() then + return + end + + update_target() + if not have_target then + return + end + + current_pos = ahrs:get_position() + if not current_pos then + return + end + current_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + + update_throttle_pos() + update_mode() + update_alt() + update_auto_offset() + + 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 + + if vehicle_mode == MODE_RTL then + local holdoff_pos = get_holdoff_position() + holdoff_pos:change_alt_frame(ALT_FRAME_ABSOLUTE) + holdoff_pos:alt(math.floor(get_target_alt()*100)) + vehicle:update_target_location(next_WP, holdoff_pos) + + if throttle_pos == THROTTLE_LOW then + check_approach_tangent() + end + + elseif vehicle_mode == MODE_QRTL then + vehicle:set_velocity_match(target_velocity:xy()) + target_pos:alt(next_WP:alt()) + vehicle:update_target_location(next_WP, target_pos) + + if throttle_pos == THROTTLE_HIGH then + check_approach_abort() + end + + elseif vehicle_mode == MODE_AUTO then + local id = mission:get_current_nav_id() + if id == NAV_VTOL_TAKEOFF or id == NAV_TAKEOFF then + vehicle:set_velocity_match(target_velocity:xy()) + local tpos = current_pos:copy() + tpos:alt(next_WP:alt()) + vehicle:update_target_location(next_WP, tpos) + end + + elseif vehicle_mode == MODE_QLOITER then + vehicle:set_velocity_match(target_velocity:xy()) + end + +end + +function loop() + update() + -- run at 20Hz + return loop, 50 +end + +check_parameters() + +-- start running update loop +return loop() From b9d96ad14802c0aa1eaac24abc6544bda774ff0c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Mar 2022 14:25:54 +1100 Subject: [PATCH 0194/3101] Tools: update runplanetest.py added prearm check and cope with python3 --- Tools/scripts/runplanetest.py | 39 +++++++++++++++++++++++++++-------- 1 file changed, 30 insertions(+), 9 deletions(-) diff --git a/Tools/scripts/runplanetest.py b/Tools/scripts/runplanetest.py index d94a55a2435..4ceaa13bc16 100755 --- a/Tools/scripts/runplanetest.py +++ b/Tools/scripts/runplanetest.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import pexpect, time, sys from pymavlink import mavutil @@ -25,6 +25,20 @@ def wait_mode(mav, modes, timeout=10): print("Failed to get mode from %s" % modes) sys.exit(1) +def wait_prearm_ok(mav, timeout=30): + '''wait for pre-arm OK''' + start_time = time.time() + last_mode = None + while time.time() < start_time+timeout: + m = mav.recv_match(type='SYS_STATUS', blocking=True, timeout=2) + if m is None: + return + if m.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK != 0: + print("Prearm OK") + return + print("Failed to get pre-arm OK") + sys.exit(1) + def wait_time(mav, simtime): '''wait for simulation time to pass''' imu = mav.recv_match(type='RAW_IMU', blocking=True) @@ -36,17 +50,24 @@ def wait_time(mav, simtime): break cmd = '../Tools/autotest/sim_vehicle.py -D -f quadplane' -mavproxy = pexpect.spawn(cmd, logfile=sys.stdout, timeout=30) +mavproxy = pexpect.spawn(cmd, logfile=sys.stdout.buffer, timeout=30) mavproxy.expect("ArduPilot Ready") mav = mavutil.mavlink_connection('127.0.0.1:14550') mavproxy.send('speedup 40\n') -mavproxy.expect('using GPS') -mavproxy.expect('using GPS') -mavproxy.expect('using GPS') -mavproxy.expect('using GPS') -mavproxy.send('auto\n') +wait_prearm_ok(mav) +mavproxy.send('mode guided\n') +wait_mode(mav, ['GUIDED']) mavproxy.send('arm throttle\n') -wait_mode(mav, ['AUTO']) -mavproxy.expect("Mission: 5 WP") +mavproxy.send('takeoff 40\n') +wait_time(mav, 30) +mavproxy.send('mode cruise\n') +wait_mode(mav, ['CRUISE']) +wait_time(mav, 10) +mavproxy.send('mode qrtl\n') +wait_mode(mav, ['QRTL']) +mavproxy.send('module load console\n') +mavproxy.send('module load map\n') +mavproxy.logfile = None +mavproxy.interact() From 2864e21b31632df56a9cfb3b206077df882202ef Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Feb 2022 12:29:02 +1100 Subject: [PATCH 0195/3101] Plane: allow for a trans decel margin so if we are behind the velocity curve we are less likely to overshoot landing --- ArduPlane/quadplane.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e20f3f76b1d..3e5bcfe5a81 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2103,7 +2103,11 @@ void QuadPlane::run_xy_controller(void) if (in_vtol_land_sequence() && (poscontrol.get_state() == QPOS_POSITION1 || poscontrol.get_state() == QPOS_POSITION2)) { - accel_cmss = MAX(accel_cmss, transition_decel*100); + // we allow for a bit higher accel limit than the trans decel, + // so if are less likely to overshoot the landing point + // if at some stage in the POS1 we are under velocity + const float accel_margin = 1.25; + accel_cmss = MAX(accel_cmss, accel_margin*transition_decel*100); } const float speed_cms = wp_nav->get_default_speed_xy(); pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss); From f9ecf15973e883f0270c6a583ba49c954ed7c44b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Mar 2022 13:57:00 +1100 Subject: [PATCH 0196/3101] Plane: setup target accel in POSITION1 state --- ArduPlane/quadplane.cpp | 48 +++++++++++++++++++++++++---------------- ArduPlane/quadplane.h | 7 ++++-- 2 files changed, 35 insertions(+), 20 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3e5bcfe5a81..bbf09c362c7 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2097,17 +2097,12 @@ void QuadPlane::update_land_positioning(void) /* run (and possibly init) xy controller */ -void QuadPlane::run_xy_controller(void) +void QuadPlane::run_xy_controller(float accel_limit) { float accel_cmss = wp_nav->get_wp_acceleration(); - if (in_vtol_land_sequence() && - (poscontrol.get_state() == QPOS_POSITION1 || - poscontrol.get_state() == QPOS_POSITION2)) { - // we allow for a bit higher accel limit than the trans decel, - // so if are less likely to overshoot the landing point - // if at some stage in the POS1 we are under velocity - const float accel_margin = 1.25; - accel_cmss = MAX(accel_cmss, accel_margin*transition_decel*100); + if (is_positive(accel_limit)) { + // allow for accel limit override + accel_cmss = MAX(accel_cmss, accel_limit*100); } const float speed_cms = wp_nav->get_default_speed_xy(); pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss); @@ -2154,10 +2149,12 @@ void QuadPlane::poscontrol_init_approach(void) */ void QuadPlane::log_QPOS(void) { - AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist", "QBf", + AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc", "QBfff", AP_HAL::micros64(), poscontrol.get_state(), - plane.auto_state.wp_distance); + plane.auto_state.wp_distance, + poscontrol.target_speed, + poscontrol.target_accel); } /* @@ -2209,10 +2206,10 @@ void QuadPlane::vtol_position_controller(void) uint32_t now_ms = AP_HAL::millis(); // distance that we switch to QPOS_POSITION2 - const float position2_dist_threshold = 5.0; + const float position2_dist_threshold = 10.0; // target speed when we reach position2 threshold - const float position2_target_speed = 2.0; + const float position2_target_speed = 3.0; if (hal.util->get_soft_armed()) { poscontrol.last_run_ms = now_ms; @@ -2402,6 +2399,7 @@ void QuadPlane::vtol_position_controller(void) const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc); const float distance = diff_wp.length(); + const float groundspeed = plane.ahrs.groundspeed(); // calculate speed we should be at to reach the position2 // target speed at the position2 distance threshold, assuming @@ -2435,17 +2433,23 @@ void QuadPlane::vtol_position_controller(void) plane.nav_controller->update_waypoint(plane.current_loc, loc); Vector2f target_speed_xy_cms; + Vector2f target_accel_cms; + const float target_accel = accel_needed(distance, sq(groundspeed)); if (distance > 0.1) { - target_speed_xy_cms = diff_wp.normalized() * target_speed * 100; + Vector2f diff_wp_norm = diff_wp.normalized(); + target_speed_xy_cms = diff_wp_norm * target_speed * 100; + target_accel_cms = diff_wp_norm * (-target_accel*100); } target_speed_xy_cms += landing_velocity * 100; + poscontrol.target_speed = target_speed_xy_cms.length()*0.01; + poscontrol.target_accel = target_accel; // use input shaping and abide by accel and jerk limits - pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f()); + pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms); // run horizontal velocity controller - run_xy_controller(); + run_xy_controller(MAX(target_accel, transition_decel)*1.5); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -2485,7 +2489,7 @@ void QuadPlane::vtol_position_controller(void) update_land_positioning(); - run_xy_controller(); + run_xy_controller(transition_decel*1.5); // nav roll and pitch are controlled by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); @@ -3499,7 +3503,7 @@ bool QuadPlane::in_transition(void) const /* calculate current stopping distance for a quadplane in fixed wing flight */ -float QuadPlane::stopping_distance(float ground_speed_squared) +float QuadPlane::stopping_distance(float ground_speed_squared) const { // use v^2/(2*accel). This is only quite approximate as the drag // varies with pitch, but it gives something for the user to @@ -3507,6 +3511,14 @@ float QuadPlane::stopping_distance(float ground_speed_squared) return ground_speed_squared / (2 * transition_decel); } +/* + calculate acceleration needed to stop in the given distance given current speed + */ +float QuadPlane::accel_needed(float stop_distance, float ground_speed_squared) const +{ + return ground_speed_squared / (2 * stop_distance); +} + /* calculate current stopping distance for a quadplane in fixed wing flight */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 284c6f99dab..5ef4039123d 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -262,12 +262,13 @@ class QuadPlane void update_throttle_suppression(void); void run_z_controller(void); - void run_xy_controller(void); + void run_xy_controller(float accel_limit=0.0); void setup_defaults(void); // calculate a stopping distance for fixed-wing to vtol transitions - float stopping_distance(float ground_speed_squared); + float stopping_distance(float ground_speed_squared) const; + float accel_needed(float stop_distance, float ground_speed_squared) const; float stopping_distance(void); // distance below which we don't do approach, based on stopping @@ -446,6 +447,8 @@ class QuadPlane float pos1_start_speed; Vector2f velocity_match; uint32_t last_velocity_match_ms; + float target_speed; + float target_accel; private: uint32_t last_state_change_ms; enum position_control_state state; From 36596694095e3e4417192fbab10ad8d869525ddd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Mar 2022 14:25:32 +1100 Subject: [PATCH 0197/3101] Plane: cope with high angle error in airbrake state if we are flying too far off the target vector then exit airbrake state. This prevents flying for a long distance away from the landing point in airbrake mode --- ArduPlane/quadplane.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index bbf09c362c7..dc52d1d842b 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2337,6 +2337,7 @@ void QuadPlane::vtol_position_controller(void) if (poscontrol.get_state() == QPOS_AIRBRAKE && poscontrol.time_since_state_start_ms() > min_airbrake_ms && (aspeed < aspeed_threshold || + fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) { From f09c715d7aa6d3a54862d1e04dade33d84cf99de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 5 Mar 2022 08:23:04 +1100 Subject: [PATCH 0198/3101] Plane: wait till motors are fully up before takeoff in guided mode this allows for guided mode takeoff in tilt-rotors. Otherwise motors till be pointing forward and takeoff will go very badly --- ArduPlane/quadplane.cpp | 9 ++++++++- ArduPlane/tiltrotor.cpp | 11 +++++++++++ ArduPlane/tiltrotor.h | 1 + 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index dc52d1d842b..1a3a6eb697d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2697,6 +2697,13 @@ void QuadPlane::takeoff_controller(void) if (!hal.util->get_soft_armed()) { return; } + + if (plane.control_mode == &plane.mode_guided && guided_takeoff + && tiltrotor.enabled() && !tiltrotor.fully_up()) { + // waiting for motors to tilt up + return; + } + /* for takeoff we use the position controller */ @@ -2724,7 +2731,7 @@ void QuadPlane::takeoff_controller(void) get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds()); float vel_z = wp_nav->get_default_speed_up(); - if (guided_takeoff) { + if (plane.control_mode == &plane.mode_guided && guided_takeoff) { // for guided takeoff we aim for a specific height with zero // velocity at that height Location origin; diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 5a48b3b4da0..3f0660d1660 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -479,6 +479,17 @@ bool Tiltrotor::fully_fwd(void) const return (current_tilt >= get_fully_forward_tilt()); } +/* + return true if the rotors are fully tilted up + */ +bool Tiltrotor::fully_up(void) const +{ + if (!enabled() || (tilt_mask == 0)) { + return false; + } + return (current_tilt <= 0); +} + /* control vectoring for tilt multicopters */ diff --git a/ArduPlane/tiltrotor.h b/ArduPlane/tiltrotor.h index e6953ebf469..2467fc28cca 100644 --- a/ArduPlane/tiltrotor.h +++ b/ArduPlane/tiltrotor.h @@ -49,6 +49,7 @@ friend class Tiltrotor_Transition; } bool fully_fwd() const; + bool fully_up() const; float tilt_max_change(bool up, bool in_flap_range = false) const; float get_fully_forward_tilt() const; float get_forward_flight_tilt() const; From 3d9086b460a6b9880238cbafde01acb9b575ba0c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Mar 2022 14:06:46 +1100 Subject: [PATCH 0199/3101] AP_Scripting: update lua docs --- libraries/AP_Scripting/docs/docs.lua | 67 ++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 073c2c291b5..ef8f2a93387 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -388,6 +388,9 @@ function Vector2f_ud:normalize() end ---@return number function Vector2f_ud:length() end +-- desc +---@return number +function Vector2f_ud:angle() end -- desc ---@class Vector3f_ud @@ -458,6 +461,14 @@ function Vector3f_ud:normalize() end ---@return number function Vector3f_ud:length() end +-- desc +---@param param1 number -- XY rotation in radians +function Vector3f_ud:rotate_xy(param1) end + +-- desc +---@return Vector2f_ud +function Vector3f_ud:xy() end + -- desc ---@class Location_ud @@ -1309,6 +1320,12 @@ function vehicle:set_target_posvel_NED(target_pos, target_vel) end ---@return boolean function vehicle:set_target_pos_NED(target_pos, use_yaw, yaw_deg, use_yaw_rate, yaw_rate_degs, yaw_relative, terrain_alt) end +-- desc +---@param param1 Location_ud -- current target +---@param param2 Location_ud -- new target +---@return boolean +function vehicle:update_target_location(param1, param2) end + -- desc ---@return Location_ud|nil function vehicle:get_target_location() end @@ -1357,6 +1374,32 @@ function vehicle:get_mode() end ---@return boolean function vehicle:set_mode(mode_number) end +-- desc +---@param param1 Vector2f_ud +---@return boolean +function vehicle:set_velocity_match(param1) end + +-- desc +---@param param1 integer +---@return boolean +function vehicle:nav_scripting_enable(param1) end + +-- desc +---@param param1 number +---@param param2 number +---@return boolean +function vehicle:set_desired_turn_rate_and_speed(param1, param2) end + +-- desc +---@param param1 number -- throttle percent +---@param param2 number -- roll rate deg/s +---@param param3 number -- pitch rate deg/s +---@param param4 number -- yaw rate deg/s +function vehicle:set_target_throttle_rate_rpy(param1, param2, param3, param4) end + +-- desc +---@param param1 integer +function vehicle:nav_script_time_done(param1) end -- desc ---@class onvif @@ -1904,4 +1947,28 @@ function ahrs:get_pitch() end ---@return number function ahrs:get_roll() end +-- desc +---@class follow +follow = {} + +-- desc +---@return number|nil +function follow:get_target_heading_deg() end + +-- desc +---@return Location_ud|nil +---@return Vector3f_ud|nil +function follow:get_target_location_and_velocity_ofs() end +-- desc +---@return Location_ud|nil +---@return Vector3f_ud|nil +function follow:get_target_location_and_velocity() end + +-- desc +---@return uint32_t_ud +function follow:get_last_update_ms() end + +-- desc +---@return boolean +function follow:have_target() end From 8973519232762b04ac45466c3cad2621b2d596f5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 08:35:25 +1100 Subject: [PATCH 0200/3101] AP_Common: improved accuracy of get_bearing() make base function ftype, then convert to int32_t for get_bearing_to() --- libraries/AP_Common/Location.cpp | 8 ++++---- libraries/AP_Common/Location.h | 11 +++++++---- libraries/AP_Common/tests/test_location.cpp | 2 +- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 0ba5e4a9a58..6594c95623e 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -376,14 +376,14 @@ bool Location::sanitize(const Location &defaultLoc) assert_storage_size _assert_storage_size_Location; -// return bearing in centi-degrees from location to loc2 -int32_t Location::get_bearing_to(const struct Location &loc2) const +// return bearing in radians from location to loc2, return is 0 to 2*Pi +ftype Location::get_bearing(const struct Location &loc2) const { const int32_t off_x = diff_longitude(loc2.lng,lng); const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2); - int32_t bearing = 9000 + atan2F(-off_y, off_x) * DEGX100; + ftype bearing = (M_PI*0.5) + atan2F(-off_y, off_x); if (bearing < 0) { - bearing += 36000; + bearing += 2*M_PI; } return bearing; } diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index d9b2565b98d..e444b6b22a2 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -92,10 +92,13 @@ class Location void zero(void); - // return bearing in centi-degrees from location to loc2 - int32_t get_bearing_to(const struct Location &loc2) const; - // return the bearing in radians - ftype get_bearing(const struct Location &loc2) const { return radians(get_bearing_to(loc2) * 0.01); } ; + // return the bearing in radians, from 0 to 2*Pi + ftype get_bearing(const struct Location &loc2) const; + + // return bearing in centi-degrees from location to loc2, return is 0 to 35999 + int32_t get_bearing_to(const struct Location &loc2) const { + return int32_t(get_bearing(loc2) * DEGX100 + 0.5); + } // check if lat and lng match. Ignore altitude and options bool same_latlon_as(const Location &loc2) const; diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index 399806b9e65..fa29927a567 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -311,7 +311,7 @@ TEST(Location, Distance) bearing = test_home.get_bearing_to(test_loc); EXPECT_EQ(31503, bearing); const float bearing_rad = test_home.get_bearing(test_loc); - EXPECT_FLOAT_EQ(radians(315.03), bearing_rad); + EXPECT_FLOAT_EQ(5.4982867, bearing_rad); } From c658f170cb7237889fddcb6f460a0f71b7324285 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Mar 2022 08:35:39 +1100 Subject: [PATCH 0201/3101] SITL: fixed ship offset velocity correction --- libraries/SITL/SIM_Ship.cpp | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index acd4bd376e1..c5807a31df5 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -95,8 +95,24 @@ Vector2f ShipSim::get_ground_speed_adjustment(const Location &loc, float &yaw_ra yaw_rate = 0; return Vector2f(0,0); } - Vector2f vel(ship.speed, 0); - vel.rotate(radians(ship.heading_deg)); + + // find center of the circle that the ship is on + Location center = shiploc; + const float path_radius = path_size.get()*0.5; + center.offset_bearing(ship.heading_deg+(ship.yaw_rate>0?90:-90), path_radius); + + // scale speed for ratio of distances + const float p = center.get_distance(loc) / path_radius; + const float scaled_speed = ship.speed * p; + + // work out how far around the circle ahead or behind we are for + // rotating velocity + const float bearing1 = center.get_bearing(loc); + const float bearing2 = center.get_bearing(shiploc); + const float heading = ship.heading_deg + degrees(bearing1-bearing2); + + Vector2f vel(scaled_speed, 0); + vel.rotate(radians(heading)); yaw_rate = ship.yaw_rate; return vel; } From cc9b9813cbc1c12c8e740702195c220b89b85a0b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 13:16:32 +1100 Subject: [PATCH 0202/3101] Plane: review fixes thanks Pete! --- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/Log.cpp | 2 +- ArduPlane/mode.cpp | 4 ++++ ArduPlane/quadplane.cpp | 28 +++++++++++++++++++--------- ArduPlane/quadplane.h | 3 +++ 5 files changed, 28 insertions(+), 11 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index e913678acb9..4ac15d383f2 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -185,7 +185,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const targets.y * 0.01, targets.z * 0.01, degrees(error.angle()), - error.length(), + MIN(error.length(), UINT16_MAX), (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0, plane.airspeed_error * 100, // incorrect units; see PR#7933 quadplane.wp_nav->crosstrack_error()); diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 7904d674003..6b7f698e254 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -23,7 +23,7 @@ void Plane::Log_Write_Attitude(void) } else { ahrs.Write_Attitude(targets); } - if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight() || quadplane.in_vtol_airbrake()) { + if (AP_HAL::millis() - quadplane.last_att_control_ms < 100) { // log quadplane PIDs separately from fixed wing PIDs logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index f4ca11a06c6..3042a939569 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -80,6 +80,10 @@ bool Mode::enter() // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands plane.new_airspeed_cm = -1; +#if HAL_QUADPLANE_ENABLED + quadplane.mode_enter(); +#endif + bool enter_result = _enter(); if (enter_result) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 1a3a6eb697d..c6a8e80da61 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2400,7 +2400,8 @@ void QuadPlane::vtol_position_controller(void) const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc); const float distance = diff_wp.length(); - const float groundspeed = plane.ahrs.groundspeed(); + const Vector2f rel_groundspeed_vector = landing_closing_velocity(); + const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared(); // calculate speed we should be at to reach the position2 // target speed at the position2 distance threshold, assuming @@ -2411,14 +2412,13 @@ void QuadPlane::vtol_position_controller(void) // maximum configured VTOL speed const float wp_speed = wp_nav->get_default_speed_xy() * 0.01; - const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared(); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); // limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed); if (poscontrol.reached_wp_speed || - current_speed_sq < sq(wp_speed) || + rel_groundspeed_sq < sq(wp_speed) || wp_speed > 1.35*scaled_wp_speed) { // once we get below the Q_WP_SPEED then we don't want to // speed up again. At that point we should fly within the @@ -2435,7 +2435,7 @@ void QuadPlane::vtol_position_controller(void) Vector2f target_speed_xy_cms; Vector2f target_accel_cms; - const float target_accel = accel_needed(distance, sq(groundspeed)); + const float target_accel = accel_needed(distance, rel_groundspeed_sq); if (distance > 0.1) { Vector2f diff_wp_norm = diff_wp.normalized(); target_speed_xy_cms = diff_wp_norm * target_speed * 100; @@ -3722,14 +3722,16 @@ bool QuadPlane::use_fw_attitude_controllers(void) const } /* - calculate our closing velocity vector on the landing point. In the - future this will take account of the landing point having a - velocity + calculate our closing velocity vector on the landing point, taking + into account target velocity */ Vector2f QuadPlane::landing_closing_velocity() { - Vector2f vel = ahrs.groundspeed_vector(); - return vel; + Vector2f landing_velocity; + if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) { + landing_velocity = poscontrol.velocity_match; + } + return ahrs.groundspeed_vector() - landing_velocity; } /* @@ -3927,4 +3929,12 @@ bool QuadPlane::in_vtol_takeoff(void) const return false; } +// called when we change mode (for any mode, not just Q modes) +void QuadPlane::mode_enter(void) +{ + poscontrol.xy_correction.zero(); + poscontrol.velocity_match.zero(); + poscontrol.last_velocity_match_ms = 0; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5ef4039123d..bad2f79544c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -164,6 +164,9 @@ class QuadPlane }; void set_q_assist_state(Q_ASSIST_STATE_ENUM state) {q_assist_state = state;}; + // called when we change mode (for any mode, not just Q modes) + void mode_enter(void); + private: AP_AHRS &ahrs; AP_Vehicle::MultiCopter aparm; From ba57e0a9d8174ed0b27aca1770f574bc8fdf1a2c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 13:16:50 +1100 Subject: [PATCH 0203/3101] AP_Scripting: review fixes --- libraries/AP_Scripting/docs/docs.lua | 6 +-- .../examples/plane_ship_landing.lua | 44 +++++++++++-------- .../generator/description/bindings.desc | 2 +- 3 files changed, 30 insertions(+), 22 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index ef8f2a93387..77b5e2c78c9 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1321,10 +1321,10 @@ function vehicle:set_target_posvel_NED(target_pos, target_vel) end function vehicle:set_target_pos_NED(target_pos, use_yaw, yaw_deg, use_yaw_rate, yaw_rate_degs, yaw_relative, terrain_alt) end -- desc ----@param param1 Location_ud -- current target ----@param param2 Location_ud -- new target +---@param current_target Location_ud -- current target, from get_target_location() +---@param new_target Location_ud -- new target ---@return boolean -function vehicle:update_target_location(param1, param2) end +function vehicle:update_target_location(current_target, new_target) end -- desc ---@return Location_ud|nil diff --git a/libraries/AP_Scripting/examples/plane_ship_landing.lua b/libraries/AP_Scripting/examples/plane_ship_landing.lua index 88209772776..fe4d3f7ade7 100644 --- a/libraries/AP_Scripting/examples/plane_ship_landing.lua +++ b/libraries/AP_Scripting/examples/plane_ship_landing.lua @@ -78,27 +78,20 @@ function sq(v) return v*v end --- return time since boot in seconds -function time_seconds() - return millis():tofloat() * 0.001 -end - ---[[ - parameter values which are auto-set on startup ---]] -local key_params = { - FOLL_ENABLE = 1, - FOLL_OFS_TYPE = 1, - FOLL_ALT_TYPE = 0, -} - -- check key parameters function check_parameters() + --[[ + parameter values which are auto-set on startup + --]] + local key_params = { + FOLL_ENABLE = 1, + FOLL_OFS_TYPE = 1, + FOLL_ALT_TYPE = 0, + } + for p, v in pairs(key_params) do local current = param:get(p) - if not current then - gcs:send_text(0,string.format("Parameter %s not found", p)) - end + 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)) @@ -435,5 +428,20 @@ end check_parameters() +-- 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, 50 +end + -- start running update loop -return loop() +return protected_wrapper() + diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index dfa2c4d283f..240b3e6cd06 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -493,7 +493,7 @@ singleton AP::fwversion() field patch uint8_t read singleton AP::fwversion() field fw_hash_str string read singleton AP::fwversion() field fw_hash_str alias hash -include AP_Follow/AP_Follow.h +include AP_Follow/AP_Follow.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AP_Follow depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AP_Follow alias follow singleton AP_Follow method have_target boolean From 3f0110e924f85ef0f4acdfe3227f3fd6d6d0c9b0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 15:17:27 +1100 Subject: [PATCH 0204/3101] AP_Scripting: fixed beacon lost in ship landing --- libraries/AP_Scripting/examples/plane_ship_landing.lua | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Scripting/examples/plane_ship_landing.lua b/libraries/AP_Scripting/examples/plane_ship_landing.lua index fe4d3f7ade7..8db18d24754 100644 --- a/libraries/AP_Scripting/examples/plane_ship_landing.lua +++ b/libraries/AP_Scripting/examples/plane_ship_landing.lua @@ -294,6 +294,7 @@ function update_target() 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") @@ -358,9 +359,6 @@ function update() if SHIP_ENABLE:get() < 1 then return end - if not follow:have_target() then - return - end update_target() if not have_target then From 5e14b9efe632d4e11089a71d4caa5ea87b2dde92 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 16:56:47 +1100 Subject: [PATCH 0205/3101] Plane: use pos control for most of LAND_FINAL this gives more accurate landing with some velocity drift --- ArduPlane/quadplane.cpp | 21 +++++++++++++++++++-- ArduPlane/quadplane.h | 1 + 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c6a8e80da61..b48a4316c82 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2179,6 +2179,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) } else if (s == QPOS_LAND_DESCEND) { // reset throttle descent control qp.thr_ctrl_land = false; + } else if (s == QPOS_LAND_FINAL) { + // remember last pos reset to handle GPS glitch in LAND_FINAL + Vector2f rpos; + last_pos_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos); } // double log to capture the state change qp.log_QPOS(); @@ -2514,10 +2518,23 @@ void QuadPlane::vtol_position_controller(void) if (should_relax()) { pos_control->relax_velocity_controller_xy(); } else { - // we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling Vector2f zero; Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100; - pos_control->input_vel_accel_xy(vel_cms, zero); + Vector2f rpos; + const uint32_t last_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos); + /* we use velocity control when we may be touching the + ground or if we've had a position reset from AHRS. This + helps us handle a GPS glitch in the final land phase, + and also prevents trying to reposition after touchdown + */ + if (motors->limit.throttle_lower || + motors->get_throttle() < 0.5*motors->get_throttle_hover() || + last_reset_ms != poscontrol.last_pos_reset_ms) { + pos_control->input_vel_accel_xy(vel_cms, zero); + } else { + // otherwise use full pos control + pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero); + } } run_xy_controller(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index bad2f79544c..68b3ad50350 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -452,6 +452,7 @@ class QuadPlane uint32_t last_velocity_match_ms; float target_speed; float target_accel; + uint32_t last_pos_reset_ms; private: uint32_t last_state_change_ms; enum position_control_state state; From cdd63a239efae40087c0901f00694590da298d1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Mar 2022 16:57:15 +1100 Subject: [PATCH 0206/3101] AP_Scripting: fixed radius selection for ship landing --- .../AP_Scripting/examples/plane_ship_landing.lua | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/examples/plane_ship_landing.lua b/libraries/AP_Scripting/examples/plane_ship_landing.lua index 8db18d24754..d725616fd6d 100644 --- a/libraries/AP_Scripting/examples/plane_ship_landing.lua +++ b/libraries/AP_Scripting/examples/plane_ship_landing.lua @@ -46,6 +46,7 @@ TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM") TECS_LAND_ARSPD = bind_param("TECS_LAND_ARSPD") Q_TRANS_DECEL = bind_param("Q_TRANS_DECEL") WP_LOITER_RAD = bind_param("WP_LOITER_RAD") +RTL_RADIUS = bind_param("RTL_RADIUS") FOLL_OFS_X = bind_param("FOLL_OFS_X") FOLL_OFS_Y = bind_param("FOLL_OFS_Y") FOLL_OFS_Z = bind_param("FOLL_OFS_Z") @@ -180,9 +181,17 @@ function stopping_distance() return sq(closing_speed) / (2.0 * Q_TRANS_DECEL:get()) end +-- get holdoff distance +function get_holdoff_radius() + if RTL_RADIUS:get() ~= 0 then + return RTL_RADIUS:get() + end + return WP_LOITER_RAD:get() +end + -- get holdoff distance function get_holdoff_distance() - local radius = WP_LOITER_RAD:get() + local radius = get_holdoff_radius() local holdoff_dist = math.abs(radius*1.5) local stop_distance = stopping_distance() @@ -193,7 +202,7 @@ end -- get the holdoff position function get_holdoff_position() - local radius = WP_LOITER_RAD:get() + local radius = get_holdoff_radius() local heading_deg = target_heading + SHIP_LAND_ANGLE:get() local holdoff_dist = get_holdoff_distance() From 228796d55be293a975c2a19ebf4ce0de00278ae6 Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Tue, 8 Mar 2022 13:38:38 +0530 Subject: [PATCH 0207/3101] AP_HAL_ChibiOS: Add Sierra-F9P support --- .../hwdef/Sierra-F9P/hwdef-bl.dat | 80 +++++++++ .../AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat | 162 ++++++++++++++++++ 2 files changed, 242 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef-bl.dat new file mode 100644 index 00000000000..927d5bda729 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef-bl.dat @@ -0,0 +1,80 @@ +# hw definition file for processing by chibios_pins.py + +# Sierra-F9P bootloader + +# MCU class and specific type +MCU STM32F4xx STM32F412Rx + +FLASH_RESERVE_START_KB 0 +# two sectors for bootloader, two for storage +FLASH_BOOTLOADER_LOAD_KB 64 + +# board ID for firmware load +APJ_BOARD_ID 1034 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +define CH_CFG_ST_FREQUENCY 1000000 + +# Available Flash +FLASH_SIZE_KB 1024 + +define HAL_STORAGE_SIZE 16384 +define STORAGE_FLASH_PAGE 1 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +# order of UARTs +SERIAL_ORDER + +# USART1 +PA9 USART1_TX USART1 +PA10 USART1_RX USART1 + +# USART2 +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD +define HAL_USE_SERIAL TRUE + +define STM32_SERIAL_USE_USART1 TRUE +define STM32_SERIAL_USE_USART2 TRUE +define STM32_SERIAL_USE_USART3 FALSE + +define HAL_NO_GPIO_IRQ +define HAL_USE_EMPTY_IO TRUE + +define DMA_RESERVE_SIZE 0 + +define HAL_DISABLE_LOOP_DELAY + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +PC1 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +define HAL_USE_CAN TRUE +define STM32_CAN_USE_CAN1 TRUE + +define CAN_APP_NODE_NAME "org.ardupilot.Sierra-F9P" + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a small bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 1000 + +# Add CS pins to ensure they are high in bootloader +#PC6 ICM_CS CS +PC12 BARO_CS CS + +#Sensors Enable & ESP Enable +PB0 VDD_3V3_SENSORS_EN OUTPUT HIGH +PC2 ESP_PWR_EN OUTPUT LOW diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat new file mode 100644 index 00000000000..a709ddc0feb --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat @@ -0,0 +1,162 @@ +# hw definition file for processing by chibios_pins.py + +# Sierra-F9P Firmware + +# MCU class and specific type +MCU STM32F4xx STM32F412Rx + +# bootloader starts firmware at 64k +FLASH_RESERVE_START_KB 64 + +# store parameters in pages 2 and 3 +STORAGE_FLASH_PAGE 2 +define HAL_STORAGE_SIZE 8192 + +# board ID for firmware load +APJ_BOARD_ID 1034 + +# setup build for a peripheral firmware +env AP_PERIPH 1 +#env AP_PERIPH_HEAVY 1 + +STM32_ST_USE_TIMER 5 + +# enable watchdog +define HAL_WATCHDOG_ENABLED_DEFAULT true + +# crystal frequency +OSCILLATOR_HZ 16000000 + +define CH_CFG_ST_FREQUENCY 1000000 + +# Available Flash +FLASH_SIZE_KB 1024 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +# order of UARTs +SERIAL_ORDER USART1 EMPTY EMPTY USART2 + +# USART1 for debug +PA9 USART1_TX USART1 NODMA +PA10 USART1_RX USART1 NODMA +define HAL_SERIAL0_BAUD_DEFAULT 57600 + +# USART2 for GPS +PA2 USART2_TX USART2 SPEED_HIGH +PA3 USART2_RX USART2 SPEED_HIGH + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# only one I2C bus in normal config +PB7 I2C1_SDA I2C1 +PB6 I2C1_SCL I2C1 + +define HAL_USE_I2C TRUE +define STM32_I2C_USE_I2C1 TRUE +define HAL_I2C_CLEAR_ON_TIMEOUT 0 +define HAL_I2C_INTERNAL_MASK 0 + +# only one I2C bus +I2C_ORDER I2C1 + +# only one SPI bus in normal config +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI CS +#PC6 ICM_CS CS +PC12 BARO_CS CS + +# SPI devices +# ToDo SPI devices +#SPIDEV icm20948 SPI1 DEVID1 MPU_CS MODE3 4*MHZ 8*MHZ +SPIDEV dps310 SPI1 DEVID3 BARO_CS MODE3 5*MHZ 5*MHZ + +# Compass +COMPASS QMC5883L I2C:0:0xd false ROTATION_PITCH_180_YAW_90 + +# Baro +BARO DPS310 SPI:dps310 + +# PWM output for buzzer +PB5 TIM3_CH2 TIM3 GPIO(77) LOW ALARM + +# WS2812 LEDs +PA15 TIM2_CH1 TIM2 PWM(1) + +define HAL_USE_ADC TRUE +define STM32_ADC_USE_ADC1 TRUE +#define HAL_DISABLE_ADC_DRIVER TRUE +PA0 VSENSE ADC1 SCALE(2) + +define HAL_NO_GPIO_IRQ + +# avoid RCIN thread to save memory +define HAL_NO_RCIN_THREAD +define HAL_USE_RTC TRUE +define DISABLE_SERIAL_ESC_COMM TRUE +define DMA_RESERVE_SIZE 0 +define HAL_DISABLE_LOOP_DELAY +define PERIPH_FW TRUE + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +PC1 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +define CAN_APP_NODE_NAME "org.ardupilot.Sierra-F9P" + +define HAL_NO_MONITOR_THREAD +define HAL_MINIMIZE_FEATURES 0 +define HAL_BUILD_AP_PERIPH +define HAL_DEVICE_THREAD_STACK 768 + +# we setup a small defaults.parm +define AP_PARAM_MAX_EMBEDDED_PARAM 256 + +# disable dual GPS and GPS blending to save flash space +define GPS_MAX_RECEIVERS 1 +define GPS_MAX_INSTANCES 1 +define HAL_COMPASS_MAX_SENSORS 1 + +# GPS+MAG+BARO+Buzzer+NeoPixels +define HAL_PERIPH_ENABLE_GPS +define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_RC_OUT +define HAL_PERIPH_ENABLE_NOTIFY +define CONFIGURE_PPS_PIN TRUE +#define HAL_INS_ENABLED 1 +define GPS_MOVING_BASELINE 1 + +# Logging +define HAL_LOGGING_ENABLED 1 +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# SD Card pins +PC8 SDIO_D0 SDIO +PC9 SDIO_D1 SDIO +PC10 SDIO_D2 SDIO +PC11 SDIO_D3 SDIO +PB15 SDIO_CK SDIO +PD2 SDIO_CMD SDIO + +# use the app descriptor needed by MissionPlanner for CAN upload +env APP_DESCRIPTOR MissionPlanner + +# reserve 256 bytes for comms between app and bootloader +RAM_RESERVE_START 256 + +# allow for reboot command for faster development +define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 + +#Sensors Enable & ESP Enable +PB0 VDD_3V3_SENSORS_EN OUTPUT HIGH +PC2 ESP_PWR_EN OUTPUT LOW From c690efb0338e402587dfefc0c37f6ad032fa4e7e Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Tue, 8 Mar 2022 15:37:27 +0530 Subject: [PATCH 0208/3101] Tools: Rename board --- Tools/AP_Bootloader/board_types.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index e38361f30aa..37a5149a405 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -132,7 +132,7 @@ AP_HW_KAKUTEF4_MINI 1030 AP_HW_H31_PIXC4_PI 1031 AP_HW_H31_PIXC4_JETSON 1032 AP_HW_CUBEORANGE_JOEY 1033 -AP_HW_SIERRAF9PGPS_PERIPH 1034 +AP_HW_SierraF9P 1034 AP_HW_HolybroGPS 1035 AP_HW_QioTekZealotH743 1036 AP_HW_HEREPRO 1037 From 55c46cd8686bc625446a98a9fa4f0f95fa3dd7dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Mar 2022 11:10:00 +1100 Subject: [PATCH 0209/3101] Plane: protect against short stop_distance --- ArduPlane/quadplane.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b48a4316c82..43582fe7777 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3541,7 +3541,7 @@ float QuadPlane::stopping_distance(float ground_speed_squared) const */ float QuadPlane::accel_needed(float stop_distance, float ground_speed_squared) const { - return ground_speed_squared / (2 * stop_distance); + return ground_speed_squared / (2 * MAX(1,stop_distance)); } /* From 0ca48fa9da1a5a525f335755907007219f1c49f3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Mar 2022 13:43:08 +1100 Subject: [PATCH 0210/3101] Plane: added release notes for 4.2.0beta2 --- ArduPlane/ReleaseNotes.txt | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 2a1f7621456..a79485d870f 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,28 @@ +Release 4.2.0beta2 10th March 2022 +---------------------------------- + +This is the 2nd beta of the 4.2.0 major release. This beta should be +close to the final 4.2.0 release. + + - fixed EKF3 constrain bug + - added quadplane ship landing support + - added Q_LAND_ALTCHG to adjust landing detection sensitivity + - fixed a bug in terrain home compensation + - changed quaplane POSITION1 landing controller to use jerk and accel limiting + - fixed default gain for weathervaning on quadplanes + - fixed health check of airspeed sensor for dual sensors + - fixed arming check on servos for GPIO outputs + - disable stick mixing when RC input is unavailable + - fixed stick mixing during GCS failsafe + - disallow mavlink disarm while flying + - adjust frequency selection for DShot + - fixed logging bug for MCU voltage + - added BeastH7v2 support + - improvement to AC_WPNav for quadplane waypoint control + - simplify RC failsafe messages sent to GCS + +Happy flying! + Release 4.2.0beta1 2nd March 2022 --------------------------------- From a2057b752dfdff16c4615c18daefb90e1695dd3a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Mar 2022 18:44:31 +1100 Subject: [PATCH 0211/3101] Tools: build_options.py: fix syntax error --- Tools/scripts/build_options.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 5f46164a02c..046a72cc1dd 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -98,7 +98,7 @@ def __init__(self, category, label, define, description, default, dependency): Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users Feature('Sensors', 'BEACON', 'BEACON_ENABLED', 'Enable Beacon', 0, None), Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None), - Feature('Rangefinders', 'RANGEFINDERS', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders', 0, None), + Feature('Rangefinders', 'RANGEFINDERS', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None), Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"), From 9eb5c93c553568226eae43268be014431ee673a3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Mar 2022 19:02:33 +1100 Subject: [PATCH 0212/3101] Tools: board_list.py: build Sub binaries --- Tools/scripts/board_list.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index 7306a0cdee5..2389c682fbf 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -21,6 +21,7 @@ def __init__(self, name): 'Heli', 'Plane', 'Rover', + 'Sub', ] From ee138e6b3c40b58d270fcd4e5ee3e68f85f21738 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Mar 2022 15:06:36 +1100 Subject: [PATCH 0213/3101] Tools: remove Solo Cubes and skyvipers from blacklist --- Tools/scripts/board_list.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index 2389c682fbf..9c605a3581a 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -117,13 +117,6 @@ def find_autobuild_boards(self, build_target=None): # should probably have a line in the hwdef indicating they # shouldn't be auto-built... blacklist = [ - # the following boards are hacked into build_binaries.py - # to be built for Copter only: - "CubeGreen-solo", - "CubeSolo", - "skyviper-journey", - "skyviper-v2450", - # IOMCU: "iomcu", 'iomcu_f103_8MHz', From bac20bd8760a96387311ceb2c06da44db2fff66c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Mar 2022 09:07:27 +1100 Subject: [PATCH 0214/3101] autotest: board_list.py: correct autobuild target name for Tracker We're using the shortened name throughout --- Tools/scripts/board_list.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index 9c605a3581a..b3d3c8d1522 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -15,7 +15,7 @@ def __init__(self, name): self.name = name self.is_ap_periph = False self.autobuild_targets = [ - 'AntennaTracker', + 'Tracker', 'Blimp', 'Copter', 'Heli', From e94f28c81d099c322ee5b446ad68ca0180b34cf4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Mar 2022 21:23:43 +1100 Subject: [PATCH 0215/3101] Tools: build_options.py: make and assert it flake8-clean --- Tools/scripts/build_options.py | 44 +++++++++++++++++++++------------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 046a72cc1dd..d07dc19d8d1 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -2,12 +2,22 @@ ''' Provide structured data understood by the CustomBuild server app.py + +AP_FLAKE8_CLEAN + ''' class Feature: - '''defines a feature which can be built into the firmware, along with its dependencies''' - def __init__(self, category, label, define, description, default, dependency): + '''defines a feature which can be built into the firmware, along with + its dependencies''' + def __init__(self, + category, + label, + define, + description, + default, + dependency): self.category = category self.label = label self.define = define @@ -15,8 +25,10 @@ def __init__(self, category, label, define, description, default, dependency): self.default = default self.dependency = dependency -# list of build options to offer -# NOTE: the dependencies must be written as a single string with commas and no spaces, eg. 'dependency1,dependency2' + +# list of build options to offer NOTE: the dependencies must be +# written as a single string with commas and no spaces, +# eg. 'dependency1,dependency2' BUILD_OPTIONS = [ Feature('AHRS', 'EKF3', 'HAL_NAVEKF3_AVAILABLE', 'Enable EKF3', 1, None), Feature('AHRS', 'EKF2', 'HAL_NAVEKF2_AVAILABLE', 'Enable EKF2', 0, None), @@ -44,14 +56,14 @@ def __init__(self, category, label, define, description, default, dependency): Feature('Telemetry', 'SPEKTRUM', 'HAL_SPEKTRUM_TELEM_ENABLED', 'Enable Spektrum Telemetry', 0, None), Feature('MSP', 'MSP', 'HAL_MSP_ENABLED', 'Enable MSP Telemetry and MSP OSD', 0, 'OSD'), - Feature('MSP', 'MSP_SENSORS', 'HAL_MSP_SENSORS_ENABLED', 'Enable MSP Sensors', 0, 'MSP_GPS,MSP_BARO,MSP_COMPASS,MSP_AIRSPEED,MSP,MSP_OPTICALFLOW,MSP_RANGEFINDER,OSD'), + Feature('MSP', 'MSP_SENSORS', 'HAL_MSP_SENSORS_ENABLED', 'Enable MSP Sensors', 0, 'MSP_GPS,MSP_BARO,MSP_COMPASS,MSP_AIRSPEED,MSP,MSP_OPTICALFLOW,MSP_RANGEFINDER,OSD'), # NOQA: E501 Feature('MSP', 'MSP_GPS', 'HAL_MSP_GPS_ENABLED', 'Enable MSP GPS', 0, 'MSP,OSD'), Feature('MSP', 'MSP_COMPASS', 'HAL_MSP_COMPASS_ENABLED', 'Enable MSP Compass', 0, 'MSP,OSD'), Feature('MSP', 'MSP_BARO', 'HAL_MSP_BARO_ENABLED', 'Enable MSP Baro', 0, 'MSP,OSD'), Feature('MSP', 'MSP_AIRSPEED', 'HAL_MSP_AIRSPEED_ENABLED', 'Enable MSP AirSpeed', 0, 'MSP,OSD'), - Feature('MSP', 'MSP_OPTICALFLOW', 'HAL_MSP_OPTICALFLOW_ENABLED', 'Enable MSP OpticalFlow', 0, 'MSP,OSD'), # also OPTFLOW dep + Feature('MSP', 'MSP_OPTICALFLOW', 'HAL_MSP_OPTICALFLOW_ENABLED', 'Enable MSP OpticalFlow', 0, 'MSP,OSD'), # also OPTFLOW dep # NOQA: E501 Feature('MSP', 'MSP_RANGEFINDER', 'HAL_MSP_RANGEFINDER_ENABLED', 'Enable MSP Rangefinder', 0, 'MSP,OSD,RANGEFINDERS'), - Feature('MSP', 'MSP_DISPLAYPORT', 'HAL_WITH_MSP_DISPLAYPORT', 'Enable MSP DisplayPort OSD (aka CANVAS MODE)', 0, 'MSP,OSD'), + Feature('MSP', 'MSP_DISPLAYPORT', 'HAL_WITH_MSP_DISPLAYPORT', 'Enable MSP DisplayPort OSD (aka CANVAS MODE)', 0, 'MSP,OSD'), # NOQA: E501 Feature('ICE', 'EFI', 'HAL_EFI_ENABLED', 'Enable EFI Monitoring', 0, None), Feature('ICE', 'EFI_NMPWU', 'HAL_EFI_NWPWU_ENABLED', 'Enable EFI NMPMU', 0, None), @@ -95,23 +107,23 @@ def __init__(self, category, label, define, description, default, dependency): Feature('Plane', 'SOARING', 'HAL_SOARING_ENABLED', 'Enable Soaring', 0, None), Feature('Plane', 'DEEPSTALL', 'HAL_LANDING_DEEPSTALL_ENABLED', 'Enable Deepstall Landing', 0, None), - Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users + Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users # NOQA: E501 Feature('Sensors', 'BEACON', 'BEACON_ENABLED', 'Enable Beacon', 0, None), Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None), Feature('Rangefinders', 'RANGEFINDERS', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), Feature('Sensors', 'OPTICALFLOW', 'AP_OPTICALFLOW_ENABLED', 'Enable Optical Flow', 0, None), Feature('Sensors', 'OPTICALFLOW_CXOF', 'AP_OPTICALFLOW_CXOF_ENABLED', 'Enable Optical flow CXOF Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_HEREFLOW', 'AP_OPTICALFLOW_HEREFLOW_ENABLED', 'Enable Optical flow HereFlow Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_MAV', 'AP_OPTICALFLOW_MAV_ENABLED', 'Enable Optical flow MAVLink Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_ONBOARD', 'AP_OPTICALFLOW_ONBOARD_ENABLED', 'Enable Optical flow ONBOARD Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_PX4FLOW', 'AP_OPTICALFLOW_PX4FLOW_ENABLED', 'Enable Optical flow PX4FLOW Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_PIXART', 'AP_OPTICALFLOW_PIXART_ENABLED', 'Enable Optical flow PIXART Sensor', 0, "OPTICALFLOW"), - Feature('Sensors', 'OPTICALFLOW_UPFLOW', 'AP_OPTICALFLOW_UPFLOW_ENABLED', 'Enable Optical flow UPFLOW Sensor', 0, "OPTICALFLOW"), + Feature('Sensors', 'OPTICALFLOW_HEREFLOW', 'AP_OPTICALFLOW_HEREFLOW_ENABLED', 'Enable Optical flow HereFlow Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Sensors', 'OPTICALFLOW_MAV', 'AP_OPTICALFLOW_MAV_ENABLED', 'Enable Optical flow MAVLink Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Sensors', 'OPTICALFLOW_ONBOARD', 'AP_OPTICALFLOW_ONBOARD_ENABLED', 'Enable Optical flow ONBOARD Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Sensors', 'OPTICALFLOW_PX4FLOW', 'AP_OPTICALFLOW_PX4FLOW_ENABLED', 'Enable Optical flow PX4FLOW Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Sensors', 'OPTICALFLOW_PIXART', 'AP_OPTICALFLOW_PIXART_ENABLED', 'Enable Optical flow PIXART Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Sensors', 'OPTICALFLOW_UPFLOW', 'AP_OPTICALFLOW_UPFLOW_ENABLED', 'Enable Optical flow UPFLOW Sensor', 0, "OPTICALFLOW"), # NOQA: E501 Feature('Sensors', 'RPM', 'RPM_ENABLED', 'Enable RPM sensors', 0, None), - Feature('Other', 'DSP', 'HAL_WITH_DSP', 'Enable DSP for In-Flight FFT', 0, None), + Feature('Other', 'DSP', 'HAL_WITH_DSP', 'Enable DSP for In-Flight FFT', 0, None), Feature('Other', 'DISPLAY', 'HAL_DISPLAY_ENABLED', 'Enable I2C Displays', 0, None), Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA Output', 0, None), Feature('Other', 'BARO_WIND_COMP', 'HAL_BARO_WIND_COMP_ENABLED', 'Enable Baro Wind Compensation', 0, None), @@ -125,6 +137,6 @@ def __init__(self, category, label, define, description, default, dependency): Feature('GPS Drivers', 'NOVA', 'AP_GPS_NOVA_ENABLED', 'Enable NOVA GPS', 0, None), Feature('GPS Drivers', 'SBF', 'AP_GPS_SBF_ENABLED', 'Enable SBF GPS', 0, None), Feature('GPS Drivers', 'SIRF', 'AP_GPS_SIRF_ENABLED', 'Enable SiRF GPS', 0, None), - ] +] BUILD_OPTIONS.sort(key=lambda x: x.category) From 068208e4d716ba89e7a6df0dd424e492d6368068 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 8 Mar 2022 14:21:27 +0530 Subject: [PATCH 0216/3101] AP_Periph: add debug option for hold in bootloader after 15s --- Tools/AP_Periph/AP_Periph.cpp | 13 +++++++++++-- Tools/AP_Periph/AP_Periph.h | 5 +++++ Tools/AP_Periph/Parameters.cpp | 3 ++- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 8364ec59fb4..2ff9ff446ef 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -384,12 +384,21 @@ void AP_Periph_FW::update() #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE static uint32_t last_debug_ms; - if (g.debug==1 && now - last_debug_ms > 5000) { + if ((g.debug&(1< 5000) { last_debug_ms = now; show_stack_free(); } #endif - + + if ((g.debug&(1< 15000) { + // attempt reboot with HOLD after 15s + periph.prepare_reboot(); +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + set_fast_reboot((rtc_boot_magic)(RTC_BOOT_HOLD)); + NVIC_SystemReset(); +#endif + } + #ifdef HAL_PERIPH_ENABLE_BATTERY if (now - battery.last_read_ms >= 100) { // update battery at 10Hz diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 77262b1634c..7454b5ef74b 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -247,6 +247,11 @@ class AP_Periph_FW { static AP_Periph_FW *_singleton; + enum { + DEBUG_SHOW_STACK, + DEBUG_AUTOREBOOT + }; + // show stack as DEBUG msgs void show_stack_free(); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 2fe378093c9..a023723043e 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -136,10 +136,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Param: DEBUG // @DisplayName: Debug // @Description: Debug - // @Values: 0:Disabled, 1:Show free stack space + // @Bitmask: 0:Disabled, 1:Show free stack space, 2:Auto Reboot after 15sec // @User: Advanced GSCALAR(debug, "DEBUG", 0), + // @Param: BRD_SERIAL_NUM // @DisplayName: Serial number of device // @Description: Non-zero positive values will be shown on the CAN App Name string From 287151a858c6e9252a43d2ba45ef44ff76a9ff86 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:22:35 +0530 Subject: [PATCH 0217/3101] AP_CANManager: add support for CANFD tests --- libraries/AP_CANManager/AP_CANTester.cpp | 34 +++++++++++++++++------- libraries/AP_CANManager/AP_CANTester.h | 3 ++- 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANTester.cpp b/libraries/AP_CANManager/AP_CANTester.cpp index dc25bc45dfa..64c50d45622 100644 --- a/libraries/AP_CANManager/AP_CANTester.cpp +++ b/libraries/AP_CANManager/AP_CANTester.cpp @@ -39,9 +39,10 @@ const AP_Param::GroupInfo CANTester::var_info[] = { // @DisplayName: CAN Test Index // @Description: Selects the Index of Test that needs to be run recursively, this value gets reset to 0 at boot. // @Range: 0 4 - // @Values: 0:TEST_NONE, 1:TEST_LOOPBACK,2:TEST_BUSOFF_RECOVERY,3:TEST_UAVCAN_DNA,4:TEST_TOSHIBA_CAN, 5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC + // @Values: 0:TEST_NONE, 1:TEST_LOOPBACK,2:TEST_BUSOFF_RECOVERY,3:TEST_UAVCAN_DNA,4:TEST_TOSHIBA_CAN, 5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC, 7:TEST_UAVCAN_FD_ESC // @User: Advanced AP_GROUPINFO("ID", 1, CANTester, _test_id, 0), + // @Param: LPR8 // @DisplayName: CANTester LoopRate // @Description: Selects the Looprate of Test methods @@ -219,7 +220,7 @@ void CANTester::main_thread() case CANTester::TEST_UAVCAN_ESC: if (_can_ifaces[1] == nullptr) { gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********"); - if (test_uavcan_esc()) { + if (test_uavcan_esc(false)) { gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********"); } else { gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********"); @@ -228,6 +229,18 @@ void CANTester::main_thread() gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST"); } break; + case CANTester::TEST_UAVCAN_FD_ESC: + if (_can_ifaces[1] == nullptr) { + gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN FD ESC Test********"); + if (test_uavcan_esc(true)) { + gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Pass********"); + } else { + gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Fail********"); + } + } else { + gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_FD_ESC_TEST"); + } + break; default: break; } @@ -340,7 +353,8 @@ void CANTester::reset_frame(AP_HAL::CANFrame& frame) void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame) { frame.id = 13 | AP_HAL::CANFrame::FlagEFF; - frame.dlc = AP_HAL::CANFrame::MaxDataLen; + frame.dlc = 8; + frame.setCanFD(stats.tx_seq%2); //every other frame is canfd if supported memcpy(frame.data, &stats.tx_seq, sizeof(stats.tx_seq)); uint32_t loopback_magic = LOOPBACK_MAGIC; memcpy(&frame.data[4], &loopback_magic, sizeof(loopback_magic)); @@ -349,7 +363,7 @@ void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL void CANTester::check_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame) { - if (frame.dlc != AP_HAL::CANFrame::MaxDataLen) { + if (frame.dlc != 8) { stats.bad_rx_data++; } @@ -380,7 +394,7 @@ bool CANTester::test_busoff_recovery() AP_HAL::CANFrame bo_frame; bo_frame.id = (10 | AP_HAL::CANFrame::FlagEFF); memset(bo_frame.data, 0xA, sizeof(bo_frame.data)); - bo_frame.dlc = AP_HAL::CANFrame::MaxDataLen; + bo_frame.dlc =8;//AP_HAL::CANFrame::MaxDataLen; bool bus_off_detected = false; // Bus Fault can be introduced by shorting CANH and CANL gcs().send_text(MAV_SEVERITY_ERROR, "Introduce Bus Off Fault on the bus."); @@ -428,7 +442,7 @@ bool CANTester::test_uavcan_dna() return false; } - auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); + auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); if (!node) { return false; } @@ -746,12 +760,12 @@ void handle_raw_command(const uavcan::ReceivedDataStructure *node = nullptr; { - node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); + node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); if (node == nullptr) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node"); ret = false; @@ -840,7 +854,7 @@ bool CANTester::test_uavcan_esc() goto exit; } - esc_status_publisher = new uavcan::Publisher(*node); + esc_status_publisher = new uavcan::Publisher(*node, enable_canfd); if (esc_status_publisher == nullptr) { ret = false; goto exit; diff --git a/libraries/AP_CANManager/AP_CANTester.h b/libraries/AP_CANManager/AP_CANTester.h index 8fad7557683..1a8c939e904 100644 --- a/libraries/AP_CANManager/AP_CANTester.h +++ b/libraries/AP_CANManager/AP_CANTester.h @@ -51,6 +51,7 @@ class CANTester : public AP_CANDriver TEST_TOSHIBA_CAN, TEST_KDE_CAN, TEST_UAVCAN_ESC, + TEST_UAVCAN_FD_ESC, TEST_END, }; @@ -79,7 +80,7 @@ class CANTester : public AP_CANDriver bool test_kdecan(); - bool test_uavcan_esc(); + bool test_uavcan_esc(bool enable_canfd); // write frame on CAN bus, returns true on success bool write_frame(uint8_t iface, AP_HAL::CANFrame &out_frame, uint64_t timeout); From 153834465bee9f1e26008c728a656180b6343f5f Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:23:04 +0530 Subject: [PATCH 0218/3101] AP_HAL: add HAL_CANFD_SUPPORTED define --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ libraries/AP_HAL/board/chibios.h | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index a9560e014f5..d98c36f27d0 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -282,6 +282,10 @@ // plus one static notch with one harmonic #define HAL_HNF_MAX_FILTERS 18 #endif +#endif // HAL_HNF_MAX_FILTERS + +#ifndef HAL_CANFD_SUPPORTED +#define HAL_CANFD_SUPPORTED 0 #endif #ifndef __RAMFUNC__ diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index fa3e48511e8..34b5726af2e 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -128,3 +128,9 @@ #ifndef HAL_BOARD_STORAGE_DIRECTORY #define HAL_BOARD_STORAGE_DIRECTORY "/APM" #endif + +#if defined(STM32H7XX) || defined(STM32G4) +#define HAL_CANFD_SUPPORTED 1 +# else +#define HAL_CANFD_SUPPORTED 0 +#endif From e9f426f8fd07aa178dd858a62236a7786a68bef1 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:23:59 +0530 Subject: [PATCH 0219/3101] AP_HAL: update CANFrame to include CANFD details --- libraries/AP_HAL/CANIface.h | 89 ++++++++++++++++++++++++++++++++++--- 1 file changed, 83 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 68988ad0e0c..be0950802ef 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -32,7 +32,7 @@ struct AP_HAL::CANFrame { static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request static const uint32_t FlagERR = 1U << 29; ///< Error frame - static const uint8_t MaxDataLen = 8; + static const uint8_t MaxDataLen = 64; uint32_t id; ///< CAN ID with flags (above) union { @@ -40,22 +40,49 @@ struct AP_HAL::CANFrame { uint32_t data_32[MaxDataLen/4]; }; uint8_t dlc; ///< Data Length Code + bool canfd; CANFrame() : id(0), - dlc(0) + dlc(0), + canfd(false) { memset(data,0, MaxDataLen); } - CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) : + CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false) : id(can_id), - dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) + canfd(canfd_frame) { - if ((can_data == nullptr) || (data_len != dlc) || (dlc == 0)) { + if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) { return; } - memcpy(this->data, can_data, dlc); + memcpy(this->data, can_data, data_len); + if (data_len <= 8) { + dlc = data_len; + } else if (!canfd) { + dlc = 8; + } else { + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (data_len <= 12) { + dlc = 9; + } else if (data_len <= 16) { + dlc = 10; + } else if (data_len <= 20) { + dlc = 11; + } else if (data_len <= 24) { + dlc = 12; + } else if (data_len <= 32) { + dlc = 13; + } else if (data_len <= 48) { + dlc = 14; + } else if (data_len <= 64) { + dlc = 15; + } + } } bool operator!=(const CANFrame& rhs) const @@ -79,7 +106,56 @@ struct AP_HAL::CANFrame { { return id & FlagERR; } + void setCanFD(bool canfd_frame) + { + canfd = canfd_frame; + } + + bool isCanFDFrame() const + { + return canfd; + } + static uint8_t dlcToDataLength(uint8_t dlc) { + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (dlc <= 8) { + return dlc; + } else if (dlc == 9) { + return 12; + } else if (dlc == 10) { + return 16; + } else if (dlc == 11) { + return 20; + } else if (dlc == 12) { + return 24; + } else if (dlc == 13) { + return 32; + } else if (dlc == 14) { + return 48; + } + return 64; + } + static uint8_t dataLengthToDlc(uint8_t data_length) { + if (data_length <= 8) { + return data_length; + } else if (data_length <= 12) { + return 9; + } else if (data_length <= 16) { + return 10; + } else if (data_length <= 20) { + return 11; + } else if (data_length <= 24) { + return 12; + } else if (data_length <= 32) { + return 13; + } else if (data_length <= 48) { + return 14; + } + return 15; + } /** * CAN frame arbitration rules, particularly STD vs EXT: * Marco Di Natale - "Understanding and using the Controller Area Network" @@ -127,6 +203,7 @@ class AP_HAL::CANIface bool aborted:1; bool pushed:1; bool setup:1; + bool canfd_frame:1; bool operator<(const CanTxItem& rhs) const { From 8aa047d8769b1fb7443adfdda782c8465adddf6f Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:24:50 +0530 Subject: [PATCH 0220/3101] AP_HAL_ChibiOS: add support for CANFD for H7 and G4 --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 98 +++++++++++++++++++------ libraries/AP_HAL_ChibiOS/CANFDIface.h | 7 ++ 2 files changed, 84 insertions(+), 21 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 9510c6161f0..2258f0f0405 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -57,20 +57,16 @@ #define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER #define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER -#if defined(STM32G4) -// on G4 FIFO elements are spaced at 18 words +// FIFO elements are spaced at 18 words #define FDCAN_FRAME_BUFFER_SIZE 18 -#else -// on H7 they are spaced at 4 words -#define FDCAN_FRAME_BUFFER_SIZE 4 -#endif + //Message RAM Allocations in Word lengths #if defined(STM32H7) -#define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List -#define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames -#define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames +#define MAX_FILTER_LIST_SIZE 78U //78 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 108U //6 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 126U //7 Frames #define MESSAGE_RAM_END_ADDR 0x4000B5FC #elif defined(STM32G4) @@ -243,6 +239,7 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); if ((prescaler < 1U) || (prescaler > 1024U)) { + Debug("Timings: No Solution found\n"); return false; // No solution } @@ -295,6 +292,9 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8)); } + Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + /* * Final validation * Helpful Python: @@ -305,12 +305,10 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing * */ if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { + Debug("Timings: Invalid Solution %lu %lu %d %d %lu \n", pclk, prescaler, int(solution.bs1), int(solution.bs2), (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))); return false; } - Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n", - int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); - out_timings.prescaler = uint16_t(prescaler - 1U); out_timings.sjw = 0; // Which means one out_timings.bs1 = uint8_t(solution.bs1 - 1); @@ -318,11 +316,23 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing return true; } +static uint32_t bytes_to_word(const uint8_t data[]) { + return (uint32_t(data[3]) << 24) | + (uint32_t(data[2]) << 16) | + (uint32_t(data[1]) << 8) | + (uint32_t(data[0]) << 0); +} + int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) { + if (!initialised_) { + return -1; + } + stats.tx_requests++; - if (frame.isErrorFrame() || frame.dlc > 8 || !initialised_) { + if (frame.isErrorFrame() || (frame.dlc > 8 && !frame.isCanFDFrame()) || + frame.dlc > 15) { stats.tx_rejected++; return -1; } @@ -336,7 +346,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, uint8_t index; if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { - stats.tx_rejected++; + stats.tx_overflow++; return 0; //we don't have free space } index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); @@ -357,9 +367,19 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, //Write Data Length Code, and Message Marker buffer[1] = frame.dlc << 16 | index << 24; + if (frame.isCanFDFrame()) { + buffer[1] |= FDF | BRS; // do CAN FD transfer and bit rate switching + stats.fdf_tx_requests++; + pending_tx_[index].canfd_frame = true; + } else { + pending_tx_[index].canfd_frame = false; + } + // Write Frame to the message RAM - buffer[2] = frame.data_32[0]; - buffer[3] = frame.data_32[1]; + const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); + for (uint8_t i = 0, l = 2; i < data_length; i+=4, l++) { + buffer[l] = bytes_to_word(&frame.data[i]); + } //Set Add Request can_->TXBAR = (1 << index); @@ -649,6 +669,19 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos) | (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); + if (!computeTimings(bitrate*8, timings)) { // Do 8x fast Data transmission for CAN FD frames + can_->CCCR &= ~FDCAN_CCCR_INIT; + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + return false; + } + Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", + unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2)); + can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); @@ -684,6 +717,8 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) #endif can_->ILE = 0x3; + can_->CCCR |= FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; // enable sending CAN FD frames, and Bitrate switching + // If mode is Filtered then we finish the initialisation in configureFilter method // otherwise we finish here if (mode != FilteredMode) { @@ -722,6 +757,8 @@ void CANIface::setupMessageRam() #else uint32_t num_elements = 0; + can_->RXESC = 0x777; //Support upto 64byte long frames + can_->TXESC = 0x7; //Support upto 64byte long frames // Rx FIFO 0 start address and element count num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); if (num_elements) { @@ -753,6 +790,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) if (!pending_tx_[i].pushed) { stats.tx_success++; + if (pending_tx_[i].canfd_frame) { + stats.fdf_tx_success++; + } pending_tx_[i].pushed = true; } else { continue; @@ -821,7 +861,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) } // Read the frame contents - AP_HAL::CANFrame frame; + AP_HAL::CANFrame frame {}; uint32_t id = frame_ptr[0]; if ((id & IDE) == 0) { //Standard ID @@ -835,10 +875,18 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) if ((id & RTR) != 0) { frame.id |= AP_HAL::CANFrame::FlagRTR; } + + if (frame_ptr[1] & FDF) { + frame.setCanFD(true); + stats.fdf_rx_received++; + } else { + frame.setCanFD(false); + } + frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16; uint8_t *data = (uint8_t*)&frame_ptr[2]; - //We only handle Data Length of 8 Bytes for now - for (uint8_t i = 0; i < 8; i++) { + + for (uint8_t i = 0; i < AP_HAL::CANFrame::MaxDataLen; i++) { frame.data[i] = data[i]; } @@ -1039,6 +1087,7 @@ void CANIface::get_stats(ExpandingString &str) CriticalSectionLocker lock; str.printf("tx_requests: %lu\n" "tx_rejected: %lu\n" + "tx_overflow: %lu\n" "tx_success: %lu\n" "tx_timedout: %lu\n" "tx_abort: %lu\n" @@ -1047,9 +1096,13 @@ void CANIface::get_stats(ExpandingString &str) "rx_errors: %lu\n" "num_busoff_err: %lu\n" "num_events: %lu\n" - "ECR: %lx\n", + "ECR: %lx\n" + "fdf_rx: %lu\n" + "fdf_tx_req: %lu\n" + "fdf_tx: %lu\n", stats.tx_requests, stats.tx_rejected, + stats.tx_overflow, stats.tx_success, stats.tx_timedout, stats.tx_abort, @@ -1058,7 +1111,10 @@ void CANIface::get_stats(ExpandingString &str) stats.rx_errors, stats.num_busoff_err, stats.num_events, - stats.ecr); + stats.ecr, + stats.fdf_rx_received, + stats.fdf_tx_requests, + stats.fdf_tx_success); } #endif diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index b133c4cc269..4d1dcb2f55a 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -61,6 +61,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface static constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask static constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request static constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code + static constexpr unsigned long FDF = (0x00200000U); // CAN FD Frame + static constexpr unsigned long BRS = (0x00100000U); // Bit Rate Switching + /** * CANx register sets @@ -151,6 +154,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface struct { uint32_t tx_requests; uint32_t tx_rejected; + uint32_t tx_overflow; uint32_t tx_success; uint32_t tx_timedout; uint32_t tx_abort; @@ -160,6 +164,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface uint32_t num_busoff_err; uint32_t num_events; uint32_t ecr; + uint32_t fdf_tx_requests; + uint32_t fdf_tx_success; + uint32_t fdf_rx_received; } stats; public: From ef1bdc2e5fe6516c5d52660c8a87dcbc27ae2bf5 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 3 May 2021 18:28:40 +0530 Subject: [PATCH 0221/3101] AP_UAVCAN: add support for optionally publishing CANFD frames --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 11 +++++++++-- libraries/AP_UAVCAN/AP_UAVCAN.h | 17 ++++++++++++++++- libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp | 6 +++--- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 3dba5e0060f..26e62ae14da 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -103,7 +103,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: OPTION // @DisplayName: UAVCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), @@ -271,6 +271,13 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) } _node->setHardwareVersion(hw_version); } + +#if UAVCAN_SUPPORT_CANFD + if (option_is_set(Options::CANFD_ENABLED)) { + _node->enableCanFd(); + } +#endif + int start_res = _node->start(); if (start_res < 0) { debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res); @@ -369,7 +376,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index); - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, UAVCAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { _node->setModeOfflineAndPublish(); debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r"); return; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 5fed1a4cc91..8ea923c3364 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -34,12 +34,26 @@ #ifndef UAVCAN_NODE_POOL_SIZE +#if HAL_CANFD_SUPPORTED +#define UAVCAN_NODE_POOL_SIZE 16384 +#else #define UAVCAN_NODE_POOL_SIZE 8192 #endif +#endif + +#if HAL_CANFD_SUPPORTED +#define UAVCAN_STACK_SIZE 8192 +#else +#define UAVCAN_STACK_SIZE 4096 +#endif #ifndef UAVCAN_NODE_POOL_BLOCK_SIZE +#if HAL_CANFD_SUPPORTED +#define UAVCAN_NODE_POOL_BLOCK_SIZE 128 +#else #define UAVCAN_NODE_POOL_BLOCK_SIZE 64 #endif +#endif #ifndef UAVCAN_SRV_NUMBER #define UAVCAN_SRV_NUMBER 18 @@ -116,7 +130,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { void init(uint8_t driver_index, bool enable_filters) override; bool add_interface(AP_HAL::CANIface* can_iface) override; - + uavcan::Node<0>* get_node() { return _node; } uint8_t get_driver_index() const { return _driver_index; } @@ -206,6 +220,7 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { enum class Options : uint16_t { DNA_CLEAR_DATABASE = (1U<<0), DNA_IGNORE_DUPLICATE_NODE = (1U<<1), + CANFD_ENABLED = (1U<<2), }; // check if a option is set diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp index ee10e5e6a2c..bfa04b40843 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp @@ -48,7 +48,7 @@ int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFl if (can_iface_ == UAVCAN_NULLPTR) { return -1; } - return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, frame.dlc), tx_deadline.toUSec(), flags); + return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags); } /** @@ -83,7 +83,7 @@ int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, if (ret < 0) { return ret; } - out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, frame.dlc); + out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd); out_flags = flags; if (rx_timestamp != 0) { out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp); @@ -185,7 +185,7 @@ CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const msk.write |= (write ? 1 : 0) << i; } } else { - AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, pending_tx[i]->dlc}; + AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)}; if (iface->can_iface_->select(read, write, &frame, 0)) { msk.read |= (read ? 1 : 0) << i; msk.write |= (write ? 1 : 0) << i; From 5e54871d82d5b5dc4adc8c725ee0b77383f49c60 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 22:59:38 +0530 Subject: [PATCH 0222/3101] ardupilotwaf: set flags for CANFD support as applicable --- Tools/ardupilotwaf/boards.py | 6 ++++++ Tools/ardupilotwaf/chibios.py | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index a64818ee7cf..7d5db8bbd2a 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -937,6 +937,12 @@ def configure_env(self, cfg, env): ('10','2','1'), ] + if cfg.env.AP_PERIPH: + if cfg.env.HAL_CANFD_SUPPORTED: + env.DEFINES.update(CANARD_ENABLE_CANFD=1) + else: + env.DEFINES.update(CANARD_ENABLE_CANFD=0) + if cfg.options.Werror or cfg.env.CC_VERSION in gcc_whitelist: cfg.msg("Enabling -Werror", "yes") if '-Werror' not in env.CXXFLAGS: diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index f6c49019298..2c022cebd2a 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -360,6 +360,12 @@ def setup_canmgr_build(cfg): 'UAVCAN_NULLPTR=nullptr' ] + if cfg.env.HAL_CANFD_SUPPORTED: + env.DEFINES += ['UAVCAN_SUPPORT_CANFD=1'] + else: + env.DEFINES += ['UAVCAN_SUPPORT_CANFD=0'] + + env.INCLUDES += [ cfg.srcnode.find_dir('modules/uavcan/libuavcan/include').abspath(), ] From 563e69e64c6e8248893f549efb1c61ef97e8ff77 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 23:03:48 +0530 Subject: [PATCH 0223/3101] AP_CANManager: add support for trx CANFD frames over SLCAN --- libraries/AP_CANManager/AP_CANTester.cpp | 12 ++- libraries/AP_CANManager/AP_SLCANIface.cpp | 90 +++++++++++++++++++---- libraries/AP_CANManager/AP_SLCANIface.h | 3 + 3 files changed, 86 insertions(+), 19 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANTester.cpp b/libraries/AP_CANManager/AP_CANTester.cpp index 64c50d45622..71b50814a46 100644 --- a/libraries/AP_CANManager/AP_CANTester.cpp +++ b/libraries/AP_CANManager/AP_CANTester.cpp @@ -442,7 +442,7 @@ bool CANTester::test_uavcan_dna() return false; } - auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); + auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); if (!node) { return false; } @@ -777,7 +777,7 @@ bool CANTester::test_uavcan_esc(bool enable_canfd) uavcan::Node<0> *node = nullptr; { - node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator, false); + node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); if (node == nullptr) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node"); ret = false; @@ -854,13 +854,19 @@ bool CANTester::test_uavcan_esc(bool enable_canfd) goto exit; } - esc_status_publisher = new uavcan::Publisher(*node, enable_canfd); + esc_status_publisher = new uavcan::Publisher(*node); if (esc_status_publisher == nullptr) { ret = false; goto exit; } esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest); + + if (enable_canfd) { + node->enableCanFd(); + } else { + node->disableCanFd(); + } node->setNodeID(client.getAllocatedNodeID()); node->setModeOperational(); } diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp index f4ad8986e43..f17bf294201 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.cpp +++ b/libraries/AP_CANManager/AP_SLCANIface.cpp @@ -122,7 +122,7 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame) */ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) { - AP_HAL::CANFrame f; + AP_HAL::CANFrame f {}; hex2nibble_error = false; f.id = f.FlagEFF | (hex2nibble(cmd[1]) << 28) | @@ -133,11 +133,11 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) (hex2nibble(cmd[6]) << 8) | (hex2nibble(cmd[7]) << 4) | (hex2nibble(cmd[8]) << 0); - if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { + if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) { return false; } f.dlc = cmd[9] - '0'; - if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { + if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) { return false; } { @@ -153,18 +153,56 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) return push_Frame(f); } +#if HAL_CANFD_SUPPORTED +/** + * General frame format: + * + * The emitting functions below are highly optimized for speed. + */ +bool SLCAN::CANIface::handle_FDFrameDataExt(const char* cmd) +{ + AP_HAL::CANFrame f {}; + hex2nibble_error = false; + f.canfd = true; + f.id = f.FlagEFF | + (hex2nibble(cmd[1]) << 28) | + (hex2nibble(cmd[2]) << 24) | + (hex2nibble(cmd[3]) << 20) | + (hex2nibble(cmd[4]) << 16) | + (hex2nibble(cmd[5]) << 12) | + (hex2nibble(cmd[6]) << 8) | + (hex2nibble(cmd[7]) << 4) | + (hex2nibble(cmd[8]) << 0); + f.dlc = hex2nibble(cmd[9]); + if (f.dlc > AP_HAL::CANFrame::dataLengthToDlc(AP_HAL::CANFrame::MaxDataLen)) { + return false; + } + { + const char* p = &cmd[10]; + for (unsigned i = 0; i < AP_HAL::CANFrame::dlcToDataLength(f.dlc); i++) { + f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1)); + p += 2; + } + } + if (hex2nibble_error) { + return false; + } + return push_Frame(f); +} +#endif //#if HAL_CANFD_SUPPORTED + bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd) { - AP_HAL::CANFrame f; + AP_HAL::CANFrame f {}; hex2nibble_error = false; f.id = (hex2nibble(cmd[1]) << 8) | (hex2nibble(cmd[2]) << 4) | (hex2nibble(cmd[3]) << 0); - if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { + if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) { return false; } f.dlc = cmd[4] - '0'; - if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { + if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) { return false; } { @@ -182,7 +220,7 @@ bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd) bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd) { - AP_HAL::CANFrame f; + AP_HAL::CANFrame f {}; hex2nibble_error = false; f.id = f.FlagEFF | f.FlagRTR | (hex2nibble(cmd[1]) << 28) | @@ -193,12 +231,12 @@ bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd) (hex2nibble(cmd[6]) << 8) | (hex2nibble(cmd[7]) << 4) | (hex2nibble(cmd[8]) << 0); - if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { + if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) { return false; } f.dlc = cmd[9] - '0'; - if (f.dlc > AP_HAL::CANFrame::MaxDataLen) { + if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) { return false; } if (hex2nibble_error) { @@ -209,17 +247,17 @@ bool SLCAN::CANIface::handle_FrameRTRExt(const char* cmd) bool SLCAN::CANIface::handle_FrameRTRStd(const char* cmd) { - AP_HAL::CANFrame f; + AP_HAL::CANFrame f {}; hex2nibble_error = false; f.id = f.FlagRTR | (hex2nibble(cmd[1]) << 8) | (hex2nibble(cmd[2]) << 4) | (hex2nibble(cmd[3]) << 0); - if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::MaxDataLen)) { + if (cmd[4] < '0' || cmd[4] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) { return false; } f.dlc = cmd[4] - '0'; - if (f.dlc <= AP_HAL::CANFrame::MaxDataLen) { + if (f.dlc <= AP_HAL::CANFrame::NonFDCANMaxDataLen) { return false; } if (hex2nibble_error) { @@ -267,7 +305,11 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim if (_port == nullptr) { return -1; } +#if HAL_CANFD_SUPPORTED + constexpr unsigned SLCANMaxFrameSize = 200; +#else constexpr unsigned SLCANMaxFrameSize = 40; +#endif uint8_t buffer[SLCANMaxFrameSize] = {'\0'}; uint8_t* p = &buffer[0]; /* @@ -277,7 +319,13 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim *p++ = frame.isExtended() ? 'R' : 'r'; } else if (frame.isErrorFrame()) { return -1; // Not supported - } else { + } +#if HAL_CANFD_SUPPORTED + else if (frame.canfd) { + *p++ = frame.isExtended() ? 'D' : 'd'; + } +#endif + else { *p++ = frame.isExtended() ? 'T' : 't'; } @@ -301,12 +349,12 @@ int16_t SLCAN::CANIface::reportFrame(const AP_HAL::CANFrame& frame, uint64_t tim /* * DLC */ - *p++ = char('0' + frame.dlc); + *p++ = nibble2hex(frame.dlc); /* * Data */ - for (unsigned i = 0; i < frame.dlc; i++) { + for (unsigned i = 0; i < AP_HAL::CANFrame::dlcToDataLength(frame.dlc); i++) { const uint8_t byte = frame.data[i]; *p++ = nibble2hex(byte >> 4); *p++ = nibble2hex(byte); @@ -361,6 +409,12 @@ const char* SLCAN::CANIface::processCommand(char* cmd) // See long commands below return handle_FrameRTRStd(cmd) ? "z\r" : "\a"; } +#if HAL_CANFD_SUPPORTED + else if (cmd[0] == 'D') { + return handle_FDFrameDataExt(cmd) ? "Z\r" : "\a"; + } +#endif + uint8_t resp_bytes[40]; uint16_t resp_len; /* @@ -622,7 +676,11 @@ int16_t SLCAN::CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadlin return ret; } - if (frame.isErrorFrame() || frame.dlc > 8) { + if (frame.isErrorFrame() +#if !HAL_CANFD_SUPPORTED + || frame.dlc > 8 +#endif + ) { return ret; } reportFrame(frame, AP_HAL::native_micros64()); diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h index 9991c37b485..b00e5428782 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.h +++ b/libraries/AP_CANManager/AP_SLCANIface.h @@ -52,6 +52,9 @@ class CANIface: public AP_HAL::CANIface bool handle_FrameDataStd(const char* cmd); bool handle_FrameDataExt(const char* cmd); +#if HAL_CANFD_SUPPORTED + bool handle_FDFrameDataExt(const char* cmd); +#endif // Parsing bytes received on the serial port inline void addByte(const uint8_t byte); From 5bc65bb54e3094b30943ea55006b9dcd527263c2 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 23:05:29 +0530 Subject: [PATCH 0224/3101] AP_HAL: set CANFrame max data length based on CANFD availability --- libraries/AP_HAL/CANIface.h | 9 ++++++++- libraries/AP_HAL/board/chibios.h | 6 ------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index be0950802ef..8246168da9a 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -19,6 +19,7 @@ #include #include "AP_HAL_Namespace.h" +#include "AP_HAL_Boards.h" class ExpandingString; @@ -32,8 +33,13 @@ struct AP_HAL::CANFrame { static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request static const uint32_t FlagERR = 1U << 29; ///< Error frame +#if HAL_CANFD_SUPPORTED + static const uint8_t NonFDCANMaxDataLen = 8; static const uint8_t MaxDataLen = 64; - +#else + static const uint8_t NonFDCANMaxDataLen = 8; + static const uint8_t MaxDataLen = 8; +#endif uint32_t id; ///< CAN ID with flags (above) union { uint8_t data[MaxDataLen]; @@ -138,6 +144,7 @@ struct AP_HAL::CANFrame { } return 64; } + static uint8_t dataLengthToDlc(uint8_t data_length) { if (data_length <= 8) { return data_length; diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index 34b5726af2e..fa3e48511e8 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -128,9 +128,3 @@ #ifndef HAL_BOARD_STORAGE_DIRECTORY #define HAL_BOARD_STORAGE_DIRECTORY "/APM" #endif - -#if defined(STM32H7XX) || defined(STM32G4) -#define HAL_CANFD_SUPPORTED 1 -# else -#define HAL_CANFD_SUPPORTED 0 -#endif From 72090d10da908f6a09a32f59993a3f64274918ac Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 23:08:11 +0530 Subject: [PATCH 0225/3101] AP_HAL_ChibiOS: add hwdef config to enable CANFD support --- libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index c0d69637842..aac0e9b47ed 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat @@ -18,6 +18,8 @@ APJ_BOARD_ID 140 FLASH_SIZE_KB 2048 +CANFD_SUPPORTED 1 + # with 2M flash we can afford to optimize for speed env OPTIMIZE -O2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index b8158c9f212..697bcdcb377 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -689,6 +689,13 @@ def enable_can(f): if 'CAN' in bytype and mcu_type.startswith("STM32F3"): f.write('#define CAN1_BASE CAN_BASE\n') env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list)) + canfd_supported = get_config('CANFD_SUPPORTED', 0, required=False) + if canfd_supported: + f.write('#define HAL_CANFD_SUPPORTED 1\n') + env_vars['HAL_CANFD_SUPPORTED'] = 1 + else: + f.write('#define HAL_CANFD_SUPPORTED 0\n') + env_vars['HAL_CANFD_SUPPORTED'] = 0 def has_sdcard_spi(): From 8a8de73d898850d47011f385bb658ad78a636ca1 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 17 May 2021 22:56:34 +0530 Subject: [PATCH 0226/3101] AP_Periph: add support for CANFD trx --- Tools/AP_Periph/AP_Periph.h | 1 + Tools/AP_Periph/Parameters.cpp | 5 ++ Tools/AP_Periph/Parameters.h | 6 ++ Tools/AP_Periph/can.cpp | 102 ++++++++++++++++++++++----------- 4 files changed, 80 insertions(+), 34 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 7454b5ef74b..d1a30466cbc 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -85,6 +85,7 @@ class AP_Periph_FW { void load_parameters(); void prepare_reboot(); + bool canfdout() const { return (g.can_fdmode == 1); } #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT void check_for_serial_reboot_cmd(const int8_t serial_index); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index a023723043e..32569804340 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -374,6 +374,11 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp GOBJECT(scripting, "SCR_", AP_Scripting), #endif + +#if HAL_CANFD_SUPPORTED + // can node FD Out mode + GSCALAR(can_fdmode, "CAN_FDMODE", 0), +#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 2320297fd1f..2bc7c46d934 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -51,6 +51,7 @@ class Parameters { k_param_gps_mb_only_can_port, k_param_scripting, k_param_esc_telem_port, + k_param_can_fdmode, }; AP_Int16 format_version; @@ -123,6 +124,11 @@ class Parameters { AP_Int16 sysid_this_mav; #endif +#if HAL_CANFD_SUPPORTED + AP_Int8 can_fdmode; +#else + static constexpr uint8_t can_fdmode = 0; +#endif Parameters() {} }; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index bdd7c5a87d8..9d8a7b4949c 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -219,7 +219,7 @@ static void handle_get_node_info(CanardInstance* ins, } pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !periph.canfdout()); const int16_t resp_res = canardRequestOrRespond(ins, transfer->source_node_id, @@ -229,7 +229,11 @@ static void handle_get_node_info(CanardInstance* ins, transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + , periph.canfdout() +#endif +); if (resp_res <= 0) { printf("Could not respond to GetNodeInfo: %d\n", resp_res); } @@ -321,7 +325,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) } uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, @@ -331,7 +335,11 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); } @@ -374,7 +382,7 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr pkt.ok = true; uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, @@ -384,7 +392,11 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); } static void canard_broadcast(uint64_t data_type_signature, @@ -423,7 +435,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !periph.canfdout()); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, @@ -432,7 +444,11 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer->priority, CanardResponse, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif +); uint8_t count = 50; while (count--) { processTx(); @@ -858,7 +874,7 @@ static void can_safety_button_update(void) pkt.press_time = counter; uint8_t buffer[ARDUPILOT_INDICATION_BUTTON_MAX_SIZE] {}; - uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer); + uint16_t total_size = ardupilot_indication_Button_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_INDICATION_BUTTON_SIGNATURE, ARDUPILOT_INDICATION_BUTTON_ID, @@ -1138,7 +1154,11 @@ static void canard_broadcast(uint64_t data_type_signature, tid_ptr, priority, payload, - payload_len); + payload_len +#if HAL_CANFD_SUPPORTED + ,periph.canfdout() +#endif + ); #if DEBUG_PKTS if (res < 0) { printf("Tx error %d, IF%d %lx\n", res, ins.index); @@ -1161,9 +1181,12 @@ static void processTx(void) #endif for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&ins.canard)) != NULL;) { AP_HAL::CANFrame txmsg {}; - txmsg.dlc = txf->data_len; - memcpy(txmsg.data, txf->data, 8); + txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len); + memcpy(txmsg.data, txf->data, txf->data_len); txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); +#if HAL_CANFD_SUPPORTED + txmsg.canfd = txf->canfd; +#endif // push message with 1s timeout const uint64_t deadline = AP_HAL::native_micros64() + 1000000; if (ins.iface->send(txmsg, deadline, 0) > 0) { @@ -1208,8 +1231,11 @@ static void processRx(void) uint64_t timestamp; AP_HAL::CANIface::CanIOFlags flags; ins.iface->receive(rxmsg, timestamp, flags); - memcpy(rx_frame.data, rxmsg.data, 8); - rx_frame.data_len = rxmsg.dlc; + rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); + memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); +#if HAL_CANFD_SUPPORTED + rx_frame.canfd = rxmsg.canfd; +#endif rx_frame.id = rxmsg.id; #if DEBUG_PKTS const int16_t res = @@ -1245,7 +1271,7 @@ static void node_status_send(void) node_status.vendor_specific_status_code = hal.util->available_memory(); - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, @@ -1390,7 +1416,11 @@ static bool can_do_dna(instance_t &ins) &node_id_allocation_transfer_id, CANARD_TRANSFER_PRIORITY_LOW, &allocation_request[0], - (uint16_t) (uid_size + 1)); + (uint16_t) (uid_size + 1) +#if HAL_CANFD_SUPPORTED + ,false +#endif + ); if (bcast_res < 0) { printf("Could not broadcast ID allocation req; error %d, IF%d\n", bcast_res, ins.index); } @@ -1495,7 +1525,7 @@ void AP_Periph_FW::pwm_hardpoint_update() cmd.command = value; uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer); + uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1523,7 +1553,7 @@ void AP_Periph_FW::hwesc_telem_update() pkt.error_count = t.error_count; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1576,7 +1606,7 @@ void AP_Periph_FW::esc_telem_update() pkt.error_count = 0; uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_esc_Status_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1695,7 +1725,7 @@ void AP_Periph_FW::can_mag_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_ahrs_MagneticFieldStrength_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, @@ -1756,7 +1786,7 @@ void AP_Periph_FW::can_battery_update(void) #endif //defined(HAL_PERIPH_BATTERY_SKIP_NAME) uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {}; - const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer); + const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, @@ -1845,7 +1875,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_gnss_Fix_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX_ID, @@ -1935,7 +1965,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX2_ID, @@ -1953,7 +1983,7 @@ void AP_Periph_FW::can_gps_update(void) aux.vdop = gps.get_vdop() * 0.01; uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer); + uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -1980,7 +2010,7 @@ void AP_Periph_FW::can_gps_update(void) } uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer); + const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE, ARDUPILOT_GNSS_STATUS_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -2010,7 +2040,7 @@ void AP_Periph_FW::send_moving_baseline_msg() mbldata.data.len = len; memcpy(mbldata.data.data, data, len); uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer); + const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !periph.canfdout()); #if HAL_NUM_CAN_IFACES >= 2 if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { @@ -2021,7 +2051,11 @@ void AP_Periph_FW::send_moving_baseline_msg() tid_ptr, CANARD_TRANSFER_PRIORITY_LOW, &buffer[0], - total_size); + total_size +#if HAL_CANFD_SUPPORTED + ,canfdout() +#endif + ); } else #endif { @@ -2056,7 +2090,7 @@ void AP_Periph_FW::send_relposheading_msg() { relpos.reported_heading_acc_deg = reported_heading_acc; relpos.reported_heading_acc_available = true; uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE] {}; - const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer); + const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE, ARDUPILOT_GNSS_RELPOSHEADING_ID, CANARD_TRANSFER_PRIORITY_LOW, @@ -2093,7 +2127,7 @@ void AP_Periph_FW::can_baro_update(void) pkt.static_pressure_variance = 0; // should we make this a parameter? uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_StaticPressure_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, @@ -2108,7 +2142,7 @@ void AP_Periph_FW::can_baro_update(void) pkt.static_temperature_variance = 0; // should we make this a parameter? uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_StaticTemperature_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, @@ -2169,7 +2203,7 @@ void AP_Periph_FW::can_airspeed_update(void) pkt.pitot_temperature = nanf(""); uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, @@ -2247,7 +2281,7 @@ void AP_Periph_FW::can_rangefinder_update(void) pkt.range = dist_cm * 0.01; uint8_t buffer[UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE] {}; - uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer); + uint16_t total_size = uavcan_equipment_range_sensor_Measurement_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, @@ -2308,7 +2342,7 @@ void AP_Periph_FW::can_send_ADSB(struct __mavlink_adsb_vehicle_t &msg) pkt.baro_valid = (msg.flags & 0x0100) != 0; uint8_t buffer[ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_MAX_SIZE] {}; - uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer); + uint16_t total_size = ardupilot_equipment_trafficmonitor_TrafficReport_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_SIGNATURE, ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ID, @@ -2329,7 +2363,7 @@ void can_printf(const char *fmt, ...) va_end(ap); pkt.text.len = MIN(n, sizeof(pkt.text.data)); - uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); + uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer, !periph.canfdout()); canard_broadcast(UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, From 79f55e7aad771edd4e1d1872c83a2caf466c03e0 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 6 Feb 2022 18:46:22 +0530 Subject: [PATCH 0227/3101] waf: enable option for setting TAO state --- Tools/ardupilotwaf/boards.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 7d5db8bbd2a..91393245ce1 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -941,7 +941,7 @@ def configure_env(self, cfg, env): if cfg.env.HAL_CANFD_SUPPORTED: env.DEFINES.update(CANARD_ENABLE_CANFD=1) else: - env.DEFINES.update(CANARD_ENABLE_CANFD=0) + env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) if cfg.options.Werror or cfg.env.CC_VERSION in gcc_whitelist: cfg.msg("Enabling -Werror", "yes") From 1f43c79bfd3bed98949f7061a504cfadd5e37efd Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 10 Feb 2022 13:10:44 +0530 Subject: [PATCH 0228/3101] AP_HAL: add support for taking in canfd bitrate in caniface --- libraries/AP_HAL/CANIface.cpp | 80 ++++++++++++++++++++++++++++++++++ libraries/AP_HAL/CANIface.h | 81 +++-------------------------------- 2 files changed, 87 insertions(+), 74 deletions(-) diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp index bce2ed6c1be..e5a712390d1 100644 --- a/libraries/AP_HAL/CANIface.cpp +++ b/libraries/AP_HAL/CANIface.cpp @@ -93,3 +93,83 @@ bool AP_HAL::CANIface::register_frame_callback(FrameCb cb) frame_callback = cb; return true; } + +AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame) : + id(can_id), + canfd(canfd_frame) +{ + if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) { + return; + } + memcpy(this->data, can_data, data_len); + if (data_len <= 8) { + dlc = data_len; + } else if (!canfd) { + dlc = 8; + } else { + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (data_len <= 12) { + dlc = 9; + } else if (data_len <= 16) { + dlc = 10; + } else if (data_len <= 20) { + dlc = 11; + } else if (data_len <= 24) { + dlc = 12; + } else if (data_len <= 32) { + dlc = 13; + } else if (data_len <= 48) { + dlc = 14; + } else if (data_len <= 64) { + dlc = 15; + } + } +} + +uint8_t AP_HAL::CANFrame::dataLengthToDlc(uint8_t data_length) +{ + if (data_length <= 8) { + return data_length; + } else if (data_length <= 12) { + return 9; + } else if (data_length <= 16) { + return 10; + } else if (data_length <= 20) { + return 11; + } else if (data_length <= 24) { + return 12; + } else if (data_length <= 32) { + return 13; + } else if (data_length <= 48) { + return 14; + } + return 15; +} + + +uint8_t AP_HAL::CANFrame::dlcToDataLength(uint8_t dlc) +{ + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (dlc <= 8) { + return dlc; + } else if (dlc == 9) { + return 12; + } else if (dlc == 10) { + return 16; + } else if (dlc == 11) { + return 20; + } else if (dlc == 12) { + return 24; + } else if (dlc == 13) { + return 32; + } else if (dlc == 14) { + return 48; + } + return 64; +} diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 8246168da9a..5abb17b1e37 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -56,40 +56,7 @@ struct AP_HAL::CANFrame { memset(data,0, MaxDataLen); } - CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false) : - id(can_id), - canfd(canfd_frame) - { - if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) { - return; - } - memcpy(this->data, can_data, data_len); - if (data_len <= 8) { - dlc = data_len; - } else if (!canfd) { - dlc = 8; - } else { - /* - Data Length Code 9 10 11 12 13 14 15 - Number of data bytes 12 16 20 24 32 48 64 - */ - if (data_len <= 12) { - dlc = 9; - } else if (data_len <= 16) { - dlc = 10; - } else if (data_len <= 20) { - dlc = 11; - } else if (data_len <= 24) { - dlc = 12; - } else if (data_len <= 32) { - dlc = 13; - } else if (data_len <= 48) { - dlc = 14; - } else if (data_len <= 64) { - dlc = 15; - } - } - } + CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len, bool canfd_frame = false); bool operator!=(const CANFrame& rhs) const { @@ -122,47 +89,9 @@ struct AP_HAL::CANFrame { return canfd; } - static uint8_t dlcToDataLength(uint8_t dlc) { - /* - Data Length Code 9 10 11 12 13 14 15 - Number of data bytes 12 16 20 24 32 48 64 - */ - if (dlc <= 8) { - return dlc; - } else if (dlc == 9) { - return 12; - } else if (dlc == 10) { - return 16; - } else if (dlc == 11) { - return 20; - } else if (dlc == 12) { - return 24; - } else if (dlc == 13) { - return 32; - } else if (dlc == 14) { - return 48; - } - return 64; - } + static uint8_t dlcToDataLength(uint8_t dlc); - static uint8_t dataLengthToDlc(uint8_t data_length) { - if (data_length <= 8) { - return data_length; - } else if (data_length <= 12) { - return 9; - } else if (data_length <= 16) { - return 10; - } else if (data_length <= 20) { - return 11; - } else if (data_length <= 24) { - return 12; - } else if (data_length <= 32) { - return 13; - } else if (data_length <= 48) { - return 14; - } - return 15; - } + static uint8_t dataLengthToDlc(uint8_t data_length); /** * CAN frame arbitration rules, particularly STD vs EXT: * Marco Di Natale - "Understanding and using the Controller Area Network" @@ -234,6 +163,10 @@ class AP_HAL::CANIface } }; + virtual bool init(const uint32_t bitrate, const OperatingMode mode, const uint32_t fdbitrate) { + return init(bitrate, mode); + } + // Initialise the interface with hardware configuration required to start comms. virtual bool init(const uint32_t bitrate, const OperatingMode mode) = 0; From cb1bc4e613c28ce81e27d9f2fd0e712f808a53a7 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 10 Feb 2022 13:09:27 +0530 Subject: [PATCH 0229/3101] AP_CANManager: add support for enabling CANFD --- libraries/AP_CANManager/AP_CANIfaceParams.cpp | 8 ++++++++ libraries/AP_CANManager/AP_CANManager.cpp | 4 ++-- libraries/AP_CANManager/AP_CANManager.h | 1 + libraries/AP_CANManager/AP_SLCANIface.cpp | 6 ++++-- libraries/AP_CANManager/AP_SLCANIface.h | 3 +-- 5 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANIfaceParams.cpp b/libraries/AP_CANManager/AP_CANIfaceParams.cpp index 32276e24816..e50990e7787 100644 --- a/libraries/AP_CANManager/AP_CANIfaceParams.cpp +++ b/libraries/AP_CANManager/AP_CANIfaceParams.cpp @@ -35,6 +35,14 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("BITRATE", 2, AP_CANManager::CANIface_Params, _bitrate, 1000000), +#if HAL_CANFD_SUPPORTED + // @Param: BITRATE + // @DisplayName: Bitrate of CAN interface + // @Description: Bit rate can be set up to from 10000 to 8000000 + // @Values: 2:2M, 5:5M, 8:8M + // @User: Advanced + AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, 8), +#endif // Index 3 occupied by Param: DEBUG AP_GROUPEND }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 8bf0c027e24..7d7fc9eac98 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -167,10 +167,10 @@ void AP_CANManager::init() // instead of a driver if (_slcan_interface.init_passthrough(i)) { // we have slcan bridge setup pass that on as can iface - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::NormalMode); + can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); iface = &_slcan_interface; } else { - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, AP_HAL::CANIface::NormalMode); + can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); } if (!can_initialised) { diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 730bdeda1f6..0d820486f0f 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -132,6 +132,7 @@ class AP_CANManager private: AP_Int8 _driver_number; AP_Int32 _bitrate; + AP_Int32 _fdbitrate; }; //Parameter Interface for CANDrivers diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp index f17bf294201..24fb97e9997 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.cpp +++ b/libraries/AP_CANManager/AP_SLCANIface.cpp @@ -153,7 +153,6 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) return push_Frame(f); } -#if HAL_CANFD_SUPPORTED /** * General frame format: * @@ -161,6 +160,9 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) */ bool SLCAN::CANIface::handle_FDFrameDataExt(const char* cmd) { +#if HAL_CANFD_SUPPORTED + return false; +#else AP_HAL::CANFrame f {}; hex2nibble_error = false; f.canfd = true; @@ -188,8 +190,8 @@ bool SLCAN::CANIface::handle_FDFrameDataExt(const char* cmd) return false; } return push_Frame(f); -} #endif //#if HAL_CANFD_SUPPORTED +} bool SLCAN::CANIface::handle_FrameDataStd(const char* cmd) { diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h index b00e5428782..87771170c92 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.h +++ b/libraries/AP_CANManager/AP_SLCANIface.h @@ -52,9 +52,8 @@ class CANIface: public AP_HAL::CANIface bool handle_FrameDataStd(const char* cmd); bool handle_FrameDataExt(const char* cmd); -#if HAL_CANFD_SUPPORTED bool handle_FDFrameDataExt(const char* cmd); -#endif + // Parsing bytes received on the serial port inline void addByte(const uint8_t byte); From 47dd964e638c4b1db9a83622bc72747aaa5c1220 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 10 Feb 2022 13:10:05 +0530 Subject: [PATCH 0230/3101] AP_HAL_ChibiOS: add init method to take in separate canfd bitrate --- libraries/AP_HAL/CANIface.h | 2 +- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 29 +++++++++++++------------ libraries/AP_HAL_ChibiOS/CANFDIface.h | 6 ++++- 3 files changed, 21 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 5abb17b1e37..5319708a915 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -163,7 +163,7 @@ class AP_HAL::CANIface } }; - virtual bool init(const uint32_t bitrate, const OperatingMode mode, const uint32_t fdbitrate) { + virtual bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) { return init(bitrate, mode); } diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2258f0f0405..2afec8938ca 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -554,7 +554,7 @@ uint16_t CANIface::getNumFilters() const } bool CANIface::clock_init_ = false; -bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) +bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) { Debug("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); if (self_index_ > HAL_NUM_CAN_IFACES) { @@ -669,23 +669,24 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos) | (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); - if (!computeTimings(bitrate*8, timings)) { // Do 8x fast Data transmission for CAN FD frames - can_->CCCR &= ~FDCAN_CCCR_INIT; - uint32_t while_start_ms = AP_HAL::millis(); - while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { - if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { - return false; + if (fdbitrate) { + if (!computeTimings(fdbitrate, timings)) { // Do 8x fast Data transmission for CAN FD frames + can_->CCCR &= ~FDCAN_CCCR_INIT; + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } } + return false; } - return false; + Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", + unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2)); + can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | + (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | + (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); } - Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", - unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2)); - can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | - (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | - (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); - //RX Config #if defined(STM32H7) can_->RXESC = 0; //Set for 8Byte Frames diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 4d1dcb2f55a..67c5acc9c8b 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -178,7 +178,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface static uint8_t next_interface; // Initialise CAN Peripheral - bool init(const uint32_t bitrate, const OperatingMode mode) override; + bool init(const uint32_t bitrate, const OperatingMode mode) override { + return init(bitrate, 0, mode); + } + + bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) override; // Put frame into Tx FIFO returns negative on error, 0 on buffer full, // 1 on successfully pushing a frame into FIFO From c0188fbf0e4cf48e6fd6c6ed40248d2a85af7dec Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 15 Feb 2022 13:33:59 +0530 Subject: [PATCH 0231/3101] AP_CANManager: disable CANTester by default --- libraries/AP_CANManager/AP_CANDriver.cpp | 2 +- libraries/AP_CANManager/AP_CANManager.cpp | 2 +- libraries/AP_CANManager/AP_CANTester.cpp | 2 +- libraries/AP_CANManager/AP_CANTester.h | 6 +++++- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index a458065145e..1a919268a9d 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -47,7 +47,7 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { AP_SUBGROUPPTR(_kdecan, "KDE_", 3, AP_CANManager::CANDriver_Params, AP_KDECAN), #endif -#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES +#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER // @Group: TST_ // @Path: ../AP_CANManager/AP_CANTester.cpp AP_SUBGROUPPTR(_testcan, "TST_", 4, AP_CANManager::CANDriver_Params, CANTester), diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 7d7fc9eac98..3ac6f087c4d 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -234,7 +234,7 @@ void AP_CANManager::init() AP_Param::load_object_from_eeprom((AP_PiccoloCAN*)_drivers[drv_num], AP_PiccoloCAN::var_info); #endif } else if (drv_type[drv_num] == Driver_Type_CANTester) { -#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES +#if HAL_NUM_CAN_IFACES > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER _drivers[drv_num] = _drv_param[drv_num]._testcan = new CANTester; if (_drivers[drv_num] == nullptr) { diff --git a/libraries/AP_CANManager/AP_CANTester.cpp b/libraries/AP_CANManager/AP_CANTester.cpp index 71b50814a46..688d6d919c5 100644 --- a/libraries/AP_CANManager/AP_CANTester.cpp +++ b/libraries/AP_CANManager/AP_CANTester.cpp @@ -20,8 +20,8 @@ #include #include "AP_CANManager.h" -#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED #include "AP_CANTester.h" +#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER #include #include #include diff --git a/libraries/AP_CANManager/AP_CANTester.h b/libraries/AP_CANManager/AP_CANTester.h index 1a8c939e904..37e39996ba7 100644 --- a/libraries/AP_CANManager/AP_CANTester.h +++ b/libraries/AP_CANManager/AP_CANTester.h @@ -18,7 +18,11 @@ #include "AP_CANDriver.h" #include #include -#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED +#ifndef HAL_ENABLE_CANTESTER +#define HAL_ENABLE_CANTESTER 0 +#endif + +#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER class CANTester : public AP_CANDriver { From e99639267115b68828c88751d33f4e4606bec236 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 15 Feb 2022 13:34:50 +0530 Subject: [PATCH 0232/3101] GCS_MAVLink: disable CANTester by default --- libraries/GCS_MAVLink/GCS_Common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 7c69a52fd22..d9c2362c9d9 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4060,7 +4060,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_ } case AP_CANManager::Driver_Type_CANTester: { // To be replaced with macro saying if KDECAN library is included -#if (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)) && (HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES) +#if (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)) && (HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER) CANTester *cantester = CANTester::get_cantester(i); if (cantester != nullptr) { From 1f6c380f6962768ac171d0e68d731822c9f891f1 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 15 Feb 2022 17:44:33 +0530 Subject: [PATCH 0233/3101] AP_Bootloader: add value for extra argument in encode methods --- Tools/AP_Bootloader/can.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Bootloader/can.cpp b/Tools/AP_Bootloader/can.cpp index 76e39747595..81b0413f16a 100644 --- a/Tools/AP_Bootloader/can.cpp +++ b/Tools/AP_Bootloader/can.cpp @@ -152,7 +152,7 @@ static void handle_get_node_info(CanardInstance* ins, pkt.name.len = strlen(CAN_APP_NODE_NAME); pkt.name.data = (uint8_t *)name; - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, true); canardRequestOrRespond(ins, transfer->source_node_id, @@ -280,7 +280,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* uavcan_protocol_file_BeginFirmwareUpdateResponse reply {}; reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, true); canardRequestOrRespond(ins, transfer->source_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, @@ -592,7 +592,7 @@ static void send_node_status(void) uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; node_status.uptime_sec = AP_HAL::millis() / 1000U; - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, true); static uint8_t transfer_id; // Note that the transfer ID variable MUST BE STATIC (or heap-allocated)! From 47e710c21a30d0ba809b029db52d4c5d26aac450 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 15 Feb 2022 19:58:24 +0530 Subject: [PATCH 0234/3101] waf: enable tao option for sitl_periph_gps --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 91393245ce1..31a3f7bb04d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -677,6 +677,7 @@ def configure_env(self, cfg, env): HAL_MISSION_ENABLED = 0, HAL_RALLY_ENABLED = 0, HAL_SCHEDULER_ENABLED = 0, + CANARD_ENABLE_TAO_OPTION = 1, ) # libcanard is written for 32bit platforms env.CXXFLAGS += [ From 356943553176d29a8155888dac47d5f02556d6fe Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 16 Feb 2022 11:36:49 +0530 Subject: [PATCH 0235/3101] AP_UAVCAN: ensure that we publish allocation messages in STD CAN format --- libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index ed4052cd7e2..995a223b1f5 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -276,7 +276,7 @@ bool AP_UAVCAN_DNA_Server::init(AP_UAVCAN *ap_uavcan) server_state = HEALTHY; //Setup publisher for this driver index - allocation_pub[driver_index] = new uavcan::Publisher(*_node); + allocation_pub[driver_index] = new uavcan::Publisher(*_node, true); if (allocation_pub[driver_index] == nullptr) { AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: allocation_pub[%d]", driver_index); } From 90a660b4452c1d1dae7aad635132e6421eab3804 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Fri, 18 Feb 2022 14:57:27 +0530 Subject: [PATCH 0236/3101] AP_HAL_ChibiOS: update clock tree to have FDCAN Base clock at 80MHz --- .../hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc | 29 +++++++++---------- .../hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc | 29 +++++++++---------- .../hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc | 29 +++++++++---------- .../hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc | 29 +++++++++---------- .../STM32CubeConf/H7-no-xtal/H7-no-xtal.ioc | 29 +++++++++---------- .../hwdef/common/stm32h7_mcuconf.h | 10 +++---- 6 files changed, 75 insertions(+), 80 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc index 1a9138e8fa2..e95f668d462 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc @@ -510,7 +510,7 @@ RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=100000000 +RCC.DFSDMACLkFreq_Value=80000000 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=2 RCC.DIVM2=2 @@ -522,8 +522,8 @@ RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=180000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 -RCC.DIVQ1=8 -RCC.DIVQ1Freq_Value=100000000 +RCC.DIVQ1=10 +RCC.DIVQ1Freq_Value=80000000 RCC.DIVQ2=5 RCC.DIVQ2Freq_Value=72000000 RCC.DIVQ3=6 @@ -533,8 +533,7 @@ RCC.DIVR2=1 RCC.DIVR2Freq_Value=360000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 -RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL2 -RCC.FDCANFreq_Value=72000000 +RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -547,7 +546,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANCLockSelection,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL1_VCO_SEL-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL1_VCO_SEL-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -566,13 +565,13 @@ RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=100000000 -RCC.SAI23Freq_Value=100000000 -RCC.SAI4AFreq_Value=100000000 -RCC.SAI4BFreq_Value=100000000 -RCC.SDMMCFreq_Value=100000000 -RCC.SPDIFRXFreq_Value=100000000 -RCC.SPI123Freq_Value=100000000 +RCC.SAI1Freq_Value=80000000 +RCC.SAI23Freq_Value=80000000 +RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BFreq_Value=80000000 +RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 @@ -597,12 +596,12 @@ SH.ADCx_INP4.0=ADC1_INP4,IN4-Single-Ended SH.ADCx_INP4.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,Input_Capture1_from_TI1 SH.S_TIM1_CH1.ConfNb=1 -SPI1.CalculateBaudRate=50.0 MBits/s +SPI1.CalculateBaudRate=40.0 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -SPI2.CalculateBaudRate=50.0 MBits/s +SPI2.CalculateBaudRate=40.0 MBits/s SPI2.Direction=SPI_DIRECTION_2LINES SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI2.Mode=SPI_MODE_MASTER diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc index 819e948d241..721b69d23ec 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc @@ -510,7 +510,7 @@ RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=100000000 +RCC.DFSDMACLkFreq_Value=80000000 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=3 RCC.DIVM2=2 @@ -522,8 +522,8 @@ RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=180000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 -RCC.DIVQ1=8 -RCC.DIVQ1Freq_Value=100000000 +RCC.DIVQ1=10 +RCC.DIVQ1Freq_Value=80000000 RCC.DIVQ2=5 RCC.DIVQ2Freq_Value=72000000 RCC.DIVQ3=6 @@ -533,8 +533,7 @@ RCC.DIVR2=1 RCC.DIVR2Freq_Value=360000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 -RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL2 -RCC.FDCANFreq_Value=72000000 +RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -547,7 +546,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANCLockSelection,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -563,13 +562,13 @@ RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=100000000 -RCC.SAI23Freq_Value=100000000 -RCC.SAI4AFreq_Value=100000000 -RCC.SAI4BFreq_Value=100000000 -RCC.SDMMCFreq_Value=100000000 -RCC.SPDIFRXFreq_Value=100000000 -RCC.SPI123Freq_Value=100000000 +RCC.SAI1Freq_Value=80000000 +RCC.SAI23Freq_Value=80000000 +RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BFreq_Value=80000000 +RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 @@ -594,12 +593,12 @@ SH.ADCx_INP4.0=ADC1_INP4,IN4-Single-Ended SH.ADCx_INP4.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,Input_Capture1_from_TI1 SH.S_TIM1_CH1.ConfNb=1 -SPI1.CalculateBaudRate=50.0 MBits/s +SPI1.CalculateBaudRate=40.0 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -SPI2.CalculateBaudRate=50.0 MBits/s +SPI2.CalculateBaudRate=40.0 MBits/s SPI2.Direction=SPI_DIRECTION_2LINES SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI2.Mode=SPI_MODE_MASTER diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc index 7121819484a..f324f458d9f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc @@ -510,7 +510,7 @@ RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=100000000 +RCC.DFSDMACLkFreq_Value=80000000 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=2 RCC.DIVM2=5 @@ -522,8 +522,8 @@ RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=180000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=80000000 -RCC.DIVQ1=8 -RCC.DIVQ1Freq_Value=100000000 +RCC.DIVQ1=10 +RCC.DIVQ1Freq_Value=80000000 RCC.DIVQ2=5 RCC.DIVQ2Freq_Value=72000000 RCC.DIVQ3=5 @@ -533,8 +533,7 @@ RCC.DIVR2=1 RCC.DIVR2Freq_Value=360000000 RCC.DIVR3=8 RCC.DIVR3Freq_Value=30000000 -RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL2 -RCC.FDCANFreq_Value=72000000 +RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -547,7 +546,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=30000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=30000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANCLockSelection,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -563,13 +562,13 @@ RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=100000000 -RCC.SAI23Freq_Value=100000000 -RCC.SAI4AFreq_Value=100000000 -RCC.SAI4BFreq_Value=100000000 -RCC.SDMMCFreq_Value=100000000 -RCC.SPDIFRXFreq_Value=100000000 -RCC.SPI123Freq_Value=100000000 +RCC.SAI1Freq_Value=80000000 +RCC.SAI23Freq_Value=80000000 +RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BFreq_Value=80000000 +RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 @@ -594,12 +593,12 @@ SH.ADCx_INP4.0=ADC1_INP4,IN4-Single-Ended SH.ADCx_INP4.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,Input_Capture1_from_TI1 SH.S_TIM1_CH1.ConfNb=1 -SPI1.CalculateBaudRate=50.0 MBits/s +SPI1.CalculateBaudRate=40.0 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -SPI2.CalculateBaudRate=50.0 MBits/s +SPI2.CalculateBaudRate=40.0 MBits/s SPI2.Direction=SPI_DIRECTION_2LINES SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI2.Mode=SPI_MODE_MASTER diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc index 5a67f5bc5b1..862b90d42d7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc @@ -510,7 +510,7 @@ RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=100000000 +RCC.DFSDMACLkFreq_Value=80000000 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=1 RCC.DIVM2=1 @@ -522,8 +522,8 @@ RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=180000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 -RCC.DIVQ1=8 -RCC.DIVQ1Freq_Value=100000000 +RCC.DIVQ1=10 +RCC.DIVQ1Freq_Value=80000000 RCC.DIVQ2=5 RCC.DIVQ2Freq_Value=72000000 RCC.DIVQ3=6 @@ -533,8 +533,7 @@ RCC.DIVR2=1 RCC.DIVR2Freq_Value=360000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 -RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL2 -RCC.FDCANFreq_Value=72000000 +RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -547,7 +546,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANCLockSelection,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -563,13 +562,13 @@ RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=100000000 -RCC.SAI23Freq_Value=100000000 -RCC.SAI4AFreq_Value=100000000 -RCC.SAI4BFreq_Value=100000000 -RCC.SDMMCFreq_Value=100000000 -RCC.SPDIFRXFreq_Value=100000000 -RCC.SPI123Freq_Value=100000000 +RCC.SAI1Freq_Value=80000000 +RCC.SAI23Freq_Value=80000000 +RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BFreq_Value=80000000 +RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 @@ -594,12 +593,12 @@ SH.ADCx_INP4.0=ADC1_INP4,IN4-Single-Ended SH.ADCx_INP4.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,Input_Capture1_from_TI1 SH.S_TIM1_CH1.ConfNb=1 -SPI1.CalculateBaudRate=50.0 MBits/s +SPI1.CalculateBaudRate=40.0 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER -SPI2.CalculateBaudRate=50.0 MBits/s +SPI2.CalculateBaudRate=40.0 MBits/s SPI2.Direction=SPI_DIRECTION_2LINES SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate SPI2.Mode=SPI_MODE_MASTER diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-no-xtal/H7-no-xtal.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-no-xtal/H7-no-xtal.ioc index 1d72c3411e8..21bd5497de5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-no-xtal/H7-no-xtal.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-no-xtal/H7-no-xtal.ioc @@ -507,7 +507,7 @@ RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 -RCC.DFSDMACLkFreq_Value=100000000 +RCC.DFSDMACLkFreq_Value=80000000 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=4 RCC.DIVM2=8 @@ -519,8 +519,8 @@ RCC.DIVP1Freq_Value=400000000 RCC.DIVP2Freq_Value=180000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=80000000 -RCC.DIVQ1=8 -RCC.DIVQ1Freq_Value=100000000 +RCC.DIVQ1=10 +RCC.DIVQ1Freq_Value=80000000 RCC.DIVQ2=5 RCC.DIVQ2Freq_Value=72000000 RCC.DIVQ3=5 @@ -530,8 +530,7 @@ RCC.DIVR2=1 RCC.DIVR2Freq_Value=360000000 RCC.DIVR3=8 RCC.DIVR3Freq_Value=30000000 -RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL2 -RCC.FDCANFreq_Value=72000000 +RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -543,7 +542,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=30000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=30000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANCLockSelection,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2FRACN,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSource,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2FRACN,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSource,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -561,13 +560,13 @@ RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 -RCC.SAI1Freq_Value=100000000 -RCC.SAI23Freq_Value=100000000 -RCC.SAI4AFreq_Value=100000000 -RCC.SAI4BFreq_Value=100000000 -RCC.SDMMCFreq_Value=100000000 -RCC.SPDIFRXFreq_Value=100000000 -RCC.SPI123Freq_Value=100000000 +RCC.SAI1Freq_Value=80000000 +RCC.SAI23Freq_Value=80000000 +RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BFreq_Value=80000000 +RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 @@ -593,13 +592,13 @@ SH.ADCx_INP4.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,Input_Capture1_from_TI1 SH.S_TIM1_CH1.ConfNb=1 SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_2 -SPI1.CalculateBaudRate=50.0 MBits/s +SPI1.CalculateBaudRate=40.0 MBits/s SPI1.Direction=SPI_DIRECTION_2LINES SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler SPI1.Mode=SPI_MODE_MASTER SPI1.VirtualType=VM_MASTER SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_2 -SPI2.CalculateBaudRate=50.0 MBits/s +SPI2.CalculateBaudRate=40.0 MBits/s SPI2.Direction=SPI_DIRECTION_2LINES SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler SPI2.Mode=SPI_MODE_MASTER diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index f0d3cc314c4..13a295014b6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -146,7 +146,7 @@ // no crystal #define STM32_PLL1_DIVN_VALUE 50 #define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 #define STM32_PLL2_DIVN_VALUE 45 @@ -167,7 +167,7 @@ #define STM32_PLL1_DIVN_VALUE 100 #endif #define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 #define STM32_PLL2_DIVN_VALUE 45 @@ -187,7 +187,7 @@ #define STM32_PLL1_DIVN_VALUE 100 #endif #define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 #define STM32_PLL2_DIVN_VALUE 30 @@ -203,7 +203,7 @@ #elif STM32_HSECLK == 25000000U #define STM32_PLL1_DIVN_VALUE 64 #define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 8 +#define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 #define STM32_PLL2_DIVN_VALUE 72 @@ -280,7 +280,7 @@ #define STM32_QSPISEL STM32_QSPISEL_PLL2_R_CK #define STM32_FMCSEL STM32_QSPISEL_HCLK #define STM32_SWPSEL STM32_SWPSEL_PCLK1 -#define STM32_FDCANSEL STM32_FDCANSEL_PLL2_Q_CK +#define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK #define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 #define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK #define STM32_SPI45SEL STM32_SPI45SEL_PCLK2 From 5d11734a74627c7183e709f2af40508b2e15f909 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 10 Feb 2022 22:09:33 +0530 Subject: [PATCH 0237/3101] modules: update DroneCAN/dronecan_dsdlc --- modules/DroneCAN/dronecan_dsdlc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc index 285a56e8274..c00fc457e22 160000 --- a/modules/DroneCAN/dronecan_dsdlc +++ b/modules/DroneCAN/dronecan_dsdlc @@ -1 +1 @@ -Subproject commit 285a56e82741886b2bc7a5878d907aa443337cd8 +Subproject commit c00fc457e22c0b71e3a385f092b519830ef58ea2 From 98c998b22f9426b69288ada4d5813152d89a3811 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 10 Feb 2022 21:52:54 +0530 Subject: [PATCH 0238/3101] modules: update pydronecan --- modules/DroneCAN/pydronecan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan index 4c5b1eb297e..7dd0cbdcd1d 160000 --- a/modules/DroneCAN/pydronecan +++ b/modules/DroneCAN/pydronecan @@ -1 +1 @@ -Subproject commit 4c5b1eb297eadb78ea14f582ccb2b7b4343b9483 +Subproject commit 7dd0cbdcd1d2a6a15cfcb4717e4f25f735173cc9 From bd432d4e229ece67ff058719d961f3b8b29209d5 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 8 Feb 2022 17:24:27 +0530 Subject: [PATCH 0239/3101] modules: update uavcan --- modules/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/uavcan b/modules/uavcan index de3174b0f3a..5eec0220302 160000 --- a/modules/uavcan +++ b/modules/uavcan @@ -1 +1 @@ -Subproject commit de3174b0f3aa9f55bc8ddfa1e7736c6a465cda18 +Subproject commit 5eec0220302dfaa23602d7ee901cde0579fbf9e2 From b3b561f32bdea54e70ca9ad9993b6ac8e4d94e88 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 6 Feb 2022 17:38:50 +0530 Subject: [PATCH 0240/3101] modules: update DroneCAN/libcanard --- modules/DroneCAN/libcanard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard index 95cb5c3b23f..5c2da7dc4c8 160000 --- a/modules/DroneCAN/libcanard +++ b/modules/DroneCAN/libcanard @@ -1 +1 @@ -Subproject commit 95cb5c3b23f5b4b049eac57631b127ed6a2247dc +Subproject commit 5c2da7dc4c8bb59f9aa3f3f2ad506a9820b52a33 From 6320599404f3212e40302bc844ad7b16056bc083 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Feb 2022 09:35:51 +1100 Subject: [PATCH 0241/3101] AP_CANManager: support mavcan with CANFD_FRAME --- libraries/AP_CANManager/AP_CANManager.cpp | 75 +++++++++++++++-------- 1 file changed, 49 insertions(+), 26 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 3ac6f087c4d..589c7fdcda1 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -426,14 +426,29 @@ bool AP_CANManager::handle_can_forward(mavlink_channel_t chan, const mavlink_com */ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) const { - mavlink_can_frame_t p; - mavlink_msg_can_frame_decode(&msg, &p); - if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) { - return; - } const uint16_t timeout_us = 2000; - AP_HAL::CANFrame frame{p.id, p.data, p.len}; - hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN); + switch (msg.msgid) { + case MAVLINK_MSG_ID_CAN_FRAME: { + mavlink_can_frame_t p; + mavlink_msg_can_frame_decode(&msg, &p); + if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) { + return; + } + AP_HAL::CANFrame frame{p.id, p.data, p.len}; + hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN); + break; + } + case MAVLINK_MSG_ID_CANFD_FRAME: { + mavlink_canfd_frame_t p; + mavlink_msg_canfd_frame_decode(&msg, &p); + if (p.bus >= HAL_NUM_CAN_IFACES || hal.can[p.bus] == nullptr) { + return; + } + AP_HAL::CANFrame frame{p.id, p.data, p.len, true}; + hal.can[p.bus]->send(frame, AP_HAL::native_micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN); + break; + } + } } /* @@ -527,7 +542,7 @@ void AP_CANManager::handle_can_filter_modify(const mavlink_message_t &msg) /* handler for CAN frames from the registered callback, sending frames - out as CAN_FRAME messages + out as CAN_FRAME or CANFD_FRAME messages */ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &frame) { @@ -543,26 +558,34 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram can_forward.frame_counter = 0; } WITH_SEMAPHORE(comm_chan_lock(can_forward.chan)); - if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) { - if (can_forward.num_filter_ids != 0) { - // work out ID of this frame - uint16_t id = 0; - if ((frame.id&0xff) != 0) { - // not anonymous - if (frame.id & 0x80) { - // service message - id = uint8_t(frame.id>>16); - } else { - // message frame - id = uint16_t(frame.id>>8); - } - } - if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) { - return; + if (can_forward.filter_ids != nullptr) { + // work out ID of this frame + uint16_t id = 0; + if ((frame.id&0xff) != 0) { + // not anonymous + if (frame.id & 0x80) { + // service message + id = uint8_t(frame.id>>16); + } else { + // message frame + id = uint16_t(frame.id>>8); } } - mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, - bus, frame.dlc, frame.id, const_cast(frame.data)); + if (!bisect_search_uint16(can_forward.filter_ids, can_forward.num_filter_ids, id)) { + return; + } + } + const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); + if (frame.isCanFDFrame()) { + if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) { + mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, + bus, data_len, frame.id, const_cast(frame.data)); + } + } else { + if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) { + mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, + bus, data_len, frame.id, const_cast(frame.data)); + } } } #endif // HAL_GCS_ENABLED From d28cecf25b1fd09f628e5bb89c25bca85452fd4b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Feb 2022 09:34:10 +1100 Subject: [PATCH 0242/3101] GCS_MAVLink: support CANFD_FRAME messages --- libraries/GCS_MAVLink/GCS_Common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d9c2362c9d9..e24e20cb394 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3733,6 +3733,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) break; case MAVLINK_MSG_ID_CAN_FRAME: + case MAVLINK_MSG_ID_CANFD_FRAME: handle_can_frame(msg); break; From cf5d94b81ff0fcc2b413e8aea7f88e7af2588dc8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 17 Feb 2022 21:11:09 +1100 Subject: [PATCH 0243/3101] AP_CANManager: fixed slcan receive of CANFD frames --- libraries/AP_CANManager/AP_SLCANIface.cpp | 17 ++++++++--------- libraries/AP_CANManager/AP_SLCANIface.h | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp index 24fb97e9997..da89d249371 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.cpp +++ b/libraries/AP_CANManager/AP_SLCANIface.cpp @@ -120,10 +120,11 @@ bool SLCAN::CANIface::push_Frame(AP_HAL::CANFrame &frame) * * The emitting functions below are highly optimized for speed. */ -bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) +bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd, bool canfd) { AP_HAL::CANFrame f {}; hex2nibble_error = false; + f.canfd = canfd; f.id = f.FlagEFF | (hex2nibble(cmd[1]) << 28) | (hex2nibble(cmd[2]) << 24) | @@ -133,16 +134,14 @@ bool SLCAN::CANIface::handle_FrameDataExt(const char* cmd) (hex2nibble(cmd[6]) << 8) | (hex2nibble(cmd[7]) << 4) | (hex2nibble(cmd[8]) << 0); - if (cmd[9] < '0' || cmd[9] > ('0' + AP_HAL::CANFrame::NonFDCANMaxDataLen)) { - return false; - } - f.dlc = cmd[9] - '0'; - if (f.dlc > AP_HAL::CANFrame::NonFDCANMaxDataLen) { + f.dlc = hex2nibble(cmd[9]); + if (hex2nibble_error || f.dlc > (canfd?15:8)) { return false; } { const char* p = &cmd[10]; - for (unsigned i = 0; i < f.dlc; i++) { + const uint8_t dlen = AP_HAL::CANFrame::dlcToDataLength(f.dlc); + for (unsigned i = 0; i < dlen; i++) { f.data[i] = (hex2nibble(*p) << 4) | hex2nibble(*(p + 1)); p += 2; } @@ -401,8 +400,8 @@ const char* SLCAN::CANIface::processCommand(char* cmd) /* * High-traffic SLCAN commands go first */ - if (cmd[0] == 'T') { - return handle_FrameDataExt(cmd) ? "Z\r" : "\a"; + if (cmd[0] == 'T' || cmd[0] == 'D') { + return handle_FrameDataExt(cmd, cmd[0]=='D') ? "Z\r" : "\a"; } else if (cmd[0] == 't') { return handle_FrameDataStd(cmd) ? "z\r" : "\a"; } else if (cmd[0] == 'R') { diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h index 87771170c92..00e96eb44bb 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.h +++ b/libraries/AP_CANManager/AP_SLCANIface.h @@ -50,7 +50,7 @@ class CANIface: public AP_HAL::CANIface bool handle_FrameRTRStd(const char* cmd); bool handle_FrameRTRExt(const char* cmd); bool handle_FrameDataStd(const char* cmd); - bool handle_FrameDataExt(const char* cmd); + bool handle_FrameDataExt(const char* cmd, bool canfd); bool handle_FDFrameDataExt(const char* cmd); From 4bdbd377313696e8caac61c7bc0aaedac9689366 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 19 Feb 2022 21:58:52 +0530 Subject: [PATCH 0244/3101] AP_HAL_ChibiOS: use datalength when copying payload into CAN Frames --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2afec8938ca..d648ee5fa81 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -887,7 +887,7 @@ bool CANIface::readRxFIFO(uint8_t fifo_index) frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16; uint8_t *data = (uint8_t*)&frame_ptr[2]; - for (uint8_t i = 0; i < AP_HAL::CANFrame::MaxDataLen; i++) { + for (uint8_t i = 0; i < AP_HAL::CANFrame::dlcToDataLength(frame.dlc); i++) { frame.data[i] = data[i]; } From 28b76474cdc9b9a839318258269125b7fd63ce30 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 1 Mar 2022 14:42:01 +0530 Subject: [PATCH 0245/3101] AP_CANManager: set canfd bitrates using CANFD_SUPPORTED opt also adds option to set as 4MBits/s --- libraries/AP_CANManager/AP_CANIfaceParams.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANIfaceParams.cpp b/libraries/AP_CANManager/AP_CANIfaceParams.cpp index e50990e7787..2e1dc2c9335 100644 --- a/libraries/AP_CANManager/AP_CANIfaceParams.cpp +++ b/libraries/AP_CANManager/AP_CANIfaceParams.cpp @@ -38,10 +38,10 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = { #if HAL_CANFD_SUPPORTED // @Param: BITRATE // @DisplayName: Bitrate of CAN interface - // @Description: Bit rate can be set up to from 10000 to 8000000 - // @Values: 2:2M, 5:5M, 8:8M + // @Description: Bit rate can be set up to from 1000000 to 8000000 + // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M // @User: Advanced - AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, 8), + AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, HAL_CANFD_SUPPORTED), #endif // Index 3 occupied by Param: DEBUG AP_GROUPEND From 29673de4f3c6d768c41fcb99e53eed42445dbb4f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 23 Feb 2022 13:51:20 +0530 Subject: [PATCH 0246/3101] AP_Periph: add parameters for setting up FD Bitrates --- Tools/AP_Periph/Parameters.cpp | 32 ++++++++++++++++++++++++++++---- Tools/AP_Periph/Parameters.h | 3 +++ Tools/AP_Periph/can.cpp | 4 ++++ 3 files changed, 35 insertions(+), 4 deletions(-) diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 32569804340..a16b49e8b9f 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -124,6 +124,34 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), #endif +#if HAL_CANFD_SUPPORTED + // @Param: CAN_FDMODE + // @DisplayName: Enable CANFD mode + // @Description: Enabling this option sets the CAN bus to be in CANFD mode with BRS. + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + // @RebootRequired: True + GSCALAR(can_fdmode, "CAN_FDMODE", 0), + + // @Param: CAN_FDBAUDRATE + // @DisplayName: Set up bitrate for data section on CAN1 + // @Description: This sets the bitrate for the data section of CAN1. + // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M + // @User: Advanced + // @RebootRequired: True + GARRAY(can_fdbaudrate, 0, "CAN_FDBAUDRATE", HAL_CANFD_SUPPORTED), + +#if HAL_NUM_CAN_IFACES >= 2 + // @Param: CAN2_FDBAUDRATE + // @DisplayName: Set up bitrate for data section on CAN2 + // @Description: This sets the bitrate for the data section of CAN2. + // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M + // @User: Advanced + // @RebootRequired: True + GARRAY(can_fdbaudrate, 1, "CAN2_FDBAUDRATE", HAL_CANFD_SUPPORTED), +#endif +#endif + #if !defined(HAL_NO_FLASH_SUPPORT) && !defined(HAL_NO_ROMFS_SUPPORT) // @Param: FLASH_BOOTLOADER // @DisplayName: Trigger bootloader update @@ -375,10 +403,6 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(scripting, "SCR_", AP_Scripting), #endif -#if HAL_CANFD_SUPPORTED - // can node FD Out mode - GSCALAR(can_fdmode, "CAN_FDMODE", 0), -#endif AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 2bc7c46d934..412f200451d 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -52,6 +52,8 @@ class Parameters { k_param_scripting, k_param_esc_telem_port, k_param_can_fdmode, + k_param_can_fdbaudrate0, + k_param_can_fdbaudrate1, }; AP_Int16 format_version; @@ -126,6 +128,7 @@ class Parameters { #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; + AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES]; #else static constexpr uint8_t can_fdmode = 0; #endif diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 9d8a7b4949c..3099e8b494b 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1470,7 +1470,11 @@ void AP_Periph_FW::can_start() CANSensor::set_periph(i, can_protocol_cached[i], can_iface_periph[i]); #endif if (can_iface_periph[i] != nullptr) { +#if HAL_CANFD_SUPPORTED + can_iface_periph[i]->init(g.can_baudrate[i], g.can_fdbaudrate[i]*1000000U, AP_HAL::CANIface::NormalMode); +#else can_iface_periph[i]->init(g.can_baudrate[i], AP_HAL::CANIface::NormalMode); +#endif } canardInit(&instances[i].canard, (uint8_t *)instances[i].canard_memory_pool, sizeof(instances[i].canard_memory_pool), onTransferReceived, shouldAcceptTransfer, NULL); From faf0c0d7c33201fd148c2f5c24309b31dc07b2b1 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 1 Mar 2022 14:45:20 +0530 Subject: [PATCH 0247/3101] AP_HAL_ChibiOS: set canfd bitrates using CANFD_SUPPORTED opt --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 32 +++++++++++++------ libraries/AP_HAL_ChibiOS/CANFDIface.h | 7 ++-- .../AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat | 3 +- .../hwdef/scripts/chibios_hwdef.py | 16 ++++++---- 4 files changed, 39 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index d648ee5fa81..84837325f6a 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -309,6 +309,7 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing return false; } + out_timings.sample_point_permill = solution.sample_point_permill; out_timings.prescaler = uint16_t(prescaler - 1U); out_timings.sjw = 0; // Which means one out_timings.bs1 = uint8_t(solution.bs1 - 1); @@ -647,8 +648,6 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper /* * CAN timings for this bitrate */ - Timings timings; - if (!computeTimings(bitrate, timings)) { can_->CCCR &= ~FDCAN_CCCR_INIT; uint32_t while_start_ms = AP_HAL::millis(); @@ -659,18 +658,18 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper } return false; } + _bitrate = bitrate; Debug("Timings: presc=%u sjw=%u bs1=%u bs2=%u\n", unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); //setup timing register - //TODO: Do timing calculations for FDCAN can_->NBTP = ((timings.sjw << FDCAN_NBTP_NSJW_Pos) | (timings.bs1 << FDCAN_NBTP_NTSEG1_Pos) | (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos) | (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); if (fdbitrate) { - if (!computeTimings(fdbitrate, timings)) { // Do 8x fast Data transmission for CAN FD frames + if (!computeTimings(fdbitrate, fdtimings)) { can_->CCCR &= ~FDCAN_CCCR_INIT; uint32_t while_start_ms = AP_HAL::millis(); while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { @@ -680,11 +679,12 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper } return false; } + _fdbitrate = fdbitrate; Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", - unsigned(timings.prescaler), unsigned(timings.bs1), unsigned(timings.bs2)); - can_->DBTP = ((timings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | - (timings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | - (timings.prescaler << FDCAN_DBTP_DBRP_Pos)); + unsigned(fdtimings.prescaler), unsigned(fdtimings.bs1), unsigned(fdtimings.bs2)); + can_->DBTP = ((fdtimings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | + (fdtimings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | + (fdtimings.prescaler << FDCAN_DBTP_DBRP_Pos)); } //RX Config @@ -1086,7 +1086,14 @@ bool CANIface::select(bool &read, bool &write, void CANIface::get_stats(ExpandingString &str) { CriticalSectionLocker lock; - str.printf("tx_requests: %lu\n" + str.printf("------- Clock Config -------\n" + "CAN_CLK_FREQ: %luMHz\n" + "Std Timings: bitrate=%lu presc=%u\n" + "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n" + "FD Timings: bitrate=%lu presc=%u\n" + "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n" + "------- CAN Interface Stats -------\n" + "tx_requests: %lu\n" "tx_rejected: %lu\n" "tx_overflow: %lu\n" "tx_success: %lu\n" @@ -1101,6 +1108,13 @@ void CANIface::get_stats(ExpandingString &str) "fdf_rx: %lu\n" "fdf_tx_req: %lu\n" "fdf_tx: %lu\n", + STM32_FDCANCLK/1000000UL, + _bitrate, unsigned(timings.prescaler), + unsigned(timings.sjw), unsigned(timings.bs1), + unsigned(timings.bs2), timings.sample_point_permill/10.0f, + _fdbitrate, unsigned(fdtimings.prescaler), + unsigned(fdtimings.sjw), unsigned(fdtimings.bs1), + unsigned(fdtimings.bs2), timings.sample_point_permill/10.0f, stats.tx_requests, stats.tx_rejected, stats.tx_overflow, diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 67c5acc9c8b..6e92fe395b2 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -91,13 +91,15 @@ class ChibiOS::CANIface : public AP_HAL::CANIface } MessageRam_; struct Timings { + uint16_t sample_point_permill; uint16_t prescaler; uint8_t sjw; uint8_t bs1; uint8_t bs2; Timings() - : prescaler(0) + : sample_point_permill(0) + , prescaler(0) , sjw(0) , bs1(0) , bs2(0) @@ -150,7 +152,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface static bool clock_init_; bool _detected_bus_off; - + Timings timings, fdtimings; + uint32_t _bitrate, _fdbitrate; struct { uint32_t tx_requests; uint32_t tx_rejected; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index aac0e9b47ed..aedc8dd0990 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat @@ -18,7 +18,8 @@ APJ_BOARD_ID 140 FLASH_SIZE_KB 2048 -CANFD_SUPPORTED 1 +# supports upto 8MBits/s +CANFD_SUPPORTED 8 # with 2M flash we can afford to optimize for speed env OPTIMIZE -O2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 697bcdcb377..226e874f1d6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -689,14 +689,16 @@ def enable_can(f): if 'CAN' in bytype and mcu_type.startswith("STM32F3"): f.write('#define CAN1_BASE CAN_BASE\n') env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list)) - canfd_supported = get_config('CANFD_SUPPORTED', 0, required=False) - if canfd_supported: - f.write('#define HAL_CANFD_SUPPORTED 1\n') - env_vars['HAL_CANFD_SUPPORTED'] = 1 - else: - f.write('#define HAL_CANFD_SUPPORTED 0\n') - env_vars['HAL_CANFD_SUPPORTED'] = 0 + if mcu_series.startswith("STM32H7") and not args.bootloader: + # set maximum supported canfd bit rate in MBits/sec + canfd_supported = int(get_config('CANFD_SUPPORTED', 0, default=4, required=False)) + f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported) + env_vars['HAL_CANFD_SUPPORTED'] = canfd_supported + else: + canfd_supported = int(get_config('CANFD_SUPPORTED', 0, default=0, required=False)) + f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported) + env_vars['HAL_CANFD_SUPPORTED'] = canfd_supported def has_sdcard_spi(): '''check for sdcard connected to spi bus''' From 586568007057077986a22c5a6620b9fa18c25b71 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 1 Mar 2022 22:17:00 +0530 Subject: [PATCH 0248/3101] AP_UAVCAN: move pool allocator to cpp --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 30 +++++++++++++++++++++++++++--- libraries/AP_UAVCAN/AP_UAVCAN.h | 25 +------------------------ 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 26e62ae14da..2653d928967 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -62,6 +62,29 @@ extern const AP_HAL::HAL& hal; +#ifndef UAVCAN_NODE_POOL_SIZE +#if HAL_CANFD_SUPPORTED +#define UAVCAN_NODE_POOL_SIZE 16384 +#else +#define UAVCAN_NODE_POOL_SIZE 8192 +#endif +#endif + +#if HAL_CANFD_SUPPORTED +#define UAVCAN_STACK_SIZE 8192 +#else +#define UAVCAN_STACK_SIZE 4096 +#endif + + +#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE +#if HAL_CANFD_SUPPORTED +#define UAVCAN_NODE_POOL_BLOCK_SIZE 128 +#else +#define UAVCAN_NODE_POOL_BLOCK_SIZE 64 +#endif +#endif + #define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0) // Translation of all messages from UAVCAN structures into AP structures is done @@ -164,9 +187,10 @@ static uavcan::Subscriber *esc_stat UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static uavcan::PoolAllocator _node_allocator[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + -AP_UAVCAN::AP_UAVCAN() : - _node_allocator() +AP_UAVCAN::AP_UAVCAN() { AP_Param::setup_object_defaults(this, var_info); @@ -225,7 +249,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) return; } - _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); + _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), _node_allocator[driver_index]); if (_node == nullptr) { debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r"); diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 8ea923c3364..a285afa92f6 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -33,27 +33,6 @@ #include -#ifndef UAVCAN_NODE_POOL_SIZE -#if HAL_CANFD_SUPPORTED -#define UAVCAN_NODE_POOL_SIZE 16384 -#else -#define UAVCAN_NODE_POOL_SIZE 8192 -#endif -#endif - -#if HAL_CANFD_SUPPORTED -#define UAVCAN_STACK_SIZE 8192 -#else -#define UAVCAN_STACK_SIZE 4096 -#endif - -#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE -#if HAL_CANFD_SUPPORTED -#define UAVCAN_NODE_POOL_BLOCK_SIZE 128 -#else -#define UAVCAN_NODE_POOL_BLOCK_SIZE 64 -#endif -#endif #ifndef UAVCAN_SRV_NUMBER #define UAVCAN_SRV_NUMBER 18 @@ -232,11 +211,11 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { // 0. return true if it was set bool check_and_reset_option(Options option); -private: // This will be needed to implement if UAVCAN is used with multithreading // Such cases will be firmware update, etc. class RaiiSynchronizer {}; +private: void loop(void); ///// SRV output ///// @@ -277,8 +256,6 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { HAL_Semaphore _param_save_sem; uint8_t param_save_request_node_id; - uavcan::PoolAllocator _node_allocator; - // UAVCAN parameters AP_Int8 _uavcan_node; AP_Int32 _servo_bm; From 633eb0db8b7d9c271957449f643d271d71842f17 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 2 Mar 2022 14:54:19 +0530 Subject: [PATCH 0249/3101] AP_HAL_ChibiOS: move to using data_32 for copying into CANFD buffer --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 84837325f6a..b84173b82b2 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -317,13 +317,6 @@ bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timing return true; } -static uint32_t bytes_to_word(const uint8_t data[]) { - return (uint32_t(data[3]) << 24) | - (uint32_t(data[2]) << 16) | - (uint32_t(data[1]) << 8) | - (uint32_t(data[0]) << 0); -} - int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) { @@ -378,8 +371,9 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, // Write Frame to the message RAM const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); - for (uint8_t i = 0, l = 2; i < data_length; i+=4, l++) { - buffer[l] = bytes_to_word(&frame.data[i]); + uint32_t *data_ptr = &buffer[2]; + for (uint8_t i = 0; i < (data_length+3)/4; i++) { + data_ptr[i] = frame.data_32[i]; } //Set Add Request From 6a0c46d241de14dd2d3e1e13aa005e333b7ee198 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 8 Mar 2022 17:32:47 +0530 Subject: [PATCH 0250/3101] AP_HAL_Periph: assert clock is FDCANCLK is 80MHz for H7 boards --- libraries/AP_HAL_ChibiOS/CANFDIface.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index b84173b82b2..f7330c9a836 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -87,7 +87,13 @@ extern AP_HAL::HAL& hal; -static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz"); +#define STR(x) #x +#define XSTR(x) STR(x) +#if defined(STM32H7) +static_assert(STM32_FDCANCLK == 80U*1000U*1000U, "FDCAN clock must be 80MHz, got " XSTR(STM32_FDCANCLK)); +#else +static_assert(STM32_FDCANCLK <= 80U*1000U*1000U, "FDCAN clock must be max 80MHz, got " XSTR(STM32_FDCANCLK)); +#endif using namespace ChibiOS; From 759e2b1b55a8c6122b7346e71825c0e10b125e2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2022 11:48:56 +0900 Subject: [PATCH 0251/3101] Copter: rename auto_take_off_xx to auto_takeoff_xx --- ArduCopter/mode.h | 4 ++-- ArduCopter/takeoff.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index bf3e302a0bf..d16cf93fdff 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -212,8 +212,8 @@ class Mode { static float auto_takeoff_no_nav_alt_cm; // auto takeoff variables - static float auto_take_off_start_alt_cm; // start altitude expressed as cm above ekf origin - static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin + static float auto_takeoff_start_alt_cm; // start altitude expressed as cm above ekf origin + static float auto_takeoff_complete_alt_cm; // completion altitude expressed as cm above ekf origin static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain static bool auto_takeoff_complete; // true when takeoff is complete static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 7dfd4e27783..b7df4ea7559 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -4,8 +4,8 @@ Mode::_TakeOff Mode::takeoff; bool Mode::auto_takeoff_no_nav_active = false; float Mode::auto_takeoff_no_nav_alt_cm = 0; -float Mode::auto_take_off_start_alt_cm = 0; -float Mode::auto_take_off_complete_alt_cm = 0; +float Mode::auto_takeoff_start_alt_cm = 0; +float Mode::auto_takeoff_complete_alt_cm = 0; bool Mode::auto_takeoff_terrain_alt = false; bool Mode::auto_takeoff_complete = false; Vector3p Mode::auto_takeoff_complete_pos; @@ -159,7 +159,7 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); // command the aircraft to the take off altitude - float pos_z = auto_take_off_complete_alt_cm + terr_offset; + float pos_z = auto_takeoff_complete_alt_cm + terr_offset; float vel_z = 0.0; copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0); @@ -179,7 +179,7 @@ void Mode::auto_takeoff_run() } // handle takeoff completion - bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm - auto_take_off_start_alt_cm) * 0.90); + bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm - auto_takeoff_start_alt_cm) * 0.90); bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1; auto_takeoff_complete = reached_altitude && reached_climb_rate; @@ -192,13 +192,13 @@ void Mode::auto_takeoff_run() void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt) { - auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm(); - auto_take_off_complete_alt_cm = complete_alt_cm; + auto_takeoff_start_alt_cm = inertial_nav.get_position_z_up_cm(); + auto_takeoff_complete_alt_cm = complete_alt_cm; auto_takeoff_terrain_alt = terrain_alt; auto_takeoff_complete = false; if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min - auto_takeoff_no_nav_alt_cm = auto_take_off_start_alt_cm + g2.wp_navalt_min * 100; + auto_takeoff_no_nav_alt_cm = auto_takeoff_start_alt_cm + g2.wp_navalt_min * 100; auto_takeoff_no_nav_active = true; } else { auto_takeoff_no_nav_active = false; From 553ad877f6f34cf94e174b04b9a6eb4611dfad97 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2022 13:09:16 +0900 Subject: [PATCH 0252/3101] Copter: fix takeoff to terrain alt --- ArduCopter/mode.h | 2 +- ArduCopter/takeoff.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d16cf93fdff..721db3a025a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -213,7 +213,7 @@ class Mode { // auto takeoff variables static float auto_takeoff_start_alt_cm; // start altitude expressed as cm above ekf origin - static float auto_takeoff_complete_alt_cm; // completion altitude expressed as cm above ekf origin + static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain static bool auto_takeoff_complete; // true when takeoff is complete static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index b7df4ea7559..136ff31ef63 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -179,7 +179,7 @@ void Mode::auto_takeoff_run() } // handle takeoff completion - bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm - auto_takeoff_start_alt_cm) * 0.90); + bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90); bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1; auto_takeoff_complete = reached_altitude && reached_climb_rate; From f345ab1581f9a7b0433b41182d73ccb6efe54e36 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 10 Mar 2022 11:44:10 +0900 Subject: [PATCH 0253/3101] Copter: 4.2.0-beta2 release notes --- ArduCopter/ReleaseNotes.txt | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index b8e04a380e3..0cd837856eb 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,22 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ -Copter 4.2.0-rc1 28-Feb-2022 +Copter 4.2.0-beta2 10-Mar-2022 +Changes from 4.2.0-beta1 +1) Auto and Guided mode changes + a) Delay removed when waypoints are very close together + b) Pause and continue support (GCS changes are still pending) + c) Takeoff and landing use position controller with slower reposition speed (also affects RTL, Land modes) + d) Waypoint navigation will make use of maximum waypoint radius to maximize speed through corners +2) Follow mode supports FOLLOW_TARGET message (sent by QGC ground station) +3) Bug fixes + a) Arming checks ignore SERVOx_MIN, MAX if setup as GPIO pin + b) BeastH7v2 BMI270 baro support + c) DShot prescaler fix (ESCs were not initialising correctly) + d) EKF3 variance constraint fix used to prevent "ill-conditioning" + e) POWR log message MCU voltage fix (min and max voltage were swapped) + f) SPRacingH7 firmware install fix +------------------------------------------------------------------ +Copter 4.2.0-beta1 28-Feb-2022 Changes from 4.1.5 1) AHRS/EKF improvements a) EKF startup messages reduced From 7c99268541becdce664d86181185b0c53372f4fd Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 10 Mar 2022 11:44:31 +0900 Subject: [PATCH 0254/3101] Rover: 4.2.0-beta2 release notes --- Rover/release-notes.txt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index 52146d4ce7e..d41265e328e 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,17 @@ Rover Release Notes: ------------------------------------------------------------------ +Rover 4.2.0-beta2 10-Mar-2022 +Changes from 4.2.0-beta1 +1) Follow mode supports FOLLOW_TARGET message (sent by QGC ground station) +2) Bug fixes + a) Arming checks ignore SERVOx_MIN, MAX if setup as GPIO pin + b) BeastH7v2 BMI270 baro support + c) DShot prescaler fix (ESCs were not initialising correctly) + d) EKF3 variance constraint fix used to prevent "ill-conditioning" + e) POWR log message MCU voltage fix (min and max voltage were swapped) + f) Sailboats loiter fix (could get stuck pointing directly into the wind) + g) SPRacingH7 firmware install fix +------------------------------------------------------------------ Rover 4.2.0-rc1 28-Feb-2022 Changes from 4.1.5 1) AHRS/EKF improvements From e486671bb20ff7d0641ecbe4abb30a1005881966 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 12 Mar 2022 17:57:48 +1100 Subject: [PATCH 0255/3101] AP_HAL_ChibiOS: correct #ifndef nesting in periph defaults --- .../AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 226e874f1d6..16441d28f9e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2701,12 +2701,15 @@ def add_apperiph_defaults(f): #ifndef HAL_SCHEDULER_ENABLED #define HAL_SCHEDULER_ENABLED 0 #endif + #ifndef HAL_LOGGING_ENABLED #define HAL_LOGGING_ENABLED 0 #endif + #ifndef HAL_GCS_ENABLED #define HAL_GCS_ENABLED 0 #endif + // default to no protocols, AP_Periph enables with params #define HAL_SERIAL1_PROTOCOL -1 #define HAL_SERIAL2_PROTOCOL -1 @@ -2716,17 +2719,22 @@ def add_apperiph_defaults(f): #ifndef HAL_LOGGING_MAVLINK_ENABLED #define HAL_LOGGING_MAVLINK_ENABLED 0 #endif + #ifndef HAL_MISSION_ENABLED #define HAL_MISSION_ENABLED 0 #endif + #ifndef HAL_RALLY_ENABLED #define HAL_RALLY_ENABLED 0 #endif + #ifndef HAL_CAN_DEFAULT_NODE_ID #define HAL_CAN_DEFAULT_NODE_ID 0 #endif + #define PERIPH_FW TRUE #define HAL_BUILD_AP_PERIPH + #ifndef HAL_WATCHDOG_ENABLED_DEFAULT #define HAL_WATCHDOG_ENABLED_DEFAULT true #endif @@ -2734,14 +2742,15 @@ def add_apperiph_defaults(f): #ifndef AP_FETTEC_ONEWIRE_ENABLED #define AP_FETTEC_ONEWIRE_ENABLED 0 #endif + #ifndef HAL_BARO_WIND_COMP_ENABLED #define HAL_BARO_WIND_COMP_ENABLED 0 +#endif #ifndef HAL_UART_STATS_ENABLED #define HAL_UART_STATS_ENABLED (HAL_GCS_ENABLED || HAL_LOGGING_ENABLED) #endif -#endif ''') From ea0b6dfcb2e8a0c5c9a55a699715c442ddd5f4bc Mon Sep 17 00:00:00 2001 From: iainguilliard Date: Mon, 2 Aug 2021 18:28:17 +1000 Subject: [PATCH 0256/3101] HAL_ChibiOS: add MatekF405-STD S7 PWM output MatekF405-STD board has a pad for S7 PWM output from pin PB8 missing in hwdef --- libraries/AP_HAL_ChibiOS/hwdef/MatekF405-STD/hwdef.dat | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-STD/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-STD/hwdef.dat index 48b1c5fa0b5..04badbd0293 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-STD/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-STD/hwdef.dat @@ -5,3 +5,13 @@ include ../MatekF405/hwdef.dat # different IMU orientation undef IMU IMU Invensense SPI:mpu6000 ROTATION_YAW_90 + +# Use TIM5 for ST clock +undef STM32_ST_USE_TIMER +undef CH_CFG_ST_RESOLUTION +define STM32_ST_USE_TIMER 5 +define CH_CFG_ST_RESOLUTION 32 + +# Add S7 Pad for MatekF405 STD +PB8 TIM4_CH3 TIM4 PWM(7) GPIO(56) NODMA + From 6ba9946cfc09069beb840deea17d4b34998c9fe9 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 12 Mar 2022 18:24:02 -0600 Subject: [PATCH 0257/3101] AP_CANManager: correct parameter metadata error --- libraries/AP_CANManager/AP_CANIfaceParams.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANIfaceParams.cpp b/libraries/AP_CANManager/AP_CANIfaceParams.cpp index 2e1dc2c9335..b088f392c74 100644 --- a/libraries/AP_CANManager/AP_CANIfaceParams.cpp +++ b/libraries/AP_CANManager/AP_CANIfaceParams.cpp @@ -36,8 +36,8 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = { AP_GROUPINFO("BITRATE", 2, AP_CANManager::CANIface_Params, _bitrate, 1000000), #if HAL_CANFD_SUPPORTED - // @Param: BITRATE - // @DisplayName: Bitrate of CAN interface + // @Param: FDBITRATE + // @DisplayName: Bitrate of CANFD interface // @Description: Bit rate can be set up to from 1000000 to 8000000 // @Values: 1:1M, 2:2M, 4:4M, 5:5M, 8:8M // @User: Advanced From caaa5002abf582a499e56f924e84cc1a4838e148 Mon Sep 17 00:00:00 2001 From: AndKe Date: Tue, 8 Feb 2022 20:46:16 +0100 Subject: [PATCH 0258/3101] AP_HAL_ChibiOS: add OREOLED to CubeGreen --- libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat index f7310ab52dc..a594650bedd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat @@ -10,4 +10,6 @@ include ../CubeBlack/hwdef.dat # these are parameters the Solo requires for proper operation that are different from the 4 standard defaults. env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param' +define HAL_OREO_LED_ENABLED 1 + AUTOBUILD_TARGETS Copter From bd68dc6a51bfa579582d1869893e868847ca6a70 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Mar 2022 17:53:33 +1100 Subject: [PATCH 0259/3101] AP_Arming: make mission checks virtual --- libraries/AP_Arming/AP_Arming.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 17ec0aed866..4d031ae9be4 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -164,7 +164,7 @@ class AP_Arming { bool manual_transmitter_checks(bool report); - bool mission_checks(bool report); + virtual bool mission_checks(bool report); bool rangefinder_checks(bool report); From 82937ca778dfa60bfba2cf138b4adbe506d49cd2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 12 Mar 2022 17:53:58 +1100 Subject: [PATCH 0260/3101] Plane: check that RTL_AUTOLAND is set if using DO_LAND_START --- ArduPlane/AP_Arming.cpp | 13 +++++++++++++ ArduPlane/AP_Arming.h | 1 + 2 files changed, 14 insertions(+) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index a38289b448a..bb15c5498c4 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -351,3 +351,16 @@ void AP_Arming_Plane::update_soft_armed() } } +/* + extra plane mission checks + */ +bool AP_Arming_Plane::mission_checks(bool report) +{ + // base checks + bool ret = AP_Arming::mission_checks(report); + if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland.get() == 0) { + ret = false; + check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); + } + return ret; +} diff --git a/ArduPlane/AP_Arming.h b/ArduPlane/AP_Arming.h index 02239458d30..5f5051e6f1f 100644 --- a/ArduPlane/AP_Arming.h +++ b/ArduPlane/AP_Arming.h @@ -34,6 +34,7 @@ class AP_Arming_Plane : public AP_Arming bool ins_checks(bool report) override; bool quadplane_checks(bool display_failure); + bool mission_checks(bool report) override; private: void change_arm_state(void); From 46a7f2cc4bb4cf03398e6019b732565dd3e8b076 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 13 Mar 2022 13:03:18 +1100 Subject: [PATCH 0261/3101] autotest: adjust for RTL_AUTOLAND failure --- Tools/autotest/arduplane.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 570fa365a6e..c0757c03888 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -766,6 +766,7 @@ def fly_flaps(self): "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION "LAND_FLAP_PERCNT": 50, "LOG_DISARMED": 1, + "RTL_AUTOLAND": 1, "RC%u_MIN" % flaps_ch: flaps_ch_min, "RC%u_MAX" % flaps_ch: flaps_ch_max, From 573cb0494d4509744c00b4d52c2f8d620967d27f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 13 Mar 2022 15:16:27 +1100 Subject: [PATCH 0262/3101] autotest: set RL_AUTOLAND=1 for more tests --- Tools/autotest/arduplane.py | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index c0757c03888..dcf351fb704 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -570,9 +570,12 @@ def fly_deepstall(self): def fly_deepstall_absolute(self): self.start_subtest("DeepStall Relative Absolute") - self.set_parameter("LAND_TYPE", 1) deepstall_elevator_pwm = 1661 - self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm) + self.set_parameters({ + "LAND_TYPE": 1, + "LAND_DS_ELEV_PWM": deepstall_elevator_pwm, + "RTL_AUTOLAND": 1, + }) self.load_mission("plane-deepstall-mission.txt") self.change_mode("AUTO") self.wait_ready_to_arm() @@ -593,9 +596,12 @@ def fly_deepstall_absolute(self): def fly_deepstall_relative(self): self.start_subtest("DeepStall Relative") - self.set_parameter("LAND_TYPE", 1) deepstall_elevator_pwm = 1661 - self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm) + self.set_parameters({ + "LAND_TYPE": 1, + "LAND_DS_ELEV_PWM": deepstall_elevator_pwm, + "RTL_AUTOLAND": 1, + }) self.load_mission("plane-deepstall-relative-mission.txt") self.change_mode("AUTO") self.wait_ready_to_arm() @@ -1096,6 +1102,7 @@ def test_gripper_mission(self): self.context_push() ex = None try: + self.set_parameter("RTL_AUTOLAND", 1) self.load_mission("plane-gripper-mission.txt") self.set_current_waypoint(1) self.change_mode('AUTO') @@ -1642,7 +1649,10 @@ def test_main_flight(self): def airspeed_autocal(self): self.progress("Ensure no AIRSPEED_AUTOCAL on ground") - self.set_parameter("ARSPD_AUTOCAL", 1) + self.set_parameters({ + "ARSPD_AUTOCAL": 1, + "RTL_AUTOLAND": 1, + }) m = self.mav.recv_match(type='AIRSPEED_AUTOCAL', blocking=True, timeout=5) @@ -1740,6 +1750,7 @@ def climb_before_turn(self): self.set_parameters({ "FLIGHT_OPTIONS": 0, "ALT_HOLD_RTL": 8000, + "RTL_AUTOLAND": 1, }) takeoff_alt = 10 self.takeoff(alt=takeoff_alt) @@ -1858,6 +1869,7 @@ def test_rangefinder(self): '''ensure rangefinder gives height-above-ground''' self.load_mission("plane-gripper-mission.txt") # borrow this + self.set_parameter("RTL_AUTOLAND", 1) self.set_current_waypoint(1) self.change_mode('AUTO') self.wait_ready_to_arm() From 81b129d3b31b63d9a8582ef22023b4568c248753 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E5=AD=9F=E6=99=93?= Date: Mon, 14 Mar 2022 06:55:05 +0000 Subject: [PATCH 0263/3101] AP_HAL_ChibiOS: CUAVv5: use icm42688p instead of icm20602 --- libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat index 923dc6bc5c7..4e39e983c31 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAVv5/hwdef.dat @@ -20,4 +20,15 @@ define HAL_GPIO_LED_OFF 1 define HAL_HAVE_PIXRACER_LED +undef IMU +undef PF11 +PF11 ICM42688_CS CS SPEED_VERYLOW + +SPIDEV icm42688 SPI1 DEVID2 ICM42688_CS MODE3 2*MHZ 8*MHZ + +IMU Invensense SPI:icm20689 ROTATION_NONE +IMU Invensense SPI:icm20602 ROTATION_NONE +IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270 +IMU BMI055 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90 +IMU BMI088 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90 From deb7b13da5691ae17405148725b36fd997d71553 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 12 Mar 2022 23:03:14 +1100 Subject: [PATCH 0264/3101] AP_Airspeed: disable AUTOCAL if airspeed is disabled --- libraries/AP_Airspeed/AP_Airspeed.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 6bdd86f65cb..4ab2f8e9d69 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -19,7 +19,7 @@ class AP_Airspeed_Backend; #endif #ifndef AP_AIRSPEED_AUTOCAL_ENABLE -#define AP_AIRSPEED_AUTOCAL_ENABLE !defined(HAL_BUILD_AP_PERIPH) +#define AP_AIRSPEED_AUTOCAL_ENABLE AP_AIRSPEED_ENABLED #endif #ifndef HAL_MSP_AIRSPEED_ENABLED From 470cb2caa249c7dbc823c2a9ae39fa5480483ab9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 13 Mar 2022 21:41:03 +1100 Subject: [PATCH 0265/3101] AP_HAL_ChibiOS: turn autocal off for periphs --- libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 16441d28f9e..22f7b3adb20 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2751,6 +2751,10 @@ def add_apperiph_defaults(f): #define HAL_UART_STATS_ENABLED (HAL_GCS_ENABLED || HAL_LOGGING_ENABLED) #endif +#ifndef AP_AIRSPEED_AUTOCAL_ENABLE +#define AP_AIRSPEED_AUTOCAL_ENABLE 0 +#endif + ''') From 1a58b3b0254ab9264640112ce7868f7772c63eef Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 14 Mar 2022 09:23:51 +1100 Subject: [PATCH 0266/3101] waf: disable airspeed on sitl_periph_gps --- Tools/ardupilotwaf/boards.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 31a3f7bb04d..ef7eb068203 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -666,6 +666,7 @@ def configure_env(self, cfg, env): HAL_BUILD_AP_PERIPH = 1, PERIPH_FW = 1, CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_gps"', + AP_AIRSPEED_ENABLED = 0, HAL_PERIPH_ENABLE_GPS = 1, HAL_WITH_DSP = 1, HAL_CAN_DEFAULT_NODE_ID = 0, From b112f8dba1d25b1a4cef505d362513bd0cdf9bfc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 8 Mar 2022 13:42:50 +0000 Subject: [PATCH 0267/3101] AP_Declination: ensure indexing into declination tables is always correct add constants for table sizes --- libraries/AP_Declination/AP_Declination.cpp | 4 ++-- libraries/AP_Declination/AP_Declination.h | 9 ++++++--- libraries/AP_Declination/tables.cpp | 6 +++--- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Declination/AP_Declination.cpp b/libraries/AP_Declination/AP_Declination.cpp index e68c69a3846..550312000e6 100644 --- a/libraries/AP_Declination/AP_Declination.cpp +++ b/libraries/AP_Declination/AP_Declination.cpp @@ -66,8 +66,8 @@ bool AP_Declination::get_mag_field_ef(float latitude_deg, float longitude_deg, f } /* find index of nearest low sampling point */ - uint32_t min_lat_index = static_cast((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES); - uint32_t min_lon_index = static_cast((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES); + uint32_t min_lat_index = constrain_int32(static_cast((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES), 0, LAT_TABLE_SIZE - 2); + uint32_t min_lon_index = constrain_int32(static_cast((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES), 0, LON_TABLE_SIZE -2); /* calculate intensity */ diff --git a/libraries/AP_Declination/AP_Declination.h b/libraries/AP_Declination/AP_Declination.h index a9dec3fea06..2832428f05f 100644 --- a/libraries/AP_Declination/AP_Declination.h +++ b/libraries/AP_Declination/AP_Declination.h @@ -35,7 +35,10 @@ class AP_Declination static const float SAMPLING_MIN_LON; static const float SAMPLING_MAX_LON; - static const float declination_table[19][37]; - static const float inclination_table[19][37]; - static const float intensity_table[19][37]; + static const uint32_t LAT_TABLE_SIZE = 19; + static const uint32_t LON_TABLE_SIZE = 37; + + static const float declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE]; + static const float inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE]; + static const float intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE]; }; diff --git a/libraries/AP_Declination/tables.cpp b/libraries/AP_Declination/tables.cpp index 5879f039162..2b1542559fb 100644 --- a/libraries/AP_Declination/tables.cpp +++ b/libraries/AP_Declination/tables.cpp @@ -9,7 +9,7 @@ const float AP_Declination::SAMPLING_MAX_LAT = 90; const float AP_Declination::SAMPLING_MIN_LON = -180; const float AP_Declination::SAMPLING_MAX_LON = 180; -const float AP_Declination::declination_table[19][37] = { +const float AP_Declination::declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = { {149.10950f,139.10950f,129.10950f,119.10950f,109.10949f,99.10950f,89.10950f,79.10950f,69.10950f,59.10950f,49.10950f,39.10950f,29.10950f,19.10950f,9.10950f,-0.89050f,-10.89050f,-20.89050f,-30.89050f,-40.89050f,-50.89050f,-60.89050f,-70.89050f,-80.89050f,-90.89050f,-100.89050f,-110.89050f,-120.89050f,-130.89050f,-140.89050f,-150.89050f,-160.89050f,-170.89050f,179.10950f,169.10950f,159.10950f,149.10950f}, {129.37759f,117.14583f,106.01898f,95.84726f,86.44522f,77.63150f,69.24826f,61.16874f,53.29825f,45.57105f,37.94414f,30.38880f,22.88112f,15.39339f,7.88854f,0.31945f,-7.36677f,-15.22089f,-23.28322f,-31.57827f,-40.11442f,-48.88906f,-57.89765f,-67.14429f,-76.65158f,-86.46832f,-96.67422f,-107.38079f,-118.72599f,-130.85732f,-143.89431f,-157.86353f,-172.61739f,172.21319f,157.16190f,142.76170f,129.37759f}, {85.60184f,77.69003f,71.32207f,65.86993f,60.92414f,56.17033f,51.35320f,46.28164f,40.84704f,35.03587f,28.92623f,22.66416f,16.41848f,10.31921f,4.39763f,-1.44271f,-7.40082f,-13.70324f,-20.51470f,-27.87783f,-35.70713f,-43.83304f,-52.06997f,-60.27655f,-68.39086f,-76.44339f,-84.56374f,-93.00460f,-102.21930f,-113.07088f,-127.37057f,-149.05145f,176.63172f,138.21637f,112.07842f,96.22737f,85.60184f}, @@ -31,7 +31,7 @@ const float AP_Declination::declination_table[19][37] = { {-177.79784f,-167.79784f,-157.79784f,-147.79784f,-137.79784f,-127.79784f,-117.79784f,-107.79784f,-97.79784f,-87.79784f,-77.79784f,-67.79784f,-57.79784f,-47.79784f,-37.79784f,-27.79784f,-17.79784f,-7.79784f,2.20217f,12.20217f,22.20217f,32.20217f,42.20217f,52.20217f,62.20217f,72.20217f,82.20217f,92.20217f,102.20217f,112.20217f,122.20217f,132.20217f,142.20217f,152.20217f,162.20217f,172.20217f,-177.79784f} }; -const float AP_Declination::inclination_table[19][37] = { +const float AP_Declination::inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = { {-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f}, {-78.33243f,-77.56645f,-76.64486f,-75.60941f,-74.49599f,-73.33711f,-72.16456f,-71.01082f,-69.90877f,-68.88978f,-67.98065f,-67.20063f,-66.55969f,-66.05909f,-65.69426f,-65.45930f,-65.35147f,-65.37404f,-65.53651f,-65.85220f,-66.33408f,-66.99021f,-67.82010f,-68.81276f,-69.94649f,-71.18994f,-72.50361f,-73.84119f,-75.15044f,-76.37388f,-77.45008f,-78.31699f,-78.91913f,-79.21830f,-79.20379f,-78.89480f,-78.33243f}, {-80.91847f,-79.09801f,-77.26826f,-75.41050f,-73.49957f,-71.51974f,-69.48020f,-67.42760f,-65.44927f,-63.66181f,-62.18407f,-61.10090f,-60.43119f,-60.11709f,-60.04466f,-60.08935f,-60.16521f,-60.25535f,-60.41391f,-60.74312f,-61.35672f,-62.34264f,-63.73840f,-65.52698f,-67.65072f,-70.03207f,-72.58967f,-75.24472f,-77.91857f,-80.52353f,-82.93966f,-84.94483f,-86.05606f,-85.75384f,-84.42566f,-82.72116f,-80.91847f}, @@ -53,7 +53,7 @@ const float AP_Declination::inclination_table[19][37] = { {88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f} }; -const float AP_Declination::intensity_table[19][37] = { +const float AP_Declination::intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = { {0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f}, {0.60733f,0.60103f,0.59321f,0.58408f,0.57385f,0.56274f,0.55099f,0.53886f,0.52664f,0.51464f,0.50318f,0.49258f,0.48311f,0.47506f,0.46864f,0.46409f,0.46158f,0.46131f,0.46341f,0.46797f,0.47499f,0.48434f,0.49579f,0.50895f,0.52332f,0.53833f,0.55334f,0.56771f,0.58086f,0.59227f,0.60156f,0.60848f,0.61292f,0.61488f,0.61448f,0.61189f,0.60733f}, {0.63154f,0.61845f,0.60363f,0.58729f,0.56950f,0.55031f,0.52986f,0.50843f,0.48660f,0.46508f,0.44473f,0.42628f,0.41025f,0.39690f,0.38632f,0.37857f,0.37385f,0.37260f,0.37540f,0.38291f,0.39557f,0.41347f,0.43621f,0.46292f,0.49236f,0.52306f,0.55344f,0.58192f,0.60704f,0.62760f,0.64283f,0.65244f,0.65659f,0.65582f,0.65087f,0.64254f,0.63154f}, From 73006d9a7c19fa464a4484f35ed7397b1d7c2984 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 9 Mar 2022 11:29:30 -0600 Subject: [PATCH 0268/3101] AP_OSD: make OSD rssi scale match link quality (0-100) --- libraries/AP_OSD/AP_OSD_Screen.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index 3e8079ceded..c9626d01f5f 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1318,7 +1318,7 @@ void AP_OSD_Screen::draw_rssi(uint8_t x, uint8_t y) { AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); if (ap_rssi) { - const uint8_t rssiv = ap_rssi->read_receiver_rssi() * 99; + const uint8_t rssiv = ap_rssi->read_receiver_rssi() * 100; backend->write(x, y, rssiv < osd->warn_rssi, "%c%2d", SYMBOL(SYM_RSSI), rssiv); } } From 222717cc88bf3d721a09e4cdc6345e7a0ae14401 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 11 Mar 2022 14:56:04 +0000 Subject: [PATCH 0269/3101] AP_HAL_ChibiOS: add KakuteH7-bdshot target --- .../hwdef/KakuteH7-bdshot/hwdef-bl.dat | 1 + .../hwdef/KakuteH7-bdshot/hwdef.dat | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef-bl.dat new file mode 100644 index 00000000000..e5069afab06 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef-bl.dat @@ -0,0 +1 @@ +include ../KakuteH7/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef.dat new file mode 100644 index 00000000000..2d1c0934f9a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-bdshot/hwdef.dat @@ -0,0 +1,18 @@ +# Bi-directional dshot version of KakuteH7 + +include ../KakuteH7/hwdef.dat + +undef PB0 PB1 PB3 PB10 PC7 + +# Must use USART6 for RCIN rather than RCINT as timer needed for bi-dir dshot +PC7 USART6_RX USART6 NODMA + +define HAL_SERIAL6_PROTOCOL SerialProtocol_RCIN +define HAL_SERIAL6_BAUD 115 +define HAL_SERIAL7_PROTOCOL SerialProtocol_ESCTelemetry +define HAL_SERIAL7_BAUD 115 + +PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR +PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) +PB3 TIM2_CH2 TIM2 PWM(3) GPIO(52) BIDIR +PB10 TIM2_CH3 TIM2 PWM(4) GPIO(53) BIDIR From 38f7c306bc91769ff6edadc8d790e31317c2ce49 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 11 Mar 2022 14:56:37 +0000 Subject: [PATCH 0270/3101] bootloaders: bootloader for KakuteH7-bdshot --- Tools/bootloaders/KakuteH7-bdshot_bl.bin | Bin 0 -> 15564 bytes Tools/bootloaders/KakuteH7-bdshot_bl.hex | 975 +++++++++++++++++++++++ 2 files changed, 975 insertions(+) create mode 100644 Tools/bootloaders/KakuteH7-bdshot_bl.bin create mode 100644 Tools/bootloaders/KakuteH7-bdshot_bl.hex diff --git a/Tools/bootloaders/KakuteH7-bdshot_bl.bin b/Tools/bootloaders/KakuteH7-bdshot_bl.bin new file mode 100644 index 0000000000000000000000000000000000000000..b96f9df2620e7ec830afd871365fa21fee1b0b4d GIT binary patch literal 15564 zcmd6O4R}=5we~(glR5bTCYdP7B*6Rx4Fq&ZKmu5q%!HGlK@&l11Z^jYUd{w53XVf&2psly+odME@U~5``h7hf1NMKGfVb1sNGZRGH z`+fK6^L)?qg=f!RXYIAu-g~XJ*IxSvl0?kUvPAgt93otSbZ;UrLEZuUbO~&s{7lfPNJ4YD7<2XmiJ4iQOSbkp| z(>GrAMBjMh6SSoNoQ#sD1no>ol}yNA{$@zI>B9AiI!R{#cxDr=#me_9tb7I1A+<`r z)xMx2r}9bX^?fA@pE?!Ft;oLY%T!^i%yG9lZ|J)jrA{OzM9gz5vM!@x<)sQIGfL+A z%o~5{&SF zbl>d0xiVMGmY8KkZuj#=Elwp^C*25bI?lED*Sp&#E==mV^Q5TF>6iETp$GZ#C~Gcr zXZeV{Y+7^^%L(O-c|bX1IH4FU1BxM?<|mX-f%_D=OyDws%LGmaP6jRuxGdnZfb*N3 zGN~ItPGRe1aH4G6Wo0w4ecP|A^9J^jb7V?zi4AEg^w$K zyX(TpzYdJQts5A>sc199olQzxB(6WZ^E=F{qsjx@CXbuf7{lf{=YlhwW^&4hfDf@9 zRjLbTux3s6kBD?Gn#+`4LRi6!q^gW&pH#M_l&vA3kr8T-Er(xlo2{yaG^ZSRMk4Zm zMSost+}_c{SbkQX?i=vL`Nj2o-xRU!*Yx}!(DOC;qIxFnPgioCMh~&ISCFS$#ddMN z*e0g>HhLInpxERa z^;nOPk_964CVi2f!FOW{5gtn>!Wfbf@G2xq<4)wg$Uj3}ieplVa690cZ(4^ntC1hZ zNWMdk?j-H3MR(wpFqtFt<%+QLqF*VQud=g;R>aHc{ zMs|L1MlW+Ivkb=TbT%G5bS*@_YHIEXZ~1!pWQezHF28`^P37aEDmgc~Tucu0$&@c> zn{@|>Y?$f^FM&Q8GrU|1nbJqf&Zee*b82st$ah4q>P(eWxzqxokL0P58)A2WS9$rn zA#%;n0`Zb(gS1bJVtSF}w+9&W4L4Dq;Dzzo(eEQu-<)ciEXPd0Vu}}=JIh5O4BcZq zBtIN{Mdaydjdb)ki19K1c5+dOu`Lu?a}nx4K9z^L^^#M6ih7HWCmFj#mUi+h#^Et+ z?pMSc#Cblm`%cd<(5JN%n-9f%wPWJzhgfrtHuvtIDuiagKY2JzlGO{9hq7f(zhgo? zG`li+Ovn95YH_Z;u=Y^TdH;^Jku`@h+&$}<7VDqlQm>zcb+nPi>V<0#ac#^>rkzum zg3m(KM{S31_^O}affta{XgSjFAe;7!R5oiTW*u6vG=1pJu$fJHX`#%K-4&JVvP$#& zDn;f;L|#6{w5gUI<*>o2s$izFyZ-F7j603;asR za%!g-3Kc`Dt0#5M=R)r)na$LKh@3P@Ld15Lg+IWf_45kJ-!XCUAZ>Gn*5-kOd@GTw zQ0tS+=>DS7wP|z~jc%Vt_qEBhT7TzixIO6a`pLg&nAsX;=Rw^Inb1(#q+aALd^3+8 zys4}bdqPxi&nf!(>V?LGuR&+`94xNXkEJAZcDL5jr_*%o-#my>>TfH3b=}&Dj)T== zN|;X}@_WjRl|Q3|wRxmQPHOaJ=*1-ZTNdGq^^p0&$zOk*B(DF2&N}tuSH$mqvQm8T6KXTmw)X#kA#>cm?wsvuB2#p_iz!GS zO1lv@dH*8mxhPj?6=ydvV!n8|Y`2pS>aiL#W|Pm?TQxC-Idhka6)R)_AKyY^J`6i0 zHhniMGL~6kJ^7)cx2utWYVLeS(UbTZS|zJ?tidD&8W6hG$Q!C(W=MaR!zuLIkLcE? zh>U2H%;KNCocoue{dG?n>7%@9KYiUPH<7%Q8yWczlXic;RMfP~DV$>D-%l1ud?{0O z6sp zl2q}el4mdKBsRe+H0%V7vGy^PvagBu%bi7g(?<;Cjy$*AZIXD=<~}-}nv{{Yps7as zjk9G^sLl`k^s(PKli)Mc7V4s;s79jLl5-5#=FAP0oa2z20#5Ba3pxE3`o6xfZhop* zIzOef#X0AA<|NZtll!esP9zF*&w0dwL!nKGfo@+o7OGh|8p;xNBTMogcINt+Q?0po z`Wz-1GoPdmAwsgqg8teyy&D8}4sGV~h6=@2Kf zV!)I!$>fklQ$h5hloMN=Yye&{XDZ|?TRN(^7TlOA>cuAU$4Z`g!)qTXzUHT225c|r zq4TO-ytYn_e5e9LO2q!K!0N{Q457@aN;nJ0+_ms*!5-^KsoS-rcr!xS{RRzxD=1&xmopOuPCL?N=!K z{+>2ZqvhP2w$9Mb@T76My*s5;xbaEntU$WLtf_GbHpe*OnKersMp@-{pRFbrf}e-% zz2;r7e8x&HDc!|*xX!FH+J9rWV=FoDzvZOze6dMN-_9?}3^EU~i_(MH4>Gol$M!mm z&eS0D!g@O^e@y{ZIXp?bm|&2!S6dqHqsjO8+GGClp8Fy?|-i z594&pDEw%G?#?(}A*H(wFtu+(_m_J$SL;<~@OJcSgVw9rLCRx+%8KQkv|m*dj{9rT zuii5T2a$L57@W2&3(fObiyE=3d^nRd z$_%scJ4F7)G;M#ka!WPEP!C7sXI?MvHFPr;UgY7uvQ%QTeFqeFs)yoky4#{sX^PRB z3%|q2x2fiNlmi3qjA|As7uacEGopP?_HgEcq+qkM6KjNJF*meeancZ#zIAal*grb2 zh#TQ5om=$QhqnxDiTlYVm`f|EZf+gCWHBa9H*+tunDihCujkF!75-7J>11A@(0mm& zF0-C~g*$BTWx}bP++D5MJdYga#aUvkGcHel>=C_vo`7EeJQkIbFq+NY94V)hX=Y@- z%CvGXF!K2r)jij~NTf5iRf$VicUbO?N}IeH5?Opckh6zbV|0XreAv&P4Z+@*6{9ta zn3A6Ih|PToh@dH!=9{8cr1*(>f(Oy3|w(>8CLw>Gd`?xf9WA)mt+PF&I*jQ?Oh%N zc&`L+YW<9S$E)%YW+AxpWsm*P@sad3*Ynt=0aAur}xIUi<=(o~&hcPRWFYJ{M*-HNZ8k(W-^OF5FQlj?!?5T2!%+wNB$ zx9e=2&xJDD$5hC)wT#$0=pLIiE03F966?#su3cw|`yyX=v*NSUSwU=m3PER7TG>I(+=rP(F~gyKm>rzFgL9o_ zZH{wnVT%*1gx@msGWHDRfmuUL!P)?e9c7E36m-GPn%KH6to6AqA8vUg^z!+gJLrkQ z%jX?Co)7L*9=0@pltcH0kQB)bDHseTJ5b!;)Ao}ux&fa3z|Y;o?G*na2^cl{RD*0F`y2GC#H%8rS9 z57Bn$>h(PGW25tK{dmjv&=JUxwl-Vq4b|Wt_=^{nxC9wZf}CcXs8RHEh!5}O>bEf9 zYunBj7=mU^Ds(RVCiV#A!&&O3-OAo-i>65`bJ{*B_;UbQI}c8jGb#8(fYo>(NB?%) z&EmWyPM*W03^H==B$LkvSVYg6hYH+np#pa+&YZZ(acv|7BM}bKI!4gH955xBMHaRG z6#W&b-iy9?iR%2p8LIt(oCH-@v<*}p(SHwRUea-axLBl?sz&bBMlkxxot;-)Dy0?` zw}5ZNE`E}F3I9c{yJ8=PNZ;#N1CAiQf>fNaTT*aCAZg8T*PO-6hvpO%4t;QjlUFWV zCRhuXCH_{fe5CO(?C}|A=J5e3XHR!@{2Uj}xb@zixLo6{sF6agXDON`&mPIt@;C*t znMISBjo#0`LZSG4SQ(AyS4Y1*Fa8MsXGrulvtSYIE~!8lV1{x!4ZX>0 z*n;R#yx!bIef?MJXOCP_Fd9tN=H^Uq;(Zy0*Ne}V0r=0&(Dz_)rdM<}z87M#OBoo& z`IyI7b_6jQu{|tt4J&e^>YkEM9G^>ng_EH}oi_F*htVYiE(HPLsowi<$tXGNL zrueNX@fcQNY^6$88sZN5zUjDz>VD0a$0MY|SKdcE{jlEv5|P;+uJUGSqqN!7>WmIy zeWUx0k%v93m3`P(woB>=Kg7C}v?Z+9rCh(niQnQS`bw9QyW}X!vWQDDEMcvjA}k^J z)i3dteasPou7OlEYfv=b?10%YUki|p-_{@4D~qTHlZ7%)oKeU zwo-mlHdI%9&^zM!z}G~ zl3;N!C)qrAc-En`CpHn%xlw-^7tUk{Ms-$dtNIKo1F8#OckWF(F%7XIXTx4JS|n~% zGOulO&I;P=QkE>~BiSkT#hy1unY_0~b+^XX=Xe8>T(ZD~61vgG!p; zRyUZKZS~&IJuf?%AS2I?J_;S@d;81c`ua<#*4yb!4j4;wOgK3yNhw&;*PoSwl}3FP z-#g=SrI9AJU8$K9BkFiO%@3)OidU_R#{yHKug20v=)b;Ow`eX_3Pd?qa@pDDxcm&2jD=}d zBj=`=R@O4BQjL5zH48nOo#@fb-3Ogndf(B`f1nDT z!gUwA7U5(Qea4poU-j3d4t=0B`;nUSsyJ-(zxHMJ}AiY{X64UMIazn+#?*=~uLGpR+vO-{LzR608*(^%H7q&vUtR z+}YdN!sO()?_?D|SJOKpijC{&H;6! zG|O>)(qK~M08S<;j^@<1e(peR)VDEP)(PF|;4zMSG};C?I4n732Rn?~v@P8r%^yETu4*X9kWnjr9!ST8V(=Hv_Asj$z_RGhsP zu@dG%khkx0PC|$1mw6g`oReSNtrORR>S1*OR+t~+o>MK-=hxRBQ!9a==o4`$$sQE?7sv7B4(JF4vA_~R)n;N&W{=Q?1VVN5Fy1G9sa5)-> zV@lexlSq_H95yW5tdE{Ng%dKI>_J92W2&NdWf0e$Wx3NycLuF8$0BaRxmt42EH5A* zD6bzz+K>H^dUaOm2)(td4Dv;P3+ck!U3nD?%MrOR$a5ECW#+;Px2ms6JSDb@LG;a_ zjqYb-d>S9lWjN1}LbOQzcgSrEPASB5rL0Su3c}{Deag9B;mi}-U6>jfk5Rdptrnaw zfOh?5Y=H#GxV0`k}jO|6MFwV(lr6z)ecfK z+ima8ZOr3!CvzKfx8ydKPLR^vEh+2SQk!(V%QfM@-{#S~DF-_^;C6a5>2fLNYK;TE zpDSl;C?9<{V}*ys2#(PmxOV?)ginB7HEQaW3b+)YmY#LS(b zuWUfM1D8XzZ}gmtL;Pbx27`Rmv7S8$BeU5>S1TQH%rC|QlA+J z|IM%(z1UqP`|p>o|FAaMeR`|yUv{G(YQz*ZATH^sHT3G-(Gaaib*u42-y(>GrjUHy zGm5({oiGiJUwdR^Kb>dg1d9c|)X_5s|>l9_v1K8X>-K4vglxh!vK;;y)yQ%xS!tIiZkblCK-4xkL6l29N_zP2f@!xKyUFKIm`V#2nw4D+V4JPT{Yu7hKQdtEU&I&g|?!_3R7c)si9vE6gzp?%WxxbM7O z;diX`JU^DKUr@?l8yU(iyMW()c0t1>t9lKWq~Q#`zofQG1V`Q)B9xaXRyMJkvLkN zw+GAIvn_XN^2MKf)JU$nRf6Z0PUANdDHpr7 z)j=yPmwK@J+79?|hU@V7dgr@aoxclaqvtoy^c?f&QqKW@spt5<^g7`ZOh;^${dl<$ zuNQ~4ddpfudiMb4h$ct)8Am2@a5%mv*W3H4JO$)RHRj zpo}LZ^x1w4R)KNk^JA_Irs${6qYCjNZ9S?8$w;Z^j;BhHnE6em)m~p`uo-Kw^j;|& zlA=K>pWTxuAyeB#te(oY1GC3rbIAs-qc+(FIe~L#ajJU^@p-=s-)pE{kTrn3$X5-5 zCl@xbm~%$(H+R(GodayZ;U~CvYDUbs#{f&#_`A+BM)a$g&Lw(RLhYVfdYb4Lhow1F zJ15JTE2SivOS*qPTL8N^V$96GzF+gjH{r!d4o1u%YU5h_lEVJ4GgI97lU~5*iAWZ$ z4&1hf57#g&T@_4Y^`MfQmFgmvH!;HsvY4w;JNp-ojo#Y%?Jpbm{Ah2xz$|X++vplX zk6W{xXE_%WcCZ%lcSDF+$HNTxWN0EMGp~br*BZ&EQ?Uzy;3R z>79#itYYpT6_R6_ASJ&=PFuE|<+y&F9IfS2EAvGQ`0e60fnQk+)buuP6<&+D`Zek0 z7TeD1^*EV;ya-R&fU?h=A@!qQYUH=@4K>RkA$tY0xdtBC!3XfWAgzzp_Q~b{hcd{P z`Ms3gkSZBHZtV4`%&ENX-G7o^^ctlTp6nh+!R|hX&FI;>CsU%j$}MHbN({{P$z4aH z;}i8RuE>VlbJy8pE*Es)58W3?95775?BH=lD5liL3I7g^Xia7xVwR0AnQX+k)}SN< zT3_Rmc^5Rk#@Wa!2Nc2kUo}MgeSZa38I-X&|xk_F<`nMq7#lA(|jn#+l zvv9gG#h=^ym<_Sg)}6n{Odk(W8kCh}mC^Pp86PA8Hp zty(SsyRV8xw!KcS$rv7l7yhYU7oa({mN$_P;wI57+Tcas1Qd%rHf`7G2C47*#xz}9 z?wju9(*_4&K}I=g&X{DI@NG=%b4?gFx+eB-9P<>y7V6=jQ#NrcnuloNNd(|Lr zB%8_A@B%5xV`Z9saqv}$X0J_$J;Li0%o?tHEM-bRZp93&-^q_X3VS+@lkrtJ?`$n& zR-VLKrgpVUKXv};l$?}JITE(v=y=LpR-VJL6LLluxDojsf7f}(waGyqGZ(SJd4Kh3 zDW~DzoesGR*G7X%R?UjRQH~oVI4xBpPsUyZ%xLi5*jB*wtXGXZ61x^Kr@=d7XIO5K z*Wd?ZF96nQ@cpp{z)2c>Z>#{YUW2#9K4-W=L4#{!KF$$-!9R-qP`t&nu@?cyTl^@t z6>z-8KgX^G9B*+b_P3;&7LUgc0FJl#PRt89-r`%a#em~2_Q%F_{|YS$7OG$Dt$y=` zys6y2IrV6?U20#q_QI+|$zyKJV6upva9F9t@-RQhBC^;_X{(CieJB&4KwW1eYj!YTXi-o z|7#&lBhbgNbX>{8ED+4^kCNPH$I{Y>m_V_x? zM`m{)o8a~dtXz45p0m+YgQi&T8Nq?|@Kw4V$8q0J;J*7RU5}}k;KoYQgVeK45aS!j zsb9Sezbb)W^%eXYc>I4zpTKMM$qWg5T#S`FU+a3iYnMWdL|(wPIb6x@ghL6%BgU}M zeF9Eui@2wmZ8%ematq(^^p7TOp1A^oGgwSQD__K&8dyXx&U#~M zvAPGGdYwbZX=`tX?BXWzV&uJ8N5eNt?=0P1+EEt@8M@i0`66Z;v}8F(wPd6xW+_D+ zbj5#3SFU>OaN&)pZjO4K4OE8&T1>IVBGz8$3GGrnS#jc2t?+A7>6#90QQ1OUUZ&{p zr5lS(z8=39kL$|@eVOJxSNB(F;mi6O#LKYD^+jy}7O?v7E#PvE(LIsbHVf|<)3tlv zA2eJ8z5P}-Xj-E>o9Cl?WAI|QqmDoE7q405SJC%ZMBA%=7K-a_me?#lMfH|KB}%S( zS^PyqRZoScvHj2(Jzce9)z&noUu`-tmZbl8?04phj%#yH>B0`~R)sHk#)-XK!DLX= z)+Jqi(dv&?s(EqUspGVjcSDQB)zH_4iyaMLE4`s~4fORfZc?b6=LT_))48i*ZzsQ| z-84I3^bFv|^nho;YbxF4h5i$>fy#oh;RF!3Pq1?Ny;nHg^VHOvYFXc7C(G|a$m$>rHA?ehFy8t|lgUA1m6SGU*es!PSa5NP0|u5&xisrRiYKcO_4 zQ{V@huqSCUTPM^ytE|VVSN!{gGNP4^pj5C>I>EB267aHg%$Rp@(&fi_TrNY;DB^H2 z;Qj=B3*c81un+K&1pGMQg9-Qm-~$Qx7~tm;@Ce|i6Yw)g=quiTdAKlih7~*jW`0*AcuY+f+Y{n-<2psg`=)1W&w0qq_ypG90i_w-wH(}=L9-9IeZXlsaQlF31+Eo1EeEa@xW|Ee95^ip?s4Fr2JUI# zo;FZTBF7&6?Vh-7YTXRRC0IO=_XOn4LdFxI$%VWroNHkbALlN+-3_@)Px8})HI*jF zx6N}~<**00o3(gtW&Oqzy zUFx zUC{=eZ>rC(UsW+&|5W{^yNWAKNW*osr30WWKuuQ8-_RA#|K?<{9QIK)(Ns5F>#&;Y zuB{xt3oULi-MzGO6H?pVg|?-YMvoEuCd8KoQ^6dZYdf*4puJ?#%dEvyh@Gf?6U>RY zeaB(VzG36E%`9G@4Oo%+emn*HPalJw(-6Hj&sQ5n+IEM(xIwVq98 zmasf2<8{KtRFyFkVkU+WP=O5RuvPM5r_g;IuP5-%2JZv$nxd1hG$Jmq#vUy`$3DXx zn?`JOIW2ul7^iIkZQv5^AO8U@e~H%U1z+zB-#7jNt#FC91+;-nv@iVwTEit;V*~g$ z%<%p37ql)zH|B|hpV*GmnkI8c5n)kMr+3hViE+_$231c$di%3i97}Q8_4G(??s-Eyk{!e@?GTJQ+i7wUYF9y z_z>PhQU2}t|4XUL#fUn6F%pcbk(;&l)W{CR8hBe+nT=Q5hJvG`B%Q?z*ZBPql?UGK ztzt`?5ycR*ZiL&8*P2xwP9~6*gR^q`IlQk0rhxt*1G0D$w@Mptxv{9QeC67;Ye@oJ zUUcos6-8)8`&)rGh!&(fkt&dGMJhpBijcnHhjq5g4l&oI}I?N;O@74?=youx> z4w8lY!hEEeGA=)7Ze7JK+f{@{$On*FDVPQZt!n2%Cb6dL6?$XttAPBm?Pn@XBBe zXbJ08M#*hp%5vxg&n5GT5fXRp)^9&>clpYtb$8t_VdVB@g+norC<2lPdg0^{dnGpSeetr2bQIA|+FV68&dtWX~RR4$d{y!?q(nd}@G+*o0jL3Yj+qJ+{ Yqx^dbnEZs22s4=`v;e)g{cryMHwcU7Pyhe` literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/KakuteH7-bdshot_bl.hex b/Tools/bootloaders/KakuteH7-bdshot_bl.hex new file mode 100644 index 00000000000..62e1f9bf182 --- /dev/null +++ b/Tools/bootloaders/KakuteH7-bdshot_bl.hex @@ -0,0 +1,975 @@ +:020000040800F2 +:1000000000060020A1020008950F0008150F000847 +:100010006D0F0008150F0008410F0008A30200082B +:10002000A3020008A3020008A3020008812200081E +:10003000A3020008A3020008A3020008A30200080C +:10004000A3020008A3020008A3020008A3020008FC +:10005000A3020008A3020008F13600081D370008BB +:100060004937000875370008A1370008A3020008C7 +:10007000A3020008A3020008A3020008A3020008CC +:10008000A3020008A3020008A3020008A3020008BC +:10009000A3020008A3020008A3020008CD3700084D +:1000A000A3020008A3020008A3020008A30200089C +:1000B000A3020008A3020008A3020008A30200088C +:1000C000A3020008A3020008A3020008A30200087C +:1000D000A3020008A3020008A3020008A30200086C +:1000E00031380008A3020008A3020008A302000898 +:1000F000A3020008A3020008A3020008A30200084C +:10010000A3020008A3020008A5380008A302000803 +:10011000A3020008A3020008A3020008A30200082B +:10012000A3020008A3020008A3020008A30200081B +:10013000A3020008A3020008A3020008A30200080B +:10014000A3020008A3020008A3020008A3020008FB +:10015000A3020008A3020008A3020008A3020008EB +:10016000A3020008A3020008A3020008A3020008DB +:10017000A3020008ED2D0008A3020008A302000856 +:10018000A3020008A3020008A3020008A3020008BB +:10019000A3020008A3020008A3020008A3020008AB +:1001A000A3020008A3020008A3020008A30200089B +:1001B000A3020008A3020008A3020008A30200088B +:1001C000A3020008A3020008A3020008A30200087B +:1001D000A3020008D92D0008A3020008A30200080A +:1001E000A3020008A3020008A3020008A30200085B +:1001F000A3020008A3020008A3020008A30200084B +:10020000A3020008A3020008A3020008A30200083A +:10021000A3020008A3020008A3020008A30200082A +:10022000A3020008A3020008A3020008A30200081A +:10023000A3020008A3020008A3020008A30200080A +:10024000A3020008A3020008A3020008A3020008FA +:10025000A3020008A3020008A3020008A3020008EA +:10026000A3020008A3020008A3020008A3020008DA +:10027000A3020008A3020008A3020008A3020008CA +:10028000A3020008A3020008A3020008A3020008BA +:10029000A3020008A3020008A3020008A3020008AA +:1002A00002E000F000F8FEE772B63A4880F30888F2 +:1002B000394880F3098839484EF60851CEF20001DA +:1002C000086040F20000CCF200004EF63471CEF22D +:1002D00000010860BFF34F8FBFF36F8F40F2000043 +:1002E000C0F2F0004EF68851CEF200010860BFF374 +:1002F0004F8FBFF36F8F4FF00000E1EE100A4EF604 +:100300003C71CEF200010860062080F31488BFF330 +:100310006F8F02F073F802F015F802F0AFFF4FF0A4 +:1003200055301F491B4A91423CBF41F8040BFAE784 +:100330001C49194A91423CBF41F8040BFAE71A499B +:100340001A4A1B4B9A423EBF51F8040B42F8040B69 +:10035000F8E700201749184A91423CBF41F8040BC6 +:10036000FAE702F02DF803F001F8144C144DAC42FA +:1003700003DA54F8041B8847F9E700F041F8114C00 +:10038000114DAC4203DA54F8041B8847F9E702F038 +:1003900015B8000000060020002200200000000820 +:1003A0000000002000060020703C00080022002011 +:1003B0005C220020602200207C420020A002000875 +:1003C000A0020008A0020008A00200082DE9F04FDA +:1003D0002DED108AC1F80CD0D0F80CD0BDEC108AED +:1003E000BDE8F08F002383F311882846A047002042 +:1003F00001F038FBFEE701F0CDFA00DFFEE7000078 +:1004000038B500F0C1FC30B1164B00220E211A7233 +:100410005A729972DA7201F0F3FE054601F026FF76 +:100420000446D0B9104B9D4219D001339D4241F290 +:10043000883512BF044600250124002001F0EAFEA1 +:100440000CB100F059F800F039FD00F0E3FB28464C +:1004500000F004F900F050F8F9E70025EDE7054653 +:10046000EBE700BF00220020010007B008B500F054 +:10047000A3FBA0F120035842584108BD07B541F243 +:100480001203022101A8ADF8043000F0B3FB03B061 +:100490005DF804FB202310B583F311881248C3686C +:1004A0000BB101F065FB0023104A4FF47A710E483E +:1004B00001F022FB002383F311880D4C236813B154 +:1004C0002368013B2360636813B16368013B636089 +:1004D000084B1B7833B9636823B9022000F068FC2D +:1004E0003223636010BD00BF602200209504000825 +:1004F0007C23002074220020F8B5514B514A1C4641 +:100500001968013100F09B8004339342F8D162688E +:100510004D4B9A4240F293804C4B9B6803F1006331 +:1005200003F500339A4280F08A80002000F08CFBB3 +:100530000220474B187000F031FC464B0021D3F8E5 +:10054000E820C3F8E810D3F81021C3F81011D3F84D +:100550001021D3F8EC20C3F8EC10D3F81421C3F821 +:100560001411D3F81421D3F8F020C3F8F010D3F805 +:100570001821C3F81811D3F81821D3F8802042F0BD +:100580000062C3F88020D3F8802022F00062C3F814 +:100590008020D3F88020D3F8802042F00072C3F886 +:1005A0008020D3F8802022F00072C3F88020D3F896 +:1005B000803072B64FF0E023C3F8084DD4E9000450 +:1005C000BFF34F8FBFF36F8F234AC2F88410BFF37E +:1005D0004F8F536923F480335361BFF34F8FD2F8A9 +:1005E000803043F6E076C3F3C905C3F34E335B01B5 +:1005F00003EA060C29464CEA81770139C2F8747285 +:10060000F9D2203B13F1200FF2D1BFF34F8FBFF38C +:100610006F8FBFF34F8FBFF36F8F536923F4003396 +:1006200053610023C2F85032BFF34F8FBFF36F8F77 +:10063000202383F31188854680F308882047F8BD7E +:100640000000020820000208FFFF0108002200202D +:10065000742200200044025800ED00E02DE9F04F24 +:1006600093B0A94B2022FF2100900AA89D6800F0BA +:10067000CFFBA64A1378A3B90121A5481170C36026 +:10068000202383F31188C3680BB101F071FA0023B2 +:10069000A04A4FF47A719E4801F02EFA002383F3AA +:1006A0001188009B9C4A03B1136000239B49009C66 +:1006B00098469B461E469A460B705360012000F0F8 +:1006C0006DFB24B1944B1B68002B00F016820020B8 +:1006D00000F072FA0390039B002BF2DB012000F084 +:1006E00055FB039B213B162BE8D801A252F823F0BF +:1006F0004D0700087507000809080008BD06000836 +:10070000BD060008BD0600089D0800086F0A000825 +:1007100089090008EB090008130A0008390A0008D3 +:10072000BD0600084B0A0008BD060008BD0A000807 +:10073000ED070008BD060008010B00085907000876 +:10074000ED070008BD060008EB0900080220FFF7CE +:100750008DFE002840F0FB81009B022105A8B8F126 +:10076000000F08BF1C4641F21233ADF8143000F000 +:1007700041FAA3E74FF47A7000F01EFA071EEBDB94 +:100780000220FFF773FE0028E6D0013F052F00F29C +:10079000E081DFE807F0030A0D101336052304217A +:1007A00005A8059300F026FA17E004215648F9E75A +:1007B00004215B48F6E704215A48F3E74FF01C098F +:1007C000484609F1040900F041FA0421059005A802 +:1007D00000F010FAB9F12C0FF2D101204FF0000A0D +:1007E00000FA07F747EA0B0B5FFA8BFB00F05CFBA4 +:1007F00026B10BF00B030B2B08BF0024FFF73EFEC6 +:100800005CE704214848CDE7002EA5D00BF00B0390 +:100810000B2BA1D10220FFF729FE074600289BD011 +:1008200001203E4E00F010FA4FF000080220307018 +:1008300000F0B4FA5FFA88F9484600F015FA044669 +:1008400090B1484608F1010800F01EFA0028F1D1E5 +:10085000B846044641F21213022105A83E46ADF8FF +:10086000143000F0C7F929E7012325460220337030 +:1008700000F08CFA244B9B68AB4207D9284600F065 +:10088000E5F9013040F068810435F3E70025234B9A +:10089000B8463E461D70204B5D60A7E7002E3FF432 +:1008A0005BAF0BF00B030B2B7FF456AF02201B4BFF +:1008B000187000F073FA322000F07EF9B0F10009F0 +:1008C000FFF64AAF19F003077FF446AF0E4A09EB73 +:1008D0000503926893423FF63FAFB9F5807F3FF73B +:1008E0003BAF124BB945019322DD4FF47A7000F013 +:1008F00063F90390039A002AFFF62EAF039A01379B +:10090000019B03F8012BEDE7002200207823002053 +:1009100060220020950400087C230020742200201F +:1009200004220020082200200C220020782200202F +:10093000C820FFF79BFD074600283FF40DAF1F2D91 +:1009400011D8C5F120020AAB25F0030084494A45BD +:10095000184428BF4A46019200F034FA019AFF2158 +:100960007F4800F055FA4FEAA903C9F387027C4992 +:100970002846019300F054FA064600283FF46AAF77 +:10098000019B05EB830531E70220FFF76FFD00288F +:100990003FF4E2AE00F094F900283FF4DDAE00270A +:1009A000B946704B9B68BB4218D91F2F11D80A9BC0 +:1009B00001330ED027F0030312AA134453F8203C4E +:1009C00005934846042205A9043700F0E5FA81465C +:1009D000E7E7384600F03AF90590F2E7CDF81490D1 +:1009E000042105A800F006F900E70023642104A80B +:1009F000049300F0F5F800287FF4AEAE0220FFF774 +:100A000035FD00283FF4A8AE049800F04FF905909A +:100A1000E6E70023642104A8049300F0E1F800282D +:100A20007FF49AAE0220FFF721FD00283FF494AE38 +:100A3000049800F03DF9EAE70220FFF717FD0028CF +:100A40003FF48AAE00F04CF9E1E70220FFF70EFD1B +:100A500000283FF481AE05A9142000F047F90746AD +:100A60000421049004A800F0C5F83946B9E7322003 +:100A700000F0A2F8071EFFF66FAEBB077FF46CAE66 +:100A8000384A07EB0A03926893423FF665AE0220AC +:100A9000FFF7ECFC00283FF45FAE27F00307574454 +:100AA000BA453FF4A3AE50460AF1040A00F0CEF86E +:100AB0000421059005A800F09DF8F1E74FF47A7045 +:100AC000FFF7D4FC00283FF447AE00F0F9F8002807 +:100AD00044D00A9B01330BD008220AA9002000F061 +:100AE0009FF900283AD02022FF210AA800F090F9AF +:100AF000FFF7C4FC1C4800F0BBFF13B0BDE8F08F4B +:100B0000002E3FF429AE0BF00B030B2B7FF424AE29 +:100B10000023642105A8059300F062F80746002829 +:100B20007FF41AAE0220FFF7A1FC814600283FF4B3 +:100B300013AEFFF7A3FC41F2883000F099FF05984F +:100B400000F0E6F94E463C4600F0AEF9B6E5064642 +:100B50004CE64FF0000AFFE5B8467BE6374679E6FB +:100B60007822002000220020A08601000F4B70B5E3 +:100B70001B780C460133DBB2012B11D80C4D4FF41E +:100B80007A732968A2FB033222460E6A0146284680 +:100B9000B047844204D1074B002201201A7070BD77 +:100BA0004FF4FA7000F064FF0020F8E710220020F4 +:100BB00070250020B0230020002307B50246012144 +:100BC0000DF107008DF80730FFF7D0FF20B19DF839 +:100BD000070003B05DF804FB4FF0FF30F9E70000B9 +:100BE0000A46042108B5FFF7C1FF80F00100C0B23A +:100BF000404208BD30B4054C0A46014623682046F1 +:100C0000DD69034BAC4630BC604700BF7025002057 +:100C1000A086010070B50A4E00240A4D01F0D6F9F5 +:100C2000308028683388834208D901F0CBF92B68DB +:100C300004440133B4F5003F2B60F2D370BD00BF14 +:100C4000B22300208423002001F090BA00F100605C +:100C500000F500300068704700F10060920000F578 +:100C6000003001F00FBA0000054B1A68054B1B88D5 +:100C70009B1A834202D9104401F0A4B900207047A6 +:100C800084230020B223002038B5074D044628688D +:100C9000204401F09DF928B928682044BDE8384077 +:100CA00001F0A8B938BD00BF8423002000207047A0 +:100CB00000F1FF5000F58F10D0F8000870470000D9 +:100CC000064991F8243033B100230822086A81F8DC +:100CD0002430FFF7C1BF0120704700BF88230020E8 +:100CE000014B1868704700BF0010005C244BF0B542 +:100CF0001A680446234BC2F30B06120C1F8858686F +:100D0000BE4293F9085028D09F89BE4206D10120E7 +:100D10000C2505FB0033586893F9085041F2010394 +:100D20009A421CD041F203039A421AD042F20103C4 +:100D30009A4218D042F203039A4208BF5625621E17 +:100D40000B46441E0A4493420FD214F9016F581CFB +:100D50006EB1034600F8016CF5E70020D8E75A258C +:100D6000EDE75925EBE75825E9E7184605E02C247F +:100D700082421C7001D9981C5D70401AF0BD00BF02 +:100D80000010005C1422002000207047022802D1CD +:100D9000014B04229A61704700080258022803D1CF +:100DA000024B4FF480229A61704700BF000802583E +:100DB000022804D1024A536983F004035361704747 +:100DC00000080258002310B5934203D0CC5CC454F1 +:100DD0000133F9E710BD0000013810B510F9013FEB +:100DE0003BB191F900409C4203D11AB10131013A63 +:100DF000F4E71AB191F90020981A10BD1046FCE7EB +:100E000003460246D01A12F9011B0029FAD1704795 +:100E100002440346934202D003F8011BFAE77047ED +:100E20002DE9F8431F4D14460746884695F82420BF +:100E300052BBDFF870909CB395F824302BB9202278 +:100E4000FF2148462F62FFF7E3FF95F82400414653 +:100E5000C0F1080205EB8000A24228BF2246D6B2AC +:100E60009200FFF7AFFF95F82430A41B17441E44EF +:100E70009044E4B2F6B2082E85F82460DBD1FFF787 +:100E80001FFF0028D7D108E02B6A03EB82038342BF +:100E9000CFD0FFF715FF0028CBD10020BDE8F883A5 +:100EA0000120FBE788230020024B1A78024B1A70BE +:100EB000704700BFB02300201022002010B5104C56 +:100EC000104800F0FBF821460E4800F023F9246892 +:100ED0000D48D4F89020D2F8043843F00203C2F849 +:100EE000043800F0C5FD0949204600F021FAD4F885 +:100EF0009020D2F8043823F00203C2F8043810BD61 +:100F0000503A00087025002040420F00583A00086F +:100F10007047000000B59BB0EFF309816822684676 +:100F2000FFF750FFEFF30583044B9A6BDA6A9A6A76 +:100F30009A6A9A6A9A6A9A6A9B6AFEE700ED00E0EA +:100F400000B59BB0EFF3098168226846FFF73AFFCE +:100F5000EFF30583044B9A6B9A6A9A6A9A6A9A6AC3 +:100F60009A6A9B6AFEE700BF00ED00E000B59BB007 +:100F7000EFF3098168226846FFF724FFEFF305834A +:100F8000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE754 +:100F900000ED00E0FEE7000030B50A44084D914244 +:100FA0000DD011F8013B5840082340F30004013BE9 +:100FB0002C4013F0FF0384EA5000F6D1EFE730BD78 +:100FC0002083B8ED026843681143016003B11847FC +:100FD0007047000013B5406B00F58054D4F8A43876 +:100FE0001A681178042914D1017C022911D11979C8 +:100FF000012312898B4013420BD101A94C3002F01E +:101000003DF8D4F8A4480246019B2179206800F0FD +:10101000DFF902B010BD0000143001F0BFBF0000C6 +:101020004FF0FF33143001F0B9BF00004C3002F034 +:1010300091B800004FF0FF334C3002F08BB8000045 +:10104000143001F08DBF00004FF0FF31143001F07B +:1010500087BF00004C3002F05DB800004FF0FF3257 +:101060004C3002F057B800000020704710B500F572 +:101070008054D4F8A4381A681178042917D1017C57 +:10108000022914D15979012352898B4013420ED180 +:10109000143001F01FFF024648B1D4F8A4484FF4C1 +:1010A000407361792068BDE8104000F07FB910BD41 +:1010B000406BFFF7DBBF0000704700007FB5124BAD +:1010C00001250426044603600023057400F1840210 +:1010D00043602946C0E902330C4B0290143001935F +:1010E0004FF44073009601F0D1FE094B04F6944290 +:1010F000294604F14C000294CDE900634FF440739B +:1011000001F098FF04B070BD8C390008B1100008E0 +:10111000D50F00080A68202383F311880B790B335D +:1011200042F823004B79133342F823008B7913B133 +:101130000B3342F8230000F58053C3F8A4180223B0 +:101140000374002383F311887047000038B5037FD0 +:10115000044613B190F85430ABB90125201D02218B +:10116000FFF730FF04F114006FF00101257700F064 +:10117000A7FC04F14C0084F854506FF00101BDE865 +:10118000384000F09DBC38BD10B501210446043044 +:10119000FFF718FF0023237784F8543010BD0000B8 +:1011A00038B504460025143001F088FE04F14C00E7 +:1011B000257701F057FF201D84F854500121FFF7D7 +:1011C00001FF2046BDE83840FFF750BF90F880305F +:1011D00003F06003202B06D190F881200023212A00 +:1011E00003D81F2A06D800207047222AFBD1C0E965 +:1011F0001D3303E0034A426707228267C367012069 +:10120000704700BF2C22002037B500F58055D5F877 +:10121000A4381A68117804291AD1017C022917D13F +:101220001979012312898B40134211D100F14C042A +:10123000204601F0D7FF58B101A9204601F01EFF5A +:10124000D5F8A4480246019B2179206800F0C0F837 +:1012500003B030BD01F10B03F0B550F8236085B049 +:1012600004460D46FEB1202383F3118804EB850765 +:10127000301D0821FFF7A6FEFB6806F14C005B69F4 +:101280001B681BB1019001F007FF019803A901F051 +:10129000F5FE024648B1039B2946204600F098F827 +:1012A000002383F3118805B0F0BDFB685A6912680A +:1012B000002AF5D01B8A013B1340F1D104F18002D2 +:1012C000EAE70000133138B550F82140ECB1202393 +:1012D00083F3118804F58053D3F8A4281368527956 +:1012E00003EB8203DB689B695D6845B104216018EC +:1012F000FFF768FE294604F1140001F0F5FD2046D1 +:10130000FFF7B4FE002383F3118838BD7047000057 +:1013100001F0E8B801234022002110B5044600F88E +:10132000303BFFF775FD0023C4E9013310BD000019 +:1013300010B52023044683F3118824224160002144 +:101340000C30FFF765FD204601F0EEF80223237014 +:10135000002383F3118810BD70B500EB81030546AF +:1013600050690E461446DA6018B110220021FFF7CA +:101370004FFDA06918B110220021FFF749FD314649 +:101380002846BDE8704001F0E1B9000083682022E2 +:10139000002103F0011310B5044683601030FFF7FD +:1013A00037FD2046BDE8104001F05CBAF0B40125DD +:1013B00000EB810447898D40E4683D43A469458181 +:1013C00023600023A2606360F0BC01F079BA0000E2 +:1013D000F0B4012500EB810407898D40E4683D43AA +:1013E0006469058123600023A2606360F0BC01F0A2 +:1013F000EFBA000070B502230025044624220370D2 +:101400002946C0F888500C3040F8045CFFF700FD16 +:10141000204684F8705001F02DF963681B6823B1F1 +:1014200029462046BDE87040184770BD037880F813 +:101430008C300523037043681B6810B504460BB15C +:10144000042198470023A36010BD000090F88C2071 +:10145000436802701B680BB10521184770470000F4 +:1014600070B590F87030044613B1002380F87030E6 +:1014700004F18002204601F019FA63689B68B3B951 +:1014800094F8803013F0600535D00021204601F03B +:10149000C3FC0021204601F0B3FC63681B6813B154 +:1014A000062120469847062384F8703070BD2046F8 +:1014B00098470028E4D0B4F88630A26F9A4288BFDB +:1014C000A36794F98030A56F002B4FF0200380F2C2 +:1014D0000381002D00F0F280092284F8702083F34C +:1014E000118800212046D4E91D23FFF771FF002356 +:1014F00083F31188DAE794F8812003F07F0343EA4D +:10150000022340F20232934200F0C58021D8B3F5A5 +:10151000807F48D00DD8012B3FD0022B00F0938064 +:10152000002BB2D104F1880262670222A267E3674E +:10153000C1E7B3F5817F00F09B80B3F5407FA4D174 +:1015400094F88230012BA0D1B4F8883043F0020324 +:1015500032E0B3F5006F4DD017D8B3F5A06F31D09E +:10156000A3F5C063012B90D86368204694F88220CD +:101570005E6894F88310B4F88430B047002884D0B3 +:10158000436863670368A3671AE0B3F5106F36D04A +:1015900040F6024293427FF478AF5C4B63670223CC +:1015A000A3670023C3E794F88230012B7FF46DAF6B +:1015B000B4F8883023F00203A4F88830C4E91D553C +:1015C000E56778E7B4F88030B3F5A06F0ED194F8F2 +:1015D0008230204684F88A3001F0AAF863681B68DC +:1015E00013B1012120469847032323700023C4E947 +:1015F0001D339CE704F18B0363670123C3E7237862 +:10160000042B10D1202383F311882046FFF7BEFE60 +:1016100085F311880321636884F88B5021701B685F +:101620000BB12046984794F88230002BDED084F826 +:101630008B300423237063681B68002BD6D00221F3 +:1016400020469847D2E794F8843020461D0603F0E0 +:101650000F010AD501F01CF9012804D002287FF4FB +:1016600014AF2B4B9AE72B4B98E701F003F9F3E704 +:1016700094F88230002B7FF408AF94F8843013F094 +:101680000F01B3D01A06204602D501F0DDFBADE70D +:1016900001F0CEFBAAE794F88230002B7FF4F5AE80 +:1016A00094F8843013F00F01A0D01B06204602D519 +:1016B00001F0B2FB9AE701F0A3FB97E7142284F84C +:1016C000702083F311882B462A4629462046FFF7CF +:1016D0006DFE85F31188E9E65DB1152284F870206E +:1016E00083F3118800212046D4E91D23FFF75EFE15 +:1016F000FDE60B2284F8702083F311882B462A46DE +:1017000029462046FFF764FEE3E700BFBC39000826 +:10171000B4390008B839000838B590F8703004467C +:10172000002B3ED0063BDAB20F2A34D80F2B32D82A +:10173000DFE803F0373131082232313131313131D4 +:1017400031313737856FB0F886309D4214D2C36887 +:101750001B8AB5FBF3F203FB12556DB9202383F30B +:1017600011882B462A462946FFF732FE85F3118859 +:101770000A2384F870300EE0142384F8703020239C +:1017800083F31188002320461A461946FFF70EFE00 +:10179000002383F3118838BDC36F03B1984700233A +:1017A000E7E70021204601F037FB0021204601F049 +:1017B00027FB63681B6813B1062120469847062360 +:1017C000D7E7000010B590F870300446142B29D0EC +:1017D00017D8062B05D001D81BB110BD093B022B31 +:1017E000FBD80021204601F017FB0021204601F024 +:1017F00007FB63681B6813B1062120469847062340 +:1018000019E0152BE9D10B2380F87030202383F3E6 +:10181000118800231A461946FFF7DAFD002383F3E7 +:101820001188DAE7C3689B695B68002BD5D1C36F69 +:1018300003B19847002384F87030CEE7024B0022B2 +:10184000C3E900339A607047B42300200023826804 +:101850000374054B1B6899689142FBD25A68036078 +:101860004260106058607047B423002008B5202300 +:1018700083F31188037C032B05D0042B0DD02BB9E7 +:1018800083F3118808BD436900221A604FF0FF33CB +:101890004361FFF7DBFF0023F2E7D0E9003213607A +:1018A0005A60F3E7002382680374054B1B6899684C +:1018B0009142FBD85A6803604260106058607047DC +:1018C000B4230020054B1969087418680268536036 +:1018D0001A60186101230374FEF778BDB423002059 +:1018E0004B1C30B5044687B00A4D10D02B6901A8B7 +:1018F000094A00F001F92046FFF7E4FF049B13B109 +:1019000001A800F035F92B69586907B030BDFFF721 +:10191000D9FFF8E7B42300206D18000838B50C4D46 +:10192000044641612B6981689A68914203D8BDE8F9 +:101930003840FFF78BBF1846FFF7B4FF01232C6137 +:10194000014623742046BDE83840FEF73FBD00BF86 +:10195000B4230020044B1A681B6990689B68984266 +:1019600094BF002001207047B423002010B5084C1C +:10197000236820691A6854602260012223611A7466 +:10198000FFF790FF01462069BDE81040FEF71EBD3D +:10199000B4230020FFF7EABFFEE7000010B50C4CAF +:1019A000FFF74CFF00F09AF880220A49204600F029 +:1019B00021F8012344F8180C037400F071FC002393 +:1019C00083F3118862B60448BDE8104000F032B8D5 +:1019D000DC230020C0390008D039000800F002B92B +:1019E000034A516853685B1A9842FBD8704700BF9E +:1019F000001000E082600222028270478368A3F137 +:101A00007C0243F80C2C026943F83C2C426943F8F1 +:101A1000382C074A43F81C2CC268A3F1180043F87D +:101A2000102C022203F8082C002203F8072C704720 +:101A3000E503000810B5202383F31188FFF7DEFFCC +:101A400000210446FFF76AFF002383F31188204634 +:101A500010BD0000024B1B6958610F20FFF732BF19 +:101A6000B4230020202383F31188FFF7F3BF000085 +:101A700008B50146202383F311880820FFF730FFC3 +:101A8000002383F3118808BD49B1064B42681B69E6 +:101A900018605A60136043600420FFF721BF4FF0C5 +:101AA000FF307047B42300200368984206D01A68BC +:101AB0000260506018465961FFF7C8BE70470000C9 +:101AC00038B504460D462068844200D138BD03680D +:101AD00023605C604561FFF7B9FEF4E7054B4FF00A +:101AE000FF3103F11402C3E905220022C3E9071202 +:101AF000704700BFB423002070B51C4E05460C464D +:101B0000C0E9032301F0A4FB334653F8142F9A4293 +:101B10000DD130620A2C2CBF00190A302A60C5E9A9 +:101B20000124C6E90555BDE8704001F07BBB316A70 +:101B3000431AE31838BF1C469368A34202D9081918 +:101B400001F080FB73699A6894420CD85A68AC60C3 +:101B50002B606A6015609A685D60121B9A604FF096 +:101B6000FF33F36170BDA41A1B68ECE7B4230020B7 +:101B700038B51B4C636998420DD08168D0E90032BA +:101B800013605A600022C2609A680A449A604FF05B +:101B9000FF33E36138BD03682246002142F8143F59 +:101BA00093425A60C16003D1BDE8384001F044BBA4 +:101BB0009A688168256A0A449A6001F049FB636962 +:101BC000411B9A688A42E5D9AB181D1A206A092D73 +:101BD00098BF01F10A02BDE83840104401F032BB61 +:101BE000B42300202DE9F041184C002704F114061D +:101BF000656901F02DFB236AAA68C11A8A4215D8CB +:101C00001344D5F80C802362D5E9003213605A6082 +:101C10006369EF60B34201D101F00EFB87F31188D5 +:101C20002869C047202383F31188E1E76169B14245 +:101C300009D013441B1ABDE8F0410A2B2CBFC01871 +:101C40000A3001F0FFBABDE8F08100BFB4230020E4 +:101C500000207047FEE70000704700004FF0FF30A3 +:101C60007047000002290CD0032904D0012907483D +:101C700018BF00207047032A05D8054800EBC200B2 +:101C80007047044870470020704700BFB43A00080E +:101C90003C220020683A000870B59AB00546084614 +:101CA000144601A900F0C2F801A8FFF7A9F8431CE7 +:101CB0000022C6B25B001046C5E90034237003233E +:101CC000023404F8013C01ABD1B202348E4201D897 +:101CD0001AB070BD13F8011B013204F8010C04F8AE +:101CE000021CF1E708B5202383F311880348FFF7AE +:101CF0008BFA002383F3118808BD00BF70250020F4 +:101D000090F8803003F01F02012A07D190F881205B +:101D10000B2A03D10023C0E91D3315E003F0600353 +:101D2000202B08D1B0F884302BB990F88120212ADB +:101D300003D81F2A04D8FFF749BA222AEBD0FAE7C2 +:101D4000034A426707228267C3670120704700BFCA +:101D50003322002007B5052917D8DFE801F019164E +:101D600003191920202383F31188104A01210190BF +:101D7000FFF7F2FA019802210D4AFFF7EDFA0D483C +:101D8000FFF70EFA002383F3118803B05DF804FB1C +:101D9000202383F311880748FFF7D8F9F2E72023BF +:101DA00083F311880348FFF7EFF9EBE7083A0008DF +:101DB0002C3A00087025002038B50C4D0C4C2A46F2 +:101DC0000C4904F10800FFF767FF05F1CA0204F1AE +:101DD00010000949FFF760FF05F5CA7204F1180009 +:101DE0000649BDE83840FFF757BF00BF483E002016 +:101DF0003C220020E8390008F2390008FD390008CB +:101E000070B5044608460D46FEF7FAFFC6B22046F6 +:101E1000013403780BB9184670BD32462946FEF7E7 +:101E2000DBFF0028F3D10120F6E700002DE9F047A1 +:101E300005460C46FEF7E4FF2C49C6B22846FFF7DC +:101E4000DFFF08B10836F6B229492846FFF7D8FF68 +:101E500008B11036F6B2632E0BD8DFF89080DFF8A9 +:101E60009090244FDFF898A02E7846B92670BDE8F0 +:101E7000F08729462046BDE8F04701F071BD252EC8 +:101E800030D1072241462846FEF7A6FF80B91A4BFB +:101E9000224603F10C0153F8040B8B4242F8040B69 +:101EA000F9D1198807359B780F3411809370DBE7DF +:101EB000082249462846FEF78FFF98B9A21C0F4B0F +:101EC000197802320909C95D02F8041C13F8011BD4 +:101ED00001F00F015345C95D02F8031CF0D118341D +:101EE0000835C1E7013504F8016BBDE7D43A0008B5 +:101EF000FD390008EC3A0008DC3A000800E8F11F60 +:101F00000CE8F11FBFF34F8F044B1A695107FCD146 +:101F1000D3F810215207F8D1704700BF00200052BB +:101F200008B50D4B1B78ABB9FFF7ECFF0B4BDA682C +:101F3000D10704D50A4A5A6002F188325A60D3F8B0 +:101F40000C21D20706D5064AC3F8042102F18832D3 +:101F5000C3F8042108BD00BFA640002000200052A5 +:101F60002301674508B5114B1B78F3B9104B1A696B +:101F7000510703D5DA6842F04002DA60D3F8102145 +:101F8000520705D5D3F80C2142F04002C3F80C21CA +:101F9000FFF7B8FF064BDA6842F00102DA60D3F8C7 +:101FA0000C2142F00102C3F80C2108BDA64000201C +:101FB000002000520F289ABF00F5806040040020E6 +:101FC000704700004FF40030704700001020704749 +:101FD0000F2808B50BD8FFF7EDFF00F500330268B6 +:101FE000013204D104308342F9D1012008BD002020 +:101FF000FCE700000F2838B505463FD8FFF782FF01 +:102000001F4CFFF78DFF4FF0FF3307286361C4F8C3 +:1020100014311DD82361FFF775FF030243F0240339 +:10202000E360E36843F08003E36023695A07FCD46C +:102030002846FFF767FFFFF7BDFF4FF4003100F0C0 +:10204000F7F82846FFF78EFFBDE83840FFF7C0BF1E +:10205000C4F81031FFF756FFA0F108031B0243F04C +:102060002403C4F80C31D4F80C3143F08003C4F8D5 +:102070000C31D4F810315B07FBD4D9E7002038BD10 +:10208000002000522DE9F84F05460C46104645EA5F +:102090000203DB065DD122F003022B462A44934261 +:1020A00009D120F01F00DFF8BCB0DFF8BCA0FFF7BB +:1020B00037FF271844E0196801314AD10433EEE7AD +:1020C00005F1784324492548B3F5801F244B38BFD8 +:1020D000184603F1F80339BF8846D9469846D146D9 +:1020E000FFF710FF4FF0FF33A5EB040C04F11C01C8 +:1020F0000360D8F8003043F00203C8F80030231F13 +:10210000D9F8006016F00506FAD153F8042F994269 +:102110004CF80320F4D1BFF34F8FFFF7F3FE4FF0DD +:10212000FF332022214603602846D8F8003023F0F0 +:102130000203C8F8003001F003FC40B92035203418 +:10214000BC42BDD10120FFF70DFFBDE8F88F30463E +:10215000F9E70020F9E700BF0C20005214210052DB +:1021600014200052102000521021005210B5084CCB +:10217000237828B11BB9FFF7D3FE0123237010BDCC +:10218000002BFCD02070BDE81040FFF7EBBE00BF75 +:10219000A64000200244074BD2B210B5904200D1B5 +:1021A00010BD441C00B253F8200041F8040BE0B20B +:1021B000F4E700BF504000580E4B30B51C6F2404AC +:1021C00005D41C6F1C671C6F44F400441C670A4C48 +:1021D00002442368D2B243F480732360074B9042D9 +:1021E00000D130BD441C51F8045B00B243F82050CC +:1021F000E0B2F4E70044025800480258504000584A +:1022000007B5012201A90020FFF7C4FF019803B020 +:102210005DF804FB13B50446FFF7F2FFA04205D0BA +:10222000012201A900200194FFF7C6FF02B010BDF2 +:102230000144BFF34F8F064B884204D3BFF34F8F47 +:10224000BFF36F8F7047C3F85C022030F4E700BF24 +:1022500000ED00E0034B1A681AB9034AD2F8D02403 +:102260001A607047A84000200040025808B5FFF7E8 +:10227000F1FF024B1868C0F3806008BDA840002041 +:10228000EFF30983054968334A6B22F001024A6380 +:1022900083F30988002383F31188704700EF00E07F +:1022A000202080F3118862B60D4B0E4AD96821F4C4 +:1022B000E0610904090C0A430B49DA60D3F8FC20F9 +:1022C00042F08072C3F8FC20084AC2F8B01F1168BF +:1022D00041F0010111601022DA7783F82200704783 +:1022E00000ED00E00003FA0555CEACC5001000E09B +:1022F000202310B583F311880E4B5B6813F4006341 +:1023000014D0F1EE103AEFF309844FF08073683C7B +:10231000E361094BDB6B236684F30988FFF71AFB43 +:1023200010B1064BA36110BD054BFBE783F3118889 +:10233000F9E700BF00ED00E000EF00E0F703000860 +:10234000FA03000870B5BFF34F8FBFF36F8F1A4ABF +:102350000021C2F85012BFF34F8FBFF36F8F536944 +:1023600043F400335361BFF34F8FBFF36F8FC2F855 +:102370008410BFF34F8FD2F8803043F6E074C3F37C +:10238000C900C3F34E335B0103EA0406014646EA83 +:1023900081750139C2F86052F9D2203B13F1200F48 +:1023A000F2D1BFF34F8F536943F480335361BFF3CE +:1023B0004F8FBFF36F8F70BD00ED00E0FEE70000B0 +:1023C0000A4B0B480B4A90420BD30B4BC11EDA1C35 +:1023D000121A22F003028B4238BF00220021FEF7BE +:1023E00017BD53F8041B40F8041BECE7CC3C000875 +:1023F0007C4200207C4200207C420020704700008C +:1024000070B5D0E9244300224FF0FF359E6804EBFD +:1024100042135101D3F80009002805DAD3F8000966 +:1024200040F08040C3F80009D3F8000B002805DA1B +:10243000D3F8000B40F08040C3F8000B0132631862 +:102440009642C3F80859C3F8085BE0D24FF0011375 +:10245000C4F81C3870BD000000EB8103D3F80CC039 +:102460002DE9F043DCF814204E1CD0F89050D2F83F +:1024700000E005EB063605EB4118506870450AD3BD +:102480000122D5F8343802FA01F123EA0101C5F836 +:102490003418BDE8F083AEEB0003BCF81040A34253 +:1024A00028BF2346D8F81849A4B2B3EB840FF0D85C +:1024B0009468A4F1040959F8047F3760A4EB090774 +:1024C0001F44042FF7D81C44034494605360D4E79E +:1024D000890141F02001016103699B06FCD41220AF +:1024E000FFF77EBA10B50A4C2046FEF713FF094BE2 +:1024F000C4F89030084BC4F89430084C2046FEF7DE +:1025000009FF074BC4F89030064BC4F8943010BD57 +:10251000AC40002000000840243B00084841002057 +:1025200000000440303B000870B503780546012BDD +:102530005DD1494BD0F89040984259D1474B0E217C +:102540006520D3F8D82042F00062C3F8D820D3F831 +:10255000002142F00062C3F80021D3F80021D3F833 +:10256000802042F00062C3F88020D3F8802022F05F +:102570000062C3F88020D3F8803000F071FC384B43 +:10258000E360384BC4F800380023D5F89060C4F8F5 +:10259000003EC02323604FF40413A3633369002B70 +:1025A000FCDA01230C203361FFF71AFA3369DB07E9 +:1025B000FCD41220FFF714FA3369002BFCDA002652 +:1025C0002846A660FFF71CFF6B68C4F81068DB683C +:1025D000C4F81468C4F81C68002B3AD1224BA361DC +:1025E0004FF0FF336361A36843F00103A36070BD44 +:1025F0001E4B9842C8D1194B0E214D20D3F8D8203C +:1026000042F00072C3F8D820D3F8002142F00072E3 +:10261000C3F80021D3F80021D3F8802042F00072E3 +:10262000C3F88020D3F8802022F00072C3F8802005 +:10263000D3F88020D3F8D82022F08062C3F8D820C5 +:10264000D3F8002122F08062C3F80021D3F80031D2 +:1026500093E7074BC3E700BFAC400020004402589B +:102660004014004003002002003C30C048410020DC +:10267000083C30C0F8B5D0F89040054600214FF036 +:1026800000662046FFF724FFD5F8941000234FF092 +:1026900001128F684FF0FF30C4F83438C4F81C289A +:1026A00004EB431201339F42C2F80069C2F8006B89 +:1026B000C2F80809C2F8080BF2D20B68D5F89020CE +:1026C000C5F89830636210231361166916F010067E +:1026D000FBD11220FFF784F9D4F8003823F4FE630D +:1026E000C4F80038A36943F4402343F01003A36106 +:1026F0000923C4F81038C4F814380B4BEB604FF0C2 +:10270000C043C4F8103B094BC4F8003BC4F810693F +:10271000C4F80039D5F8983003F1100243F480135F +:10272000C5F89820A362F8BD003B00084080001067 +:10273000D0F8902090F88A10D2F8003823F4FE6385 +:1027400043EA0113C2F80038704700002DE9F8434E +:1027500000EB8103D0F890500C468046DA680FFAFF +:1027600081F94801166806F00306731E022B05EB7B +:1027700041134FF0000194BFB604384EC3F8101B4C +:102780004FF0010104F1100398BF06F1805601FAE1 +:1027900003F3916998BF06F5004600293AD0578A9D +:1027A00004F15801374349016F50D5F81C180B4309 +:1027B0000021C5F81C382B180127C3F81019A740B1 +:1027C0005369611E9BB3138A928B9B08012A88BFB1 +:1027D0005343D8F89820981842EA034301F1400285 +:1027E0002146C8F89800284605EB82025360FFF79F +:1027F0006FFE08EB8900C3681B8A43EA84534834A0 +:102800001E4364012E51D5F81C381F43C5F81C78AF +:10281000BDE8F88305EB4917D7F8001B21F4004108 +:10282000C7F8001BD5F81C1821EA0303C0E704F120 +:102830003F030B4A2846214605EB83035A60FFF706 +:1028400047FE05EB4910D0F8003923F40043C0F8E7 +:102850000039D5F81C3823EA0707D7E700800010B5 +:1028600000040002D0F894201268C0F89820FFF706 +:10287000C7BD00005831D0F8903049015B5813F4BF +:10288000004004D013F4001F0CBF02200120704749 +:102890004831D0F8903049015B5813F4004004D01F +:1028A00013F4001F0CBF02200120704700EB8101D0 +:1028B000CB68196A0B6813604B685360704700005F +:1028C00000EB810330B5DD68AA691368D36019B9DC +:1028D000402B84BF402313606B8A1468D0F890208B +:1028E0001C4402EB4110013C09B2B4FBF3F4634316 +:1028F000033323F0030343EAC44343F0C043C0F867 +:10290000103B2B6803F00303012B0ED1D2F80838DB +:1029100002EB411013F4807FD0F8003B14BF43F06A +:10292000805343F00053C0F8003B02EB4112D2F851 +:10293000003B43F00443C2F8003B30BD2DE9F041B9 +:10294000D0F8906005460C4606EB4113D3F8087B9F +:102950003A07C3F8087B08D5D6F814381B0704D506 +:1029600000EB8103DB685B689847FA071FD5D6F850 +:102970001438DB071BD505EB8403D968CCB98B6908 +:10298000488A5A68B2FBF0F600FB16228AB918682A +:10299000DA6890420DD2121AC3E90024202383F38F +:1029A000118821462846FFF78BFF84F31188BDE884 +:1029B000F081012303FA04F26B8923EA02036B819D +:1029C000CB68002BF3D021462846BDE8F0411847DC +:1029D00000EB81034A0170B5DD68D0F890306C6976 +:1029E0002668E66056BB1A444FF40020C2F810096E +:1029F0002A6802F00302012A0AB20ED1D3F80808AD +:102A000003EB421410F4807FD4F8000914BF40F0A7 +:102A1000805040F00050C4F8000903EB4212D2F895 +:102A2000000940F00440C2F800090122D3F834083C +:102A300002FA01F10143C3F8341870BD19B9402EF0 +:102A400084BF4020206020681A442E8A8419013CEB +:102A5000B4FBF6F440EAC44040F00050C6E7000082 +:102A60002DE9F041D0F8906004460D4606EB411385 +:102A7000D3F80879C3F80879FB071CD5D6F81038C5 +:102A8000DA0718D500EB8103D3F80CC0DCF814305A +:102A9000D3F800E0DA6896451BD2A2EB0E024FF0A5 +:102AA00000081A60C3F80480202383F31188FFF71D +:102AB0008FFF88F311883B0618D50123D6F83428F8 +:102AC000AB40134212D029462046BDE8F041FFF743 +:102AD000C3BC012303FA01F2038923EA0203038141 +:102AE000DCF80830002BE6D09847E4E7BDE8F08139 +:102AF0002DE9F84FD0F8905004466E69AB691E403E +:102B000016F480586E6103D0BDE8F84FFEF772BC32 +:102B1000002E12DAD5F8003E9F0705D0D5F8003E0A +:102B200023F00303C5F8003ED5F80438204623F00F +:102B30000103C5F80438FEF789FC300505D52046A9 +:102B4000FFF75EFC2046FEF771FCB1040CD5D5F80A +:102B5000083813F0060FEB6823F470530CBF43F4EE +:102B6000105343F4A053EB60320704D56368DB686D +:102B70000BB120469847F30200F1BA80B70226D580 +:102B8000D4F8909000274FF0010A09EB4712D2F8D1 +:102B9000003B03F44023B3F5802F11D1D2F8003B62 +:102BA000002B0DDA62890AFA07F322EA0303638134 +:102BB00004EB8703DB68DB6813B13946204698478E +:102BC0000137D4F89430FFB29B689F42DDD9F006FC +:102BD00019D5D4F89000026AC2F30A1702F00F0365 +:102BE00002F4F012B2F5802F00F0CC80B2F5402F45 +:102BF00009D104EB8303002200F58050DB681B6AD7 +:102C0000974240F0B2803003D5F8185835D5E90323 +:102C100003D500212046FFF791FEAA0303D5012129 +:102C20002046FFF78BFE6B0303D502212046FFF7FA +:102C300085FE2F0303D503212046FFF77FFEE80220 +:102C400003D504212046FFF779FEA90203D505210B +:102C50002046FFF773FE6A0203D506212046FFF7E0 +:102C60006DFE2B0203D507212046FFF767FEEF011B +:102C700003D508212046FFF761FE700340F1A980CB +:102C8000E90703D500212046FFF7EAFEAA0703D58E +:102C900001212046FFF7E4FE6B0703D50221204601 +:102CA000FFF7DEFE2F0703D503212046FFF7D8FEEE +:102CB000EE0603D504212046FFF7D2FEA80603D571 +:102CC00005212046FFF7CCFE690603D506212046E4 +:102CD000FFF7C6FE2A0603D507212046FFF7C0FEF0 +:102CE000EB0576D520460821BDE8F84FFFF7B8BEC2 +:102CF000D4F8909000274FF0010AD4F894305FFA8E +:102D000087FB9B689B453FF639AF09EB4B13D3F824 +:102D1000002902F44022B2F5802F24D1D3F80029F3 +:102D2000002A20DAD3F8002942F09042C3F80029A3 +:102D3000D3F80029002AFBDB5946D4F89000FFF7AE +:102D4000C7FB22890AFA0BF322EA0303238104EB6F +:102D50008B03DB689B6813B1594620469847594658 +:102D60002046FFF779FB0137C7E7910701D1D0F87B +:102D70000080072A02F101029CBF03F8018B4FEA91 +:102D800018283DE704EB830300F58050DA68D2F899 +:102D900018C0DCF80820DCE9001CA1EB0C0C0021B9 +:102DA0008F4208D1DB689B699A683A449A605A68F6 +:102DB0003A445A6027E711F0030F01D1D0F80080A0 +:102DC0008C4501F1010184BF02F8018B4FEA1828FC +:102DD000E6E7BDE8F88F000008B50348FFF788FE76 +:102DE000BDE80840FFF784BAAC40002008B50348AE +:102DF000FFF77EFEBDE80840FFF77ABA48410020A1 +:102E0000D0F8903003EB4111D1F8003B43F40013AC +:102E1000C1F8003B70470000D0F8903003EB41113F +:102E2000D1F8003943F40013C1F8003970470000AD +:102E3000D0F8903003EB4111D1F8003B23F400139C +:102E4000C1F8003B70470000D0F8903003EB41110F +:102E5000D1F8003923F40013C1F80039704700009D +:102E6000090100F16043012203F56143C9B283F80F +:102E7000001300F01F039A4043099B0003F16043D5 +:102E800003F56143C3F880211A60704730B50433FD +:102E9000039C0172002104FB0325C160C0E90653B5 +:102EA000049B0363059BC0E90000C0E90422C0E95C +:102EB0000842C0E90A11436330BD00000022416AA4 +:102EC000C260C0E90411C0E90A226FF00101FEF7F7 +:102ED000F7BD0000D0E90432934201D1C2680AB9BB +:102EE000181D704700207047036919600021C268EF +:102EF0000132C260C269134482699342036124BFF4 +:102F0000436A0361FEF7D0BD38B504460D46E36859 +:102F10003BB162690020131D1268A3621344E3628F +:102F200007E0237A33B929462046FEF7ADFD002895 +:102F3000EDDA38BD6FF00100FBE70000C368C2693D +:102F4000013BC3604369134482699342436124BFD8 +:102F5000436A436100238362036B03B118477047E0 +:102F600070B52023044683F31188866A3EB9FFF7C3 +:102F7000CBFF054618B186F31188284670BDA36AB9 +:102F8000E26A13F8015B9342A36202D32046FFF783 +:102F9000D5FF002383F31188EFE700002DE9F84FF8 +:102FA00004460E46174698464FF0200989F31188CB +:102FB0000025AA46D4F828B0BBF1000F09D141463C +:102FC0002046FFF7A1FF20B18BF311882846BDE80A +:102FD000F88FD4E90A12A7EB050B521A934528BFC4 +:102FE0009346BBF1400F1BD9334601F1400251F823 +:102FF000040B914243F8040BF9D1A36A40364035E3 +:103000004033A362D4E90A239A4202D32046FFF751 +:1030100095FF8AF31188BD42D8D289F31188C9E798 +:1030200030465A46FDF7CEFEA36A5E445D445B44DB +:10303000A362E7E710B5029C0433017204FB03218D +:10304000C460C0E906130023C0E90A33039B03638D +:10305000049BC0E90000C0E90422C0E908424363C0 +:1030600010BD0000026A6FF00101C260426AC0E94F +:1030700004220022C0E90A22FEF722BDD0E904237F +:103080009A4201D1C26822B9184650F8043B0B603D +:103090007047002070470000C3680021C2690133F7 +:1030A000C3604369134482699342436124BF436A06 +:1030B0004361FEF7F9BC000038B504460D46E368ED +:1030C0003BB1236900201A1DA262E2691344E36246 +:1030D00007E0237A33B929462046FEF7D5FC0028BD +:1030E000EDDA38BD6FF00100FBE7000003691960FD +:1030F000C268013AC260C2691344826993420361A3 +:1031000024BF436A036100238362036B03B1184742 +:103110007047000070B520230D460446114683F326 +:103120001188866A2EB9FFF7C7FF10B186F31188A0 +:1031300070BDA36A1D70A36AE26A01339342A36261 +:1031400004D3E16920460439FFF7D0FF002080F363 +:103150001188EDE72DE9F84F04460D469046994653 +:103160004FF0200A8AF311880026B346A76A4FB9A8 +:1031700049462046FFF7A0FF20B187F3118830466B +:10318000BDE8F88FD4E90A073A1AA8EB0607974278 +:1031900028BF1746402F1BD905F1400355F8042BD3 +:1031A0009D4240F8042BF9D1A36A40364033A36214 +:1031B000D4E90A239A4204D3E16920460439FFF78F +:1031C00095FF8BF311884645D9D28AF31188CDE754 +:1031D00029463A46FDF7F6FDA36A3D443E443B448A +:1031E000A362E5E7D0E904239A4217D1C3689BB1F3 +:1031F000836A8BB1043B9B1A0ED01360C368013BFA +:10320000C360C3691A4483699A42026124BF436A56 +:103210000361002383620123184670470023FBE704 +:1032200000F0CEB8034B002258631A610222DA6024 +:10323000704700BF000C0040014B0022DA6070476D +:10324000000C0040014B5863704700BF000C004069 +:10325000014B586A704700BF000C00404B68436048 +:103260008B688360CB68C3600B6943614B69036201 +:103270008B6943620B6803607047000008B5364BEA +:1032800040F2FF713548D3F888200A43C3F88820FC +:10329000D3F8882022F4FF6222F00702C3F88820C6 +:1032A000D3F88820D3F8E0200A43C3F8E020D3F80D +:1032B00008210A43C3F80821294AD3F808311146E6 +:1032C000FFF7CCFF00F5806002F11C01FFF7C6FF9D +:1032D00000F5806002F13801FFF7C0FF00F5806063 +:1032E00002F15401FFF7BAFF00F5806002F17001AE +:1032F000FFF7B4FF00F5806002F18C01FFF7AEFF2D +:1033000000F5806002F1A801FFF7A8FF00F58060DA +:1033100002F1C401FFF7A2FF00F5806002F1E001B5 +:10332000FFF79CFF00F5806002F1FC01FFF796FFBC +:1033300002F58C7100F58060FFF790FF00F0F4F863 +:10334000084B0522C3F898204FF06052C3F89C2028 +:10335000054AC3F8A02008BD0044025800000258E6 +:103360003C3B000800ED00E01F00080308B500F03A +:10337000C9FAFEF713FB0F4BD3F8DC2042F04002F2 +:10338000C3F8DC20D3F8042122F04002C3F8042162 +:10339000D3F80431084B1A6842F008021A601A6820 +:1033A00042F004021A60FEF755FFBDE80840FEF740 +:1033B00003BD00BF004402580018024870470000D7 +:1033C000114BD3F8E82042F00802C3F8E820D3F804 +:1033D000102142F00802C3F810210C4AD3F8103132 +:1033E000D36B43F00803D363C722094B9A624FF0B3 +:1033F000FF32DA6200229A615A63DA605A6001226F +:103400005A611A60704700BF004402580010005C07 +:10341000000C0040094A08B51169D3680B40D9B2C5 +:103420009B076FEA0101116107D5202383F31188FF +:10343000FEF7D4FA002383F3118808BD000C004086 +:10344000384B4FF0FF31D3F88020C3F88010D3F809 +:1034500080200022C3F88020D3F88000D3F88400B5 +:10346000C3F88410D3F88400C3F88420D3F8840010 +:10347000D86F40F0FF4040F4FF0040F43F5040F070 +:103480003F00D867D86F20F0FF4020F4FF0020F401 +:103490003F5020F03F00D867D86FD3F888006FEA1C +:1034A00040506FEA5050C3F88800D3F88800C0F34A +:1034B0000A00C3F88800D3F88800D3F89000C3F856 +:1034C0009010D3F89000C3F89020D3F89000D3F870 +:1034D0009400C3F89410D3F89400C3F89420D3F860 +:1034E0009400D3F89800C3F89810D3F89800C3F864 +:1034F0009820D3F89800D3F88C00C3F88C10D3F838 +:103500008C00C3F88C20D3F88C00D3F89C00C3F84F +:103510009C10D3F89C10C3F89C20D3F89C3000F08A +:10352000C9B900BF00440258614B0122C3F8082109 +:10353000604BD3F8F42042F00202C3F8F420D3F831 +:103540001C2142F00202C3F81C210222D3F81C31D4 +:10355000594BDA605A689104FCD5584A1A60012226 +:103560009A60574ADA6000221A614FF440429A6129 +:10357000514B9A699204FCD51A6842F480721A6021 +:103580004C4B1A6F12F4407F04D04FF480321A670C +:1035900000221A671A6842F001021A60454B1A6845 +:1035A0005007FCD500221A611A6912F03802FBD1CB +:1035B000012119604FF0804159605A67414ADA622F +:1035C000414A1A611A6842F480321A60394B1A680B +:1035D0009103FCD51A6842F480521A601A6892046A +:1035E000FCD53A4A3A499A6200225A63196339492A +:1035F000DA6399635A64384A1A64384ADA621A6894 +:1036000042F0A8521A602B4B1A6802F02852B2F10D +:10361000285FF9D148229A614FF48862DA6140222A +:103620001A622F4ADA644FF080521A652D4A5A65A1 +:103630002D4A9A6532232D4A1360136803F00F0355 +:10364000022BFAD11B4B1A6942F003021A611A6964 +:1036500002F03802182AFAD1D3F8DC2042F00052E6 +:10366000C3F8DC20D3F8042142F00052C3F804214F +:10367000D3F80421D3F8DC2042F08042C3F8DC20E8 +:10368000D3F8042142F08042C3F80421D3F8042186 +:10369000D3F8DC2042F00042C3F8DC20D3F8042148 +:1036A00042F00042C3F80421D3F80431704700BF50 +:1036B00000800051004402580048025800C000F049 +:1036C000020000010000FF01008890081210200095 +:1036D000630209012C02040047040508FD0BFF01E9 +:1036E000200000200010E000000101000020005236 +:1036F0004FF0B04208B5D2F8883003F00103C2F8A9 +:10370000883023B1044A13680BB150689847BDE86C +:103710000840FEF7EDBD00BFFC4100204FF0B04275 +:1037200008B5D2F8883003F00203C2F8883023B11C +:10373000044A93680BB1D0689847BDE80840FEF78B +:10374000D7BD00BFFC4100204FF0B04208B5D2F811 +:10375000883003F00403C2F8883023B1044A1369A7 +:103760000BB150699847BDE80840FEF7C1BD00BFE6 +:10377000FC4100204FF0B04208B5D2F8883003F089 +:103780000803C2F8883023B1044A93690BB1D069A9 +:103790009847BDE80840FEF7ABBD00BFFC410020E4 +:1037A0004FF0B04208B5D2F8883003F01003C2F8E9 +:1037B000883023B1044A136A0BB1506A9847BDE8B8 +:1037C0000840FEF795BD00BFFC4100204FF0B0431C +:1037D00010B5D3F8884004F47872C3F88820A306A3 +:1037E00004D5124A936A0BB1D06A9847600604D593 +:1037F0000E4A136B0BB1506B9847210604D50B4A48 +:10380000936B0BB1D06B9847E20504D5074A136C54 +:103810000BB1506C9847A30504D5044A936C0BB1C7 +:10382000D06C9847BDE81040FEF762BDFC41002017 +:103830004FF0B04310B5D3F8884004F47C42C3F88D +:103840008820620504D5164A136D0BB1506D984758 +:10385000230504D5124A936D0BB1D06D9847E0044F +:1038600004D50F4A136E0BB1506E9847A10404D5CE +:103870000B4A936E0BB1D06E9847620404D5084A88 +:10388000136F0BB1506F9847230404D5044A936F0C +:103890000BB1D06F9847BDE81040FEF729BD00BFBF +:1038A000FC41002008B5FFF7B5FDBDE80840FEF774 +:1038B0001FBD0000062108B50846FFF7D1FA062112 +:1038C0000720FFF7CDFA06210820FFF7C9FA0621E5 +:1038D0000920FFF7C5FA06210A20FFF7C1FA0621E1 +:1038E0001720FFF7BDFA06212820FFF7B9FA0921B2 +:1038F0007A20FFF7B5FA07213220BDE80840FFF72C +:10390000AFBA000008B5FFF79BFD00F00BF8FDF71C +:10391000FFFCFDF7D1FBFFF751FDBDE80840FFF7C5 +:103920007FBC00000023054A19460133102BC2E971 +:10393000001102F10802F8D1704700BFFC410020DD +:1039400010B501390244904201D1002005E003780E +:1039500011F8014FA34201D0181B10BD0130F2E74E +:10396000034611F8012B03F8012B002AF9D1704707 +:1039700053544D333248373F3F3F0053544D333259 +:10398000483734332F373533000000000000000083 +:1039900035100008211000085D10000849100008CB +:1039A00055100008411000082D10000819100008DB +:1039B0006910000800000000010000000000000085 +:1039C0006D61696E0000000069646C6500000000B4 +:1039D000C8390008F82300207025002001000000ED +:1039E00099190008000000004172647550696C6FFD +:1039F000740025424F415244252D424C0025534529 +:103A00005249414C25000000020000000000000067 +:103A100055120008C512000840004000183E002062 +:103A2000283E00200200000000000000030000000B +:103A3000000000000D13000800000000100000004E +:103A4000383E0020000000000100000000000000DF +:103A5000AC40002001010200551D0008651C000853 +:103A6000011D0008E51C000843000000703A000832 +:103A700009024300020100C03209040000010202F1 +:103A800001000524001001052401000104240202A4 +:103A90000524060001070582030800FF0904010050 +:103AA000020A00000007050102400000070581022C +:103AB0004000000012000000BC3A00081201100192 +:103AC00002000040091241570002010203010000F8 +:103AD0000403090425424F41524425004B616B7594 +:103AE000746548372D626473686F74003031323307 +:103AF00034353637383941424344454600000000EA +:103B0000000000006114000819170008C51700081C +:103B100040004000E4410020E4410020010000009A +:103B2000F441002080000000400100000800000077 +:103B30000001000000040000080000000001802ACD +:103B400000000000AAAAAAAA00010024FFFF0000AA +:103B50000000000000A00A000000000100000000BA +:103B6000AAAAAAAA00000001FFFF000000000000AE +:103B7000000000001000000400000000AAAAAAAA89 +:103B800000000008FBDF0000000000000000000053 +:103B90000000000000000000AAAAAAAA000000007D +:103BA000FFFF000000000000000000000001000016 +:103BB00000000000AAAAAAAA00010000FFFF00005E +:103BC00000000000000000000000000000000000F5 +:103BD000AAAAAAAA00000000FFFF0000000000003F +:103BE000000000000000000000000000AAAAAAAA2D +:103BF00000000000FFFF00000000000000000000C7 +:103C00000000000000000000AAAAAAAA000000000C +:103C1000FFFF0000000000000000000000000000A6 +:103C200000000000AAAAAAAA00000000FFFF0000EE +:103C30000000000000000000000000000000000084 +:103C4000AAAAAAAA00000000FFFF000000000000CE +:103C5000000000000000000000000000AAAAAAAABC +:103C600000000000FFFF0000000000000000000056 +:103C7000180400000000000000001E00000000000A +:103C8000FF00000000000000703900083F00000045 +:103C9000500400007B3900083F000000009600003F +:103CA000000008009600000000080000040000006A +:103CB000D03A0008000000000000000000000000F2 +:0C3CC000000000000000000000000000F8 +:00000001FF From 6c74c5d4da8e701154941a89b5aa687a891ff2a9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 11 Mar 2022 15:00:41 +0000 Subject: [PATCH 0271/3101] scripts: add KakuteH7 and KakuteH7-bdshot. --- Tools/scripts/generate_manifest.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/scripts/generate_manifest.py b/Tools/scripts/generate_manifest.py index 4733e2c367a..64f8406ba2e 100755 --- a/Tools/scripts/generate_manifest.py +++ b/Tools/scripts/generate_manifest.py @@ -31,6 +31,8 @@ 'KakuteF7Mini' : ('KakuteF7Mini', 'Holybro'), 'KakuteF4Mini' : ('KakuteF4Mini', 'Holybro'), 'KakuteH7Mini' : ('KakuteH7Mini', 'Holybro'), + 'KakuteH7' : ('KakuteH7', 'Holybro'), + 'KakuteH7-bdshot' : ('KakuteH7', 'Holybro'), 'CubeBlack' : ('CubeBlack', 'Hex/ProfiCNC'), 'CubeYellow' : ('CubeYellow', 'Hex/ProfiCNC'), 'CubeOrange' : ('CubeOrange', 'Hex/ProfiCNC'), From 7d95b1ddd367c593ad56a103d6b8db8433d7a15e Mon Sep 17 00:00:00 2001 From: Jaaaky <43599380+Jaaaky@users.noreply.github.com> Date: Sun, 6 Mar 2022 20:04:30 +0300 Subject: [PATCH 0272/3101] AP_FlashIface: Fixed could be used unintialized --- libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp index 74471879287..193b1cd78ac 100644 --- a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp +++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp @@ -668,7 +668,7 @@ bool AP_FlashIface_JEDEC::start_sector_erase(uint32_t sector, uint32_t &delay_ms bool AP_FlashIface_JEDEC::start_erase_offset(uint32_t offset, uint32_t size, uint32_t &erasing, uint32_t &delay_ms, uint32_t &timeout_ms) { - uint8_t ins; + uint8_t ins = 0; uint32_t erase_size = 0; erasing = 0; // Find the maximum size we can erase From a19fa24ccd66fcad143a9b26ad7afdb327af2d86 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 12 Mar 2022 23:21:55 +1100 Subject: [PATCH 0273/3101] Tools: test_build_options.py: add bin mappings for all vehicles --- Tools/autotest/test_build_options.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 0bd08fa7787..b130ba3e52a 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -88,6 +88,11 @@ def find_build_sizes(self): ret = {} target_to_binpath = { "copter": "arducopter", + "plane": "arduplane", + "rover": "ardurover", + "antennatracker": "antennatracker", + "sub": "ardusub", + "blimp": "blimp", } for target in self.build_targets(): path = os.path.join("build", self.board(), "bin", "%s.bin" % target_to_binpath[target]) From a1856c5c22d9d7a4fc36db64c93f872a23aa8c17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 06:48:06 +1100 Subject: [PATCH 0274/3101] Plane: added an arming check for Q_ASSIST_SPEED Q_ASSIST should really be enabled for all non-tailsitter quadplanes. This arming check will help users remember to configure it --- ArduPlane/AP_Arming.cpp | 10 ++++++++++ ArduPlane/quadplane.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index bb15c5498c4..60d5f7f3b3f 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -174,6 +174,16 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure) } } + /* + Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters + */ + if (check_enabled(ARMING_CHECK_PARAMETERS) && + is_zero(plane.quadplane.assist_speed) && + !plane.quadplane.tailsitter.enabled()) { + check_failed(display_failure,"Q_ASSIST_SPEED is not set"); + ret = false; + } + return ret; } #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 43582fe7777..3634c2633fb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -103,7 +103,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: ASSIST_SPEED // @DisplayName: Quadplane assistance speed - // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition + // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition. Note that if this is set to zero then other Q_ASSIST features are also disabled. A higher value will lead to more false positives which can waste battery. A lower value will result in less false positive, but will result in assistance taking longer to trigger. If unsure then set to 3 m/s below the minimum airspeed you will fly at. If you don't have an airspeed sensor then use 5 m/s below the minimum airspeed you fly at. If you want to disable the arming check Q_ASSIST_SPEED then set to -1. // @Units: m/s // @Range: 0 100 // @Increment: 0.1 From a79ad5489c307020ba1c0c15959c3a4880778986 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 15 Mar 2022 20:07:13 +0900 Subject: [PATCH 0275/3101] Rover: balance bot stands in acro with no position estimate --- Rover/mode_acro.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Rover/mode_acro.cpp b/Rover/mode_acro.cpp index 7e1e5894022..5379be5fe45 100644 --- a/Rover/mode_acro.cpp +++ b/Rover/mode_acro.cpp @@ -9,6 +9,12 @@ void ModeAcro::update() float desired_throttle; // convert pilot stick input into desired steering and throttle get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); + + // if vehicle is balance bot, calculate actual throttle required for balancing + if (rover.is_balancebot()) { + rover.balancebot_pitch_control(desired_throttle); + } + // no valid speed, just use the provided throttle g2.motors.set_throttle(desired_throttle); } else { From 21ec47f1845a1813c5635ac111a50d8af6b9d746 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Wed, 9 Mar 2022 23:55:57 -0800 Subject: [PATCH 0276/3101] Plane: Display Fence Breach message in GCS Send a "Geofence breach" message to the Ground Control Station. Without this when the fence is breached and if anything happens as a result, such as RTL, it will happen silently. --- ArduPlane/fence.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index df2b69c09c4..d22a50bb751 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -50,6 +50,10 @@ void Plane::fence_check() // user disables/re-enables using the fence channel switch return; } + + if(new_breaches && plane.is_flying()) { + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached"); + } if (new_breaches || orig_breaches) { // if the user wants some kind of response and motors are armed From 7d23d3763098e6272a98bc485e31d5fa644b21e2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 16 Mar 2022 10:24:27 +1100 Subject: [PATCH 0277/3101] Tracker: fix sim_vehicle.py with --tracker option --- AntennaTracker/system.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index e888eef494f..b1b4223cc1a 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -27,6 +27,10 @@ void Tracker::init_ardupilot() // setup telem slots with serial ports gcs().setup_uarts(); + // update_send so that if the first packet we receive happens to + // be an arm message we don't trigger an internal error when we + // try to initialise stream rates in the main loop. + gcs().update_send(); #if LOGGING_ENABLED == ENABLED log_init(); From 08a1ad3c39f694dfa1374efec7d66e60e0b1da7b Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:29:59 +0900 Subject: [PATCH 0278/3101] Copter: Change from division to multiplication --- ArduCopter/Log.cpp | 2 +- ArduCopter/compassmot.cpp | 4 ++-- ArduCopter/esc_calibration.cpp | 2 +- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_turtle.cpp | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index a3a6cdb74f3..71b2341e70c 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -35,7 +35,7 @@ void Copter::Log_Write_Control_Tuning() float des_alt_m = 0.0f; int16_t target_climb_rate_cms = 0; if (!flightmode->has_manual_throttle()) { - des_alt_m = pos_control->get_pos_target_z_cm() / 100.0f; + des_alt_m = pos_control->get_pos_target_z_cm() * 0.01f; target_climb_rate_cms = pos_control->get_vel_target_z_cms(); } diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 81a24c3d810..ab722e4ca30 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -147,7 +147,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) // pass through throttle to motors SRV_Channels::cork(); - motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); + motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); SRV_Channels::push(); // read some compass values @@ -157,7 +157,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) battery.read(); // calculate scaling for throttle - throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f; + throttle_pct = (float)channel_throttle->get_control_in() * 0.001f; throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); // record maximum throttle diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 1457502252b..b4c5c119063 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -96,7 +96,7 @@ void Copter::esc_calibration_passthrough() // pass through to motors SRV_Channels::cork(); - motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f); + motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); SRV_Channels::push(); } #endif // FRAME_CONFIG != HELI_FRAME diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f475e7ffc88..1b31f7b54cc 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -713,7 +713,7 @@ void Mode::land_run_horizontal_control() // interpolate for 1m above that const float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); - const float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; + const float thrust_vector_max = sinf(radians(attitude_limit_cd * 0.01f)) * GRAVITY_MSS * 100.0f; const float thrust_vector_mag = thrust_vector.xy().length(); if (thrust_vector_mag > thrust_vector_max) { float ratio = thrust_vector_max / thrust_vector_mag; diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 8551a7d5edc..cb4c9fde1e9 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -179,7 +179,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) for (uint8_t i=0; i<2; i++) { float &velocity = sensor_flow[i]; float abs_vel_cms = fabsf(velocity)*100; - const float brake_gain = (15.0f * brake_rate_dps.get() + 95.0f) / 100.0f; + const float brake_gain = (15.0f * brake_rate_dps.get() + 95.0f) * 0.01f; float lean_angle_cd = brake_gain * abs_vel_cms * (1.0f+500.0f/(abs_vel_cms+60.0f)); if (velocity < 0) { lean_angle_cd = -lean_angle_cd; diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 4862c12295f..484dbf2f395 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -41,7 +41,7 @@ bool ModePosHold::init(bool ignore_checks) pilot_pitch = 0.0f; // compute brake_gain - brake.gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) / 100.0f; + brake.gain = (15.0f * (float)g.poshold_brake_rate + 95.0f) * 0.01f; if (copter.ap.land_complete) { // if landed begin in loiter mode diff --git a/ArduCopter/mode_turtle.cpp b/ArduCopter/mode_turtle.cpp index 552bcad266e..72a431c0f73 100644 --- a/ArduCopter/mode_turtle.cpp +++ b/ArduCopter/mode_turtle.cpp @@ -85,7 +85,7 @@ void ModeTurtle::change_motor_direction(bool reverse) void ModeTurtle::run() { - const float flip_power_factor = 1.0f - CRASH_FLIP_EXPO / 100.0f; + const float flip_power_factor = 1.0f - CRASH_FLIP_EXPO * 0.01f; const bool norc = copter.failsafe.radio || !copter.ap.rc_receiver_present; const float stick_deflection_pitch = norc ? 0.0f : channel_pitch->norm_input_dz(); const float stick_deflection_roll = norc ? 0.0f : channel_roll->norm_input_dz(); From 12957c835a45cf5ef8b18b78863951fcfa6f5f75 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:30:57 +0900 Subject: [PATCH 0279/3101] AC_InputManager: Change from division to multiplication --- .../AC_InputManager/AC_InputManager_Heli.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AC_InputManager/AC_InputManager_Heli.cpp b/libraries/AC_InputManager/AC_InputManager_Heli.cpp index fc46fbe0a31..10461b46ecb 100644 --- a/libraries/AC_InputManager/AC_InputManager_Heli.cpp +++ b/libraries/AC_InputManager/AC_InputManager_Heli.cpp @@ -67,20 +67,20 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) // calculate stabilize collective value which scales pilot input to reduced collective range // code implements a 3-segment curve with knee points at 40% and 60% throttle input if (control_in < 400){ // control_in ranges from 0 to 1000 - slope_low = _heli_stab_col_min / 100.0f; - slope_high = _heli_stab_col_low / 100.0f; + slope_low = _heli_stab_col_min * 0.01f; + slope_high = _heli_stab_col_low * 0.01f; slope_range = 0.4f; - slope_run = control_in / 1000.0f; + slope_run = control_in * 0.001f; } else if(control_in <600){ // control_in ranges from 0 to 1000 - slope_low = _heli_stab_col_low / 100.0f; - slope_high = _heli_stab_col_high / 100.0f; + slope_low = _heli_stab_col_low * 0.01f; + slope_high = _heli_stab_col_high * 0.01f; slope_range = 0.2f; - slope_run = (control_in - 400) / 1000.0f; // control_in ranges from 0 to 1000 + slope_run = (control_in - 400) * 0.001f; // control_in ranges from 0 to 1000 } else { - slope_low = _heli_stab_col_high / 100.0f; - slope_high = _heli_stab_col_max / 100.0f; + slope_low = _heli_stab_col_high * 0.01f; + slope_high = _heli_stab_col_max * 0.01f; slope_range = 0.4f; - slope_run = (control_in - 600) / 1000.0f; // control_in ranges from 0 to 1000 + slope_run = (control_in - 600) * 0.001f; // control_in ranges from 0 to 1000 } scalar = (slope_high - slope_low)/slope_range; @@ -95,7 +95,7 @@ float AC_InputManager_Heli::get_pilot_desired_collective(int16_t control_in) } if (_acro_col_expo <= 0.0f) { - acro_col_out = control_in / 1000.0f; // control_in ranges from 0 to 1000 + acro_col_out = control_in * 0.001f; // control_in ranges from 0 to 1000 } else { // expo variables float col_in, col_in3, col_out; From aabbda3978e1d5bee172bdacf3d8a645a38cb354 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:31:18 +0900 Subject: [PATCH 0280/3101] AP_Baro: Change from division to multiplication --- libraries/AP_Baro/AP_Baro_BMP280.cpp | 2 +- libraries/AP_Baro/AP_Baro_SITL.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index d6823ee2fb4..a08e7af7ded 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -162,7 +162,7 @@ void AP_Baro_BMP280::_update_temperature(int32_t temp_raw) _t_fine = var1 + var2; t = (_t_fine * 5 + 128) >> 8; - const float temp = ((float)t) / 100.0f; + const float temp = ((float)t) * 0.01f; WITH_SEMAPHORE(_sem); diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index f160cf8affe..ec05c90aa99 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -66,7 +66,7 @@ void AP_Baro_SITL::_timer() return; } - sim_alt += _sitl->baro[_instance].drift * now / 1000.0f; + sim_alt += _sitl->baro[_instance].drift * now * 0.001f; sim_alt += _sitl->baro[_instance].noise * rand_float(); // add baro glitch From 9d321c5c31f6ba586dfc767e4d19f6846294bba8 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:31:56 +0900 Subject: [PATCH 0281/3101] AP_BattMonitor: Change from division to multiplication --- libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp | 4 ++-- libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp | 4 ++-- libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index edce254eb16..c203cc88982 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -75,7 +75,7 @@ void AP_BattMonitor_Backend::update_resistance_estimate() // calculate time since last update uint32_t now = AP_HAL::millis(); - float loop_interval = (now - _resistance_timer_ms) / 1000.0f; + float loop_interval = (now - _resistance_timer_ms) * 0.001f; _resistance_timer_ms = now; // estimate short-term resistance diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp index c43e41671ec..eb12e427126 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp @@ -177,7 +177,7 @@ void AP_BattMonitor_Bebop::read(void) } /* get battery voltage observed by cypress */ - vbat_raw = (float)data.batt_mv / 1000.0f; + vbat_raw = (float)data.batt_mv * 0.001f; /* do not compute battery status on ramping or braking transition */ if (data.status == BEBOP_BLDC_STATUS_RAMPING || @@ -214,7 +214,7 @@ void AP_BattMonitor_Bebop::read(void) _state.voltage = vbat; _state.last_time_micros = tnow; _state.healthy = true; - _state.consumed_mah = capacity - (remaining * capacity) / 100.0f; + _state.consumed_mah = capacity - (remaining * capacity) * 0.01f; } #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp index 0e59af57d0e..ac23bc44c8e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.cpp @@ -60,7 +60,7 @@ void AP_BattMonitor_SMBus_Generic::timer() // read voltage (V) if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) { - _state.voltage = (float)data / 1000.0f; + _state.voltage = (float)data * 0.001f; _state.last_time_micros = tnow; _state.healthy = true; } @@ -113,7 +113,7 @@ void AP_BattMonitor_SMBus_Generic::timer() // read current (A) if (read_word(BATTMONITOR_SMBUS_CURRENT, data)) { - _state.current_amps = -(float)((int16_t)data) / 1000.0f; + _state.current_amps = -(float)((int16_t)data) * 0.001f; _state.last_time_micros = tnow; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index 4520d1430e9..0528d70ca60 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -66,7 +66,7 @@ void AP_BattMonitor_SMBus_Solo::timer() // read current if (read_block(BATTMONITOR_SMBUS_SOLO_CURRENT, buff, 4) == 4) { - _state.current_amps = -(float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + _state.current_amps = -(float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) * 0.001f; _state.last_time_micros = tnow; } From 12b3b22dbcee1d3b20b044822551af00b13bb348 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:32:15 +0900 Subject: [PATCH 0282/3101] AP_Beacon: Change from division to multiplication --- libraries/AP_Beacon/AP_Beacon_Pozyx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp b/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp index 7b5e3b11028..9880d783555 100644 --- a/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp +++ b/libraries/AP_Beacon/AP_Beacon_Pozyx.cpp @@ -113,7 +113,7 @@ void AP_Beacon_Pozyx::parse_buffer() int32_t beacon_x = (uint32_t)linebuf[5] << 24 | (uint32_t)linebuf[4] << 16 | (uint32_t)linebuf[3] << 8 | (uint32_t)linebuf[2]; int32_t beacon_y = (uint32_t)linebuf[9] << 24 | (uint32_t)linebuf[8] << 16 | (uint32_t)linebuf[7] << 8 | (uint32_t)linebuf[6]; int32_t beacon_z = (uint32_t)linebuf[13] << 24 | (uint32_t)linebuf[12] << 16 | (uint32_t)linebuf[11] << 8 | (uint32_t)linebuf[10]; - Vector3f beacon_pos(beacon_x / 1000.0f, beacon_y / 1000.0f, -beacon_z / 1000.0f); + Vector3f beacon_pos(beacon_x * 0.001f, beacon_y * 0.001f, -beacon_z * 0.001f); if (beacon_pos.length() <= AP_BEACON_DISTANCE_MAX) { set_beacon_position(beacon_id, beacon_pos); parsed = true; @@ -139,7 +139,7 @@ void AP_Beacon_Pozyx::parse_buffer() int32_t vehicle_y = (uint32_t)linebuf[7] << 24 | (uint32_t)linebuf[6] << 16 | (uint32_t)linebuf[5] << 8 | (uint32_t)linebuf[4]; int32_t vehicle_z = (uint32_t)linebuf[11] << 24 | (uint32_t)linebuf[10] << 16 | (uint32_t)linebuf[9] << 8 | (uint32_t)linebuf[8]; int16_t position_error = (uint32_t)linebuf[13] << 8 | (uint32_t)linebuf[12]; - Vector3f veh_pos(Vector3f(vehicle_x / 1000.0f, vehicle_y / 1000.0f, -vehicle_z / 1000.0f)); + Vector3f veh_pos(Vector3f(vehicle_x * 0.001f, vehicle_y * 0.001f, -vehicle_z * 0.001f)); if (veh_pos.length() <= AP_BEACON_DISTANCE_MAX) { set_vehicle_position(veh_pos, position_error); parsed = true; From be89285d10bb2a66c7b6889c05aa7dba184e0b4b Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:32:42 +0900 Subject: [PATCH 0283/3101] AP_Generator: Change from division to multiplication --- libraries/AP_Generator/AP_Generator_RichenPower.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.cpp b/libraries/AP_Generator/AP_Generator_RichenPower.cpp index 66b1ec073d6..210bcb58f62 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.cpp +++ b/libraries/AP_Generator/AP_Generator_RichenPower.cpp @@ -129,8 +129,8 @@ bool AP_Generator_RichenPower::get_reading() last_reading.seconds_until_maintenance = be16toh(u.packet.seconds_until_maintenance_high) * 65536 + be16toh(u.packet.seconds_until_maintenance_low); last_reading.errors = be16toh(u.packet.errors); last_reading.rpm = be16toh(u.packet.rpm); - last_reading.output_voltage = be16toh(u.packet.output_voltage) / 100.0f; - last_reading.output_current = be16toh(u.packet.output_current) / 100.0f; + last_reading.output_voltage = be16toh(u.packet.output_voltage) * 0.01f; + last_reading.output_current = be16toh(u.packet.output_current) * 0.01f; last_reading.mode = (Mode)u.packet.mode; last_reading_ms = AP_HAL::millis(); From c90e42d10ad151ddedc9741834ca996c89a5210f Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:33:10 +0900 Subject: [PATCH 0284/3101] AP_GyroFFT: Change from division to multiplication --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 82ac4fd49c6..eb3272f021a 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -475,7 +475,7 @@ void AP_GyroFFT::update_parameters() // determine the endt FFT bin for all frequency detection _config._fft_end_bin = MIN(ceilf(_fft_max_hz.get() / _state->_bin_resolution), _state->_bin_count); // actual attenuation from the db value - _config._attenuation_cutoff = powf(10.0f, -_attenuation_power_db / 10.0f); + _config._attenuation_cutoff = powf(10.0f, -_attenuation_power_db * 0.1f); } // thread for processing gyro data via FFT @@ -574,7 +574,7 @@ bool AP_GyroFFT::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) if (_calibrated) { // provide the user with some useful information about what they have configured - gcs().send_text(MAV_SEVERITY_INFO, "FFT: calibrated %.1fKHz/%.1fHz/%.1fHz", _fft_sampling_rate_hz / 1000.0f, + gcs().send_text(MAV_SEVERITY_INFO, "FFT: calibrated %.1fKHz/%.1fHz/%.1fHz", _fft_sampling_rate_hz * 0.001f, _state->_bin_resolution * 0.5, 1000.0f * XYZ_AXIS_COUNT / _frame_time_ms); } From efe8427279ef404de7c62560dc8e4c0bbb4ffd10 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:33:46 +0900 Subject: [PATCH 0285/3101] AP_InertialSensor: Change from division to multiplication --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 2 +- libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index afeace96608..a811bb845d7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -417,7 +417,7 @@ void AP_InertialSensor_Invensense::start() bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) { if (_fast_sampling) { snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", - _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate / 1000.0, _gyro_backend_rate_hz / 1000.0); + _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); return true; } return false; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index 28bbc2aa9f3..f5da4c7e077 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -244,7 +244,7 @@ void AP_InertialSensor_Invensensev2::start() bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t banner_len) { if (_fast_sampling) { snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz", - _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate / 1000.0, _gyro_backend_rate_hz / 1000.0); + _gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate * 0.001, _gyro_backend_rate_hz * 0.001); return true; } return false; From 6d4a923cce38f730ab3dd43c26d86997ace49424 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:35:05 +0900 Subject: [PATCH 0286/3101] AP_Motors: Change from division to multiplication --- libraries/AP_Motors/AP_MotorsCoax.cpp | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsSingle.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 2141858c3e0..f79b4f4c9a5 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -153,7 +153,7 @@ void AP_MotorsCoax::output_armed_stabilizing() if (is_zero(rp_thrust_max)) { rp_scale = 1.0f; } else { - rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f * (float)_yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f); + rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f * (float)_yaw_headroom * 0.001f)) / rp_thrust_max, 0.0f, 1.0f); if (rp_scale < 1.0f) { limit.roll = true; limit.pitch = true; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 9016eeef0a9..3b647570ae9 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -295,7 +295,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() // calculate the maximum yaw control that can be used // todo: make _yaw_headroom 0 to 1 - float yaw_allowed_min = (float)_yaw_headroom / 1000.0f; + float yaw_allowed_min = (float)_yaw_headroom * 0.001f; // increase yaw headroom to 50% if thrust boost enabled yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 74c000ec6f6..30801e54d5d 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -162,7 +162,7 @@ void AP_MotorsSingle::output_armed_stabilizing() if (is_zero(rp_thrust_max)) { rp_scale = 1.0f; } else { - rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float) _yaw_headroom / 1000.0f)) / rp_thrust_max, 0.0f, 1.0f); + rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float) _yaw_headroom * 0.001f)) / rp_thrust_max, 0.0f, 1.0f); if (rp_scale < 1.0f) { limit.roll = true; limit.pitch = true; From e977f856475b1598ac0d5e4b49284768190d5e0c Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:35:26 +0900 Subject: [PATCH 0287/3101] AP_OSD: Change from division to multiplication --- libraries/AP_OSD/AP_OSD_ParamSetting.cpp | 4 ++-- libraries/AP_OSD/AP_OSD_Screen.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp index ff952ed88aa..f84731c26d8 100644 --- a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp @@ -380,7 +380,7 @@ void AP_OSD_ParamSetting::guess_ranges(bool force) float floatp = p->get(); if (digits < 1) { if (!is_zero(floatp)) { - incr = floatp / 100.0f; // move in 1% increments + incr = floatp * 0.01f; // move in 1% increments } else { incr = 0.01f; // move in absolute 1% increments } @@ -388,7 +388,7 @@ void AP_OSD_ParamSetting::guess_ranges(bool force) min = 0.0f; } else { if (!is_zero(floatp)) { - incr = floatp / 100.0f; // move in 1% increments + incr = floatp * 0.01f; // move in 1% increments } else { incr = MAX(1, powf(10, digits - 2)); } diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index c9626d01f5f..871ce6498cf 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1862,7 +1862,7 @@ void AP_OSD_Screen::draw_temp(uint8_t x, uint8_t y) void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y) { AP_GPS & gps = AP::gps(); - float hdp = gps.get_hdop() / 100.0f; + float hdp = gps.get_hdop() * 0.01f; backend->write(x, y, false, "%c%c%3.2f", SYMBOL(SYM_HDOP_L), SYMBOL(SYM_HDOP_R), (double)hdp); } From 7897807a782f446ccbe4253bbc7b130849ed3652 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:35:51 +0900 Subject: [PATCH 0288/3101] AP_Winch: Change from division to multiplication --- libraries/AP_Winch/AP_Winch_Daiwa.cpp | 2 +- libraries/AP_Winch/AP_Winch_PWM.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index d660d15e384..a213b389076 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -184,7 +184,7 @@ void AP_Winch_Daiwa::read_data_from_winch() void AP_Winch_Daiwa::control_winch() { const uint32_t now_ms = AP_HAL::millis(); - float dt = (now_ms - control_update_ms) / 1000.0f; + float dt = (now_ms - control_update_ms) * 0.001f; if (dt > 1.0f) { dt = 0.0f; } diff --git a/libraries/AP_Winch/AP_Winch_PWM.cpp b/libraries/AP_Winch/AP_Winch_PWM.cpp index f7782559265..7a26f931de7 100644 --- a/libraries/AP_Winch/AP_Winch_PWM.cpp +++ b/libraries/AP_Winch/AP_Winch_PWM.cpp @@ -32,7 +32,7 @@ void AP_Winch_PWM::update() void AP_Winch_PWM::control_winch() { const uint32_t now_ms = AP_HAL::millis(); - float dt = (now_ms - control_update_ms) / 1000.0f; + float dt = (now_ms - control_update_ms) * 0.001f; if (dt > 1.0f) { dt = 0.0f; } From ae896ff8c259132bcbd14b6e395f43b09ba22d75 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:36:20 +0900 Subject: [PATCH 0289/3101] AP_LTM_Telem: Change from division to multiplication --- libraries/AP_LTM_Telem/AP_LTM_Telem.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp index c9267b19851..66e9554cb15 100644 --- a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp +++ b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp @@ -168,9 +168,9 @@ void AP_LTM_Telem::send_Aframe(void) { AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); - pitch = roundf(ahrs.pitch_sensor / 100.0); // attitude pitch in degrees - roll = roundf(ahrs.roll_sensor / 100.0); // attitude roll in degrees - heading = roundf(ahrs.yaw_sensor / 100.0); // heading in degrees + pitch = roundf(ahrs.pitch_sensor * 0.01); // attitude pitch in degrees + roll = roundf(ahrs.roll_sensor * 0.01); // attitude roll in degrees + heading = roundf(ahrs.yaw_sensor * 0.01); // heading in degrees } uint8_t lt_buff[LTM_AFRAME_SIZE]; From db7fe2ba4efb66bcf34cbd2dd4225a4851eea6db Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:36:41 +0900 Subject: [PATCH 0290/3101] AP_Mission: Change from division to multiplication --- libraries/AP_Mission/AP_Mission.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index dd9c399193e..1b7c94b2b60 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1648,7 +1648,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.x = cmd.content.location.lat; packet.y = cmd.content.location.lng; - packet.z = cmd.content.location.alt / 100.0f; // cmd alt in cm to m + packet.z = cmd.content.location.alt * 0.01f; // cmd alt in cm to m if (cmd.content.location.relative_alt) { packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } else { From 4eca7383b8d9e626ad425f35cf40c8cd78a49b66 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:36:59 +0900 Subject: [PATCH 0291/3101] AP_Mount: Change from division to multiplication --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 0011b5f9123..8b34a05626b 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -212,7 +212,7 @@ void AP_Mount_Alexmos::parse_body() switch (_command_id ) { case CMD_BOARD_INFO: _board_version = _buffer.version._board_version/ 10; - _current_firmware_version = _buffer.version._firmware_version / 1000.0f ; + _current_firmware_version = _buffer.version._firmware_version * 0.001f ; _firmware_beta_version = _buffer.version._firmware_version % 10 ; _gimbal_3axis = (_buffer.version._board_features & 0x1); _gimbal_bat_monitoring = (_buffer.version._board_features & 0x2); From af3fbac118ddc63f1ab0f338bff7237a2b9f2001 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:37:37 +0900 Subject: [PATCH 0292/3101] AP_PiccoloCAN: Change from division to multiplication --- libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 1881bc46193..e834195865d 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -337,7 +337,7 @@ void AP_PiccoloCAN::update() timestamp, ii, (float) servo.statusA.position, // Servo position (represented in microsecond units) - (float) servo.statusB.current / 100.0f, // Servo force (actually servo current, 0.01A per bit) + (float) servo.statusB.current * 0.01f, // Servo force (actually servo current, 0.01A per bit) (float) servo.statusB.speed, // Servo speed (degrees per second) (uint8_t) abs(servo.statusB.dutyCycle) // Servo duty cycle (absolute value as it can be +/- 100%) ); From a1a46b54e9e116a5920a36520120ce562fd4be62 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:38:04 +0900 Subject: [PATCH 0293/3101] AP_Soaring: Change from division to multiplication --- libraries/AP_Soaring/Variometer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index 28c19fc01f7..408652d2201 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -21,7 +21,7 @@ void Variometer::update(const float thermal_bank) float aspd = 0; if (!_ahrs.airspeed_estimate(aspd)) { - aspd = _aparm.airspeed_cruise_cm / 100.0f; + aspd = _aparm.airspeed_cruise_cm * 0.01f; } float aspd_filt = _sp_filter.apply(aspd); From cad63eb6b6c8b9143ea56d4f5b8a60cd2b8530c9 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:38:34 +0900 Subject: [PATCH 0294/3101] AP_Torqeedo: Change from division to multiplication --- libraries/AP_Torqeedo/AP_Torqeedo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp index 19da31067a4..a7186cd4c72 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp +++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp @@ -1031,7 +1031,7 @@ int16_t AP_Torqeedo::calc_motor_speed_limited(int16_t desired_motor_speed) } // calculate dt since last update - float dt = (now_ms - _motor_speed_limited_ms) / 1000.0f; + float dt = (now_ms - _motor_speed_limited_ms) * 0.001f; if (dt > 1.0) { // after a long delay limit motor output to zero to avoid sudden starts lower_limit = 0; From 5d2c49e2020233f11db0561618ef8b713a4da1e3 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:38:56 +0900 Subject: [PATCH 0295/3101] AP_VideoTX: Change from division to multiplication --- libraries/AP_VideoTX/AP_VideoTX.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_VideoTX/AP_VideoTX.cpp b/libraries/AP_VideoTX/AP_VideoTX.cpp index dd2370af7ca..ea786e6d85c 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.cpp +++ b/libraries/AP_VideoTX/AP_VideoTX.cpp @@ -161,7 +161,7 @@ void AP_VideoTX::set_power_dbm(uint8_t power) { _current_power = 800; break; default: - _current_power = uint16_t(roundf(powf(10, power / 10.0f))); + _current_power = uint16_t(roundf(powf(10, power * 0.1f))); break; } } From 8f91fe9c2e293f40b5b7e2de8462f7a3b8b2b5ce Mon Sep 17 00:00:00 2001 From: murata Date: Tue, 15 Mar 2022 01:59:26 -0400 Subject: [PATCH 0296/3101] AP_VisualOdom: Change from division to multiplication --- libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index a09b15230da..3a01cb7a839 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -56,7 +56,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa _last_update_ms = now_ms; // send to EKF - const float time_delta_sec = packet.time_delta_usec / 1000000.0f; + const float time_delta_sec = packet.time_delta_usec * 1.0E-6; AP::ahrs().writeBodyFrameOdom(packet.confidence, position_delta, angle_delta, From bf738b03a69c4ca42017daf63c5e4bdfac74ce49 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:39:48 +0900 Subject: [PATCH 0297/3101] AP_WheelEncoder: Change from division to multiplication --- libraries/AP_WheelEncoder/AP_WheelRateControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp b/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp index fe2fce23357..732a2e35c2f 100644 --- a/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelRateControl.cpp @@ -203,7 +203,7 @@ float AP_WheelRateControl::get_rate_controlled_throttle(uint8_t instance, float _last_update_ms = now; // convert desired rate as a percentage to radians/sec - float desired_rate = desired_rate_pct / 100.0f * get_rate_max_rads(); + float desired_rate = desired_rate_pct * 0.01f * get_rate_max_rads(); // get actual rate from wheeel encoder float actual_rate = _wheel_encoder.get_rate(instance); From bb72f91dda72fae062a5c75ad593f510e3f9356f Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:40:11 +0900 Subject: [PATCH 0298/3101] APM_Control: Change from division to multiplication --- libraries/APM_Control/AP_YawController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index b72608c9e3e..913f2c62e4a 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -173,7 +173,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) aspd_min = 1; } - float delta_time = (float) dt / 1000.0f; + float delta_time = (float) dt * 0.001f; // Calculate yaw rate required to keep up with a constant height coordinated turn float aspeed; From 82afaf70f66fd94c494a1fe74a9cee22f140580c Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:43:01 +0900 Subject: [PATCH 0299/3101] AR_Motors: Change from division to multiplication --- libraries/AR_Motors/AP_MotorsUGV.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index 4ae276adc7b..98bf28c40a8 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -634,7 +634,7 @@ void AP_MotorsUGV::output_regular(bool armed, float ground_speed, float steering // normalise desired steering and throttle to ease calculations float steering_norm = steering / 4500.0f; - const float throttle_norm = throttle / 100.0f; + const float throttle_norm = throttle * 0.01f; // steering can never be more than throttle * tan(_vector_angle_max) const float vector_angle_max_rad = radians(constrain_float(_vector_angle_max, 0.0f, 90.0f)); @@ -739,7 +739,7 @@ void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float thrott // skid steering mixer float steering_scaled = steering / 4500.0f; // steering scaled -1 to +1 - float throttle_scaled = throttle / 100.0f; // throttle scaled -1 to +1 + float throttle_scaled = throttle * 0.01f; // throttle scaled -1 to +1 // apply constraints steering_scaled = constrain_float(steering_scaled, -1.0f, 1.0f); @@ -800,9 +800,9 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float steering = constrain_float(steering, -4500.0f, 4500.0f); // scale throttle, steering and lateral inputs to -1 to 1 - const float scaled_throttle = throttle / 100.0f; + const float scaled_throttle = throttle * 0.01f; const float scaled_steering = steering / 4500.0f; - const float scaled_lateral = lateral / 100.0f; + const float scaled_lateral = lateral * 0.01f; float thr_str_ltr_out[AP_MOTORS_NUM_MOTORS_MAX]; float thr_str_ltr_max = 1; @@ -956,9 +956,9 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const // scale using throttle_min if (_throttle_min > 0) { if (is_negative(throttle)) { - throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f)); + throttle = -_throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f)); } else { - throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) / 100.0f)); + throttle = _throttle_min + (throttle * ((100.0f - _throttle_min) * 0.01f)); } } @@ -969,7 +969,7 @@ float AP_MotorsUGV::get_scaled_throttle(float throttle) const // calculate scaler const float sign = (throttle < 0.0f) ? -1.0f : 1.0f; - const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) / 100.0f; + const float throttle_pct = constrain_float(throttle, -100.0f, 100.0f) * 0.01f; return 100.0f * sign * ((_thrust_curve_expo - 1.0f) + safe_sqrt((1.0f - _thrust_curve_expo) * (1.0f - _thrust_curve_expo) + 4.0f * _thrust_curve_expo * fabsf(throttle_pct))) / (2.0f * _thrust_curve_expo); } From 8f1369f065a70b89d7540ed885be598aa9939162 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 12 Mar 2022 02:43:16 +0900 Subject: [PATCH 0300/3101] PID: Change from division to multiplication --- libraries/PID/PID.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 141afcd05f8..71079cdbe85 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -52,7 +52,7 @@ float PID::get_pid(float error, float scaler) } _last_t = tnow; - delta_time = (float)dt / 1000.0f; + delta_time = (float)dt * 0.001f; // Compute proportional component _pid_info.P = error * _kp; From fcb0538fef534b1bc5bf2b50a4b751a50c12f382 Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Tue, 15 Mar 2022 02:51:43 -0400 Subject: [PATCH 0301/3101] autotest: common check_mission_items_same loosen z to 1E-3 --- Tools/autotest/common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 8da34d53dc4..532702dd31a 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3965,9 +3965,9 @@ def check_mission_items_same(self, raise NotAchievedException("Frame not same (got=%s want=%s)" % (self.string_for_frame(downloaded_item_val), self.string_for_frame(item_val))) - if abs(item.z - downloaded_item.z) > 0.00001: + if abs(item.z - downloaded_item.z) > 1.0E-3: # error should be less than 1 mm raise NotAchievedException("Z not preserved (got=%f want=%f)" % - (item.z, downloaded_item.z)) + (downloaded_item.z, item.z)) def check_fence_items_same(self, want, got, strict=True): check_atts = ['mission_type', 'command', 'x', 'y', 'seq', 'param1'] From ceef68e07b159a6940298e847fce0b98436f61d2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 23 Feb 2022 20:05:38 +0000 Subject: [PATCH 0302/3101] AP_HAL_ChibiOS: allow H7 480Mhz clock speed to be configured in hwdef via MCU_CLOCKRATE_MHZ --- .../AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat | 2 ++ .../hwdef/common/stm32h7_mcuconf.h | 17 ++++++++++++++--- .../AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py | 2 +- .../hwdef/scripts/chibios_hwdef.py | 6 +++++- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat index 5d317955279..4d654f00696 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat @@ -11,6 +11,8 @@ APJ_BOARD_ID 1060 # crystal frequency, setup to use external oscillator OSCILLATOR_HZ 8000000 +MCU_CLOCKRATE_MHZ 480 + env OPTIMIZE -O2 STM32_ST_USE_TIMER 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 13a295014b6..7b2a749335d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -19,7 +19,7 @@ #pragma once // we want to cope with both revision XY chips and newer chips -#ifndef STM32H750xx +#if !defined(HAL_CUSTOM_MCU_CLOCKRATE) || HAL_CUSTOM_MCU_CLOCKRATE <= 400000000 #define STM32_ENFORCE_H7_REV_XY #endif @@ -161,9 +161,13 @@ #elif (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) // common clock tree for multiples of 8MHz crystals -#ifdef STM32H750xx +#ifdef HAL_CUSTOM_MCU_CLOCKRATE +#if HAL_CUSTOM_MCU_CLOCKRATE == 480000000 #define STM32_PLL1_DIVN_VALUE 120 #else +#error "Unable to configure custom clockrate" +#endif +#else #define STM32_PLL1_DIVN_VALUE 100 #endif #define STM32_PLL1_DIVP_VALUE 2 @@ -181,9 +185,13 @@ #define STM32_PLL3_DIVR_VALUE 9 #elif STM32_HSECLK == 24000000U -#ifdef STM32H750xx +#ifdef HAL_CUSTOM_MCU_CLOCKRATE +#if HAL_CUSTOM_MCU_CLOCKRATE == 480000000 #define STM32_PLL1_DIVN_VALUE 120 #else +#error "Unable to configure custom clockrate" +#endif +#else #define STM32_PLL1_DIVN_VALUE 100 #endif #define STM32_PLL1_DIVP_VALUE 2 @@ -201,6 +209,9 @@ #define STM32_PLL3_DIVR_VALUE 9 #elif STM32_HSECLK == 25000000U +#ifdef HAL_CUSTOM_MCU_CLOCKRATE +#error "Unable to configure custom clockrate" +#endif #define STM32_PLL1_DIVN_VALUE 64 #define STM32_PLL1_DIVP_VALUE 2 #define STM32_PLL1_DIVQ_VALUE 10 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py index e0e43fddcc8..1d7d9ac4334 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py @@ -61,7 +61,7 @@ (0x38000000, 64, 1), # SRAM4. ], - 'EXPECTED_CLOCK' : 480000000, + 'EXPECTED_CLOCK' : 400000000, # this MCU has M7 instructions and hardware double precision 'CORTEX' : 'cortex-m7', diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 22f7b3adb20..fbcd2aaa011 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1037,7 +1037,11 @@ def write_mcu_config(f): #endif ''') - if get_mcu_config('EXPECTED_CLOCK', required=True): + if get_config('MCU_CLOCKRATE_MHZ', required=False): + clockrate = int(get_config('MCU_CLOCKRATE_MHZ')) + f.write('#define HAL_CUSTOM_MCU_CLOCKRATE %u\n' % (clockrate * 1000000)) + f.write('#define HAL_EXPECTED_SYSCLOCK %u\n' % (clockrate * 1000000)) + elif get_mcu_config('EXPECTED_CLOCK', required=True): f.write('#define HAL_EXPECTED_SYSCLOCK %u\n' % get_mcu_config('EXPECTED_CLOCK')) env_vars['CORTEX'] = cortex From e1424d7c3f186dc3caea0a2dd5f3d9ce5b46eda9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 16:40:00 +1100 Subject: [PATCH 0303/3101] DroneCAN: update pydronecan avoid python2 build error of AP_Periph --- modules/DroneCAN/pydronecan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan index 7dd0cbdcd1d..21eab6f065a 160000 --- a/modules/DroneCAN/pydronecan +++ b/modules/DroneCAN/pydronecan @@ -1 +1 @@ -Subproject commit 7dd0cbdcd1d2a6a15cfcb4717e4f25f735173cc9 +Subproject commit 21eab6f065a7fc61672e6324b65a95d6127bb103 From 92a8964503181d3900fef4ff5c728c5ec1ec78b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 20:08:08 +1100 Subject: [PATCH 0304/3101] AP_Arming: display a warning if arming checks disabled when arming --- libraries/AP_Arming/AP_Arming.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index a09f0e4be7f..dd28febee75 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1365,6 +1365,10 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) armed = false; } + if (armed && do_arming_checks && checks_to_perform == 0) { + gcs().send_text(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled"); + } + return armed; } From 4896f8a6d7ba69f2c771d5a7a6bc4123c1c1cc18 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:38:13 +1100 Subject: [PATCH 0305/3101] AP_AHRS: rename SITL backend to SIM --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 1409af24deb..9816e11b3c7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -393,7 +393,7 @@ void AP_AHRS::update(bool skip_ins_update) break; #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - shortname = "SITL"; + shortname = "SIM"; break; #endif #if HAL_EXTERNAL_AHRS_ENABLED From 3cf70915252fde952e946d4aebeeca133a1ecb7e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Oct 2021 16:07:31 +1100 Subject: [PATCH 0306/3101] SITL: make parsing of json files dependent on PICOJSON picojson uses the standard library; this will be a problem on embedded platforms --- libraries/SITL/SIM_Frame.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 45b736fb978..2068a259164 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -23,7 +23,10 @@ #include #include +#define USE_PICOJSON (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#if USE_PICOJSON #include "picojson.h" +#endif using namespace SITL; @@ -322,6 +325,7 @@ float Frame::get_air_density(float alt_amsl) const return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN(model.refTempC))); } +#if USE_PICOJSON /* load frame specific parameters from a json file if available */ @@ -418,6 +422,7 @@ void Frame::load_frame_params(const char *model_json) ::printf("Loaded model params from %s\n", model_json); } +#endif /* initialise the frame @@ -427,11 +432,13 @@ void Frame::init(const char *frame_str, Battery *_battery) model = default_model; battery = _battery; +#if USE_PICOJSON const char *colon = strchr(frame_str, ':'); size_t slen = strlen(frame_str); if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) { load_frame_params(colon+1); } +#endif mass = model.mass; const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle)); From 09571315435a227b39b3658033eeaa185a3c908e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Oct 2021 16:08:40 +1100 Subject: [PATCH 0307/3101] SITL: use AP_HAL::micros() for get_wall_time_us --- libraries/SITL/SIM_Aircraft.cpp | 5 ++++- libraries/SITL/SIM_GPS.cpp | 2 ++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index d99a39a14d4..bcc95e85878 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -536,6 +536,7 @@ float Aircraft::rangefinder_range() const } +// potentially replace this with a call to AP_HAL::Util::get_hw_rtc uint64_t Aircraft::get_wall_time_us() const { #if defined(__CYGWIN__) || defined(__CYGWIN64__) @@ -549,10 +550,12 @@ uint64_t Aircraft::get_wall_time_us() const last_ret_us += (uint64_t)((now - tPrev)*1000UL); tPrev = now; return last_ret_us; -#else +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); return uint64_t(ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL); +#else + return AP_HAL::micros64(); #endif } diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 4a2479e153f..9790ef49ecf 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -10,6 +10,8 @@ #if HAL_SIM_GPS_ENABLED +#include + #include #include #include From 2e809282e7eb7e484e5dea9c48576facd5b21213 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Oct 2021 12:12:20 +1100 Subject: [PATCH 0308/3101] SITL: split AP_HAL_SITL and AP_SIM_ENABLED --- libraries/SITL/SIM_GPS.h | 2 +- libraries/SITL/SITL.cpp | 6 +++--- libraries/SITL/SITL.h | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 1244f573389..dd2fa3d0f3e 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -26,7 +26,7 @@ param set SERIAL5_PROTOCOL 5 #include #ifndef HAL_SIM_GPS_ENABLED -#define HAL_SIM_GPS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)) +#define HAL_SIM_GPS_ENABLED (AP_SIM_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) #endif #if HAL_SIM_GPS_ENABLED diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 66b8f962a68..03899443df2 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -19,11 +19,11 @@ #include "SITL.h" +#if AP_SIM_ENABLED + #include #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - #include #include #include @@ -659,4 +659,4 @@ SITL::SIM *sitl() }; -#endif // CONFIG_HAL_BOARD +#endif // AP_SIM_ENABLED diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index ec210a99d33..b3e95e0b87a 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -2,7 +2,7 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED #include #include @@ -486,4 +486,4 @@ namespace AP { SITL::SIM *sitl(); }; -#endif // CONFIG_HAL_BOARD +#endif // AP_SIM_ENABLED From 4c8fc3bcafae91d78307877664afe7160353d8ef Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Oct 2021 16:31:22 +1100 Subject: [PATCH 0309/3101] waf: split AP_HAL_SITL and AP_SIM_ENABLED --- Tools/ardupilotwaf/boards.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index ef7eb068203..3c5080743e7 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -545,6 +545,7 @@ def configure_env(self, cfg, env): HAL_PROBE_EXTERNAL_I2C_BAROS = 1, ) + cfg.define('AP_SIM_ENABLED', 1) cfg.define('HAL_WITH_SPI', 1) cfg.define('HAL_WITH_RAMTRON', 1) @@ -708,7 +709,8 @@ def expand_path(p): super(esp32, self).configure_env(cfg, env) cfg.load('esp32') env.DEFINES.update( - CONFIG_HAL_BOARD = 'HAL_BOARD_ESP32' + CONFIG_HAL_BOARD = 'HAL_BOARD_ESP32', + AP_SIM_ENABLED = 0, ) tt = self.name[5:] #leave off 'esp32' so we just get 'buzz','diy','icarus, etc @@ -920,6 +922,18 @@ def configure_env(self, cfg, env): else: cfg.msg("Enabling ChibiOS thread statistics", "no") + if cfg.env.SIM_ENABLED: + env.DEFINES.update( + AP_SIM_ENABLED = 1, + ) + env.AP_LIBRARIES += [ + 'SITL', + ] + else: + env.DEFINES.update( + AP_SIM_ENABLED = 0, + ) + env.LIB += ['gcc', 'm'] env.GIT_SUBMODULES += [ @@ -984,6 +998,7 @@ def configure_env(self, cfg, env): env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_LINUX', CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_NONE', + AP_SIM_ENABLED = 0, ) if not cfg.env.DEBUG: From b360521d0ba3c4d25628e5705e1a5a7778e0543f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Oct 2021 17:10:20 +1100 Subject: [PATCH 0310/3101] SITL: permit double-precision maths in SITL even on embedded hardware --- libraries/SITL/SIM_Aircraft.cpp | 3 ++- libraries/SITL/SIM_BalanceBot.cpp | 2 ++ libraries/SITL/SIM_Blimp.cpp | 2 ++ libraries/SITL/SIM_GPS.cpp | 2 ++ libraries/SITL/SIM_MS5611.cpp | 2 ++ libraries/SITL/SIM_Plane.cpp | 2 ++ libraries/SITL/SIM_Submarine.h | 6 +++--- libraries/SITL/SITL.cpp | 2 ++ 8 files changed, 17 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index bcc95e85878..00f355a336b 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -16,13 +16,14 @@ parent class for aircraft simulators */ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SIM_Aircraft.h" #include #include #include - #if defined(__CYGWIN__) || defined(__CYGWIN64__) #include #include diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp index 384be77f672..f3b11d0c903 100644 --- a/libraries/SITL/SIM_BalanceBot.cpp +++ b/libraries/SITL/SIM_BalanceBot.cpp @@ -16,6 +16,8 @@ Balance Bot simulator class */ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SIM_BalanceBot.h" #include diff --git a/libraries/SITL/SIM_Blimp.cpp b/libraries/SITL/SIM_Blimp.cpp index ea129152b97..73e41d0eae1 100644 --- a/libraries/SITL/SIM_Blimp.cpp +++ b/libraries/SITL/SIM_Blimp.cpp @@ -16,6 +16,8 @@ Blimp simulator class */ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SIM_Blimp.h" #include diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 9790ef49ecf..5e45104178b 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -12,6 +12,8 @@ #include +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include #include #include diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp index a1957df17da..d269272598b 100644 --- a/libraries/SITL/SIM_MS5611.cpp +++ b/libraries/SITL/SIM_MS5611.cpp @@ -1,3 +1,5 @@ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SIM_MS5611.h" #include diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index c529f33814d..f7b61f7df38 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -17,6 +17,8 @@ just enough to be able to debug control logic for new frame types */ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SIM_Plane.h" #include diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h index 42181e5a728..6c562cd496c 100644 --- a/libraries/SITL/SIM_Submarine.h +++ b/libraries/SITL/SIM_Submarine.h @@ -67,12 +67,12 @@ class Submarine : public Aircraft { float thruster_mount_radius = 0.25; // distance in meters from thrusters to center of mass. Used to calculate torque. float equivalent_sphere_radius = 0.2; // volume = 4.pi.r³/3 - float volume = 4 * M_PI * pow(equivalent_sphere_radius, 3) / 3; + float volume = 4 * M_PI * powf(equivalent_sphere_radius, 3) / 3; float density = 500; float mass = volume * density; // 16.75 kg // Moment of Inertia (I)(kg.m²) approximated with a sphere with a 25 cm radius (r) and same density as water // I = 2.m.r²/5 - float moment_of_inertia = 2 * (mass * pow(equivalent_sphere_radius, 2) / 5); + float moment_of_inertia = 2 * (mass * powf(equivalent_sphere_radius, 2) / 5); // Frame drag coefficient const Vector3f linear_drag_coefficient = Vector3f(1.4, 1.8, 2.0); @@ -83,7 +83,7 @@ class Submarine : public Aircraft { // $ V = 4 * pi * r^3 / 3 $ // $ r ^2 = (V * 3 / 4) ^ (2/3) $ // A = area (m^3), r = sphere radius (m) - float equivalent_sphere_area = M_PI_4 * pow(volume * 3.0f / 4.0f, 2.0f / 3.0f); + float equivalent_sphere_area = M_PI_4 * powf(volume * 3.0f / 4.0f, 2.0f / 3.0f); } frame_property; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 03899443df2..8007e4e4e1f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -17,6 +17,8 @@ SITL.cpp - software in the loop state */ +#define ALLOW_DOUBLE_MATH_FUNCTIONS + #include "SITL.h" #if AP_SIM_ENABLED From edef2ceb56880511a5b4987c11657374f02c88d4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 1 Nov 2021 18:18:52 +1100 Subject: [PATCH 0311/3101] SITL: usleep only on AP_HAL_SITL --- libraries/SITL/SIM_Aircraft.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 00f355a336b..5c5f9d5f2b4 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -40,6 +40,8 @@ using namespace SITL; +extern const AP_HAL::HAL& hal; + /* parent class for all simulator types */ @@ -281,7 +283,13 @@ void Aircraft::sync_frame_time(void) } if (sleep_debt_us > min_sleep_time) { // sleep if we have built up a debt of min_sleep_tim +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL usleep(sleep_debt_us); +#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + hal.scheduler->delay_microseconds(sleep_debt_us); +#else + // ?? +#endif sleep_debt_us -= (get_wall_time_us() - now); } last_wall_time_us = get_wall_time_us(); From a9aa5c2d60aa967ecc851aa9da0af6d3f346af19 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 15 Jan 2022 15:26:06 +1100 Subject: [PATCH 0312/3101] AP_GPS: add a new AP_GPS_SITL object --- libraries/AP_GPS/AP_GPS.cpp | 10 +++ libraries/AP_GPS/AP_GPS.h | 4 ++ libraries/AP_GPS/AP_GPS_SITL.cpp | 120 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_SITL.h | 40 +++++++++++ 4 files changed, 174 insertions(+) create mode 100644 libraries/AP_GPS/AP_GPS_SITL.cpp create mode 100644 libraries/AP_GPS/AP_GPS_SITL.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 3c9ede363e2..d794aa1e346 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -37,6 +37,9 @@ #include "AP_GPS_MSP.h" #include "AP_GPS_ExternalAHRS.h" #include "GPS_Backend.h" +#if HAL_SIM_GPS_ENABLED +#include "AP_GPS_SITL.h" +#endif #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include @@ -655,6 +658,13 @@ void AP_GPS::detect_instance(uint8_t instance) new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); break; #endif //AP_GPS_NOVA_ENABLED + +#if HAL_SIM_GPS_ENABLED + case GPS_TYPE_SITL: + new_gps = new AP_GPS_SITL(*this, state[instance], _port[instance]); + break; +#endif // HAL_SIM_GPS_ENABLED + default: break; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 89411ac9b57..65c37b2a680 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -22,6 +22,7 @@ #include "GPS_detect_state.h" #include #include +#include /** maximum number of GPS instances available on this platform. If more @@ -127,6 +128,9 @@ class AP_GPS GPS_TYPE_EXTERNAL_AHRS = 21, GPS_TYPE_UAVCAN_RTK_BASE = 22, GPS_TYPE_UAVCAN_RTK_ROVER = 23, +#if HAL_SIM_GPS_ENABLED + GPS_TYPE_SITL = 100, +#endif }; /// GPS status codes diff --git a/libraries/AP_GPS/AP_GPS_SITL.cpp b/libraries/AP_GPS/AP_GPS_SITL.cpp new file mode 100644 index 00000000000..fda48fb69a7 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SITL.cpp @@ -0,0 +1,120 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_GPS_SITL.h" + +#if HAL_SIM_GPS_ENABLED + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +/* + return GPS time of week in milliseconds + */ +/* + get timeval using simulation time + */ +static void simulation_timeval(struct timeval *tv) +{ + uint64_t now = AP_HAL::micros64(); + static uint64_t first_usec; + static struct timeval first_tv; + if (first_usec == 0) { + first_usec = now; + first_tv.tv_sec = AP::sitl()->start_time_UTC; + } + *tv = first_tv; + tv->tv_sec += now / 1000000ULL; + uint64_t new_usec = tv->tv_usec + (now % 1000000ULL); + tv->tv_sec += new_usec / 1000000ULL; + tv->tv_usec = new_usec % 1000000ULL; +} +static void gps_time(uint16_t *time_week, uint32_t *time_week_ms) +{ + struct timeval tv; + simulation_timeval(&tv); + const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL); + uint32_t epoch_seconds = tv.tv_sec - epoch; + *time_week = epoch_seconds / AP_SEC_PER_WEEK; + uint32_t t_ms = tv.tv_usec / 1000; + // round time to nearest 200ms + *time_week_ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200); +} + +bool AP_GPS_SITL::read(void) +{ + const uint32_t now = AP_HAL::millis(); + if (now - last_update_ms < 200) { + return false; + } + last_update_ms = now; + + auto *sitl = AP::sitl(); + + double latitude =sitl->state.latitude; + double longitude = sitl->state.longitude; + float altitude = sitl->state.altitude; + const double speedN = sitl->state.speedN; + const double speedE = sitl->state.speedE; + const double speedD = sitl->state.speedD; + // const double yaw = sitl->state.yawDeg; + + uint16_t time_week; + uint32_t time_week_ms; + + gps_time(&time_week, &time_week_ms); + + state.time_week = time_week; + state.time_week_ms = time_week_ms; + state.status = AP_GPS::GPS_OK_FIX_3D; + state.num_sats = 15; + + state.location = Location{ + int32_t(latitude*1e7), + int32_t(longitude*1e7), + int32_t(altitude*100), + Location::AltFrame::ABSOLUTE + }; + + state.hdop = 100; + state.vdop = 100; + + state.have_vertical_velocity = true; + state.velocity.x = speedN; + state.velocity.y = speedE; + state.velocity.z = speedD; + + state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x))); + state.ground_speed = state.velocity.xy().length(); + + state.have_speed_accuracy = true; + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + state.have_vertical_velocity = true; + + // state.horizontal_accuracy = pkt.horizontal_pos_accuracy; + // state.vertical_accuracy = pkt.vertical_pos_accuracy; + // state.speed_accuracy = pkt.horizontal_vel_accuracy; + + state.last_gps_time_ms = now; + + return true; +} + +#endif // HAL_SIM_GPS_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_SITL.h b/libraries/AP_GPS/AP_GPS_SITL.h new file mode 100644 index 00000000000..52079df1a08 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_SITL.h @@ -0,0 +1,40 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "GPS_Backend.h" + +#include + +#if HAL_SIM_GPS_ENABLED + +class AP_GPS_SITL : public AP_GPS_Backend +{ + +public: + + using AP_GPS_Backend::AP_GPS_Backend; + + bool read() override; + + const char *name() const override { return "SITL"; } + +private: + + uint32_t last_update_ms; +}; + +#endif // HAL_SIM_GPS_ENABLED From e07ddf95b43e0c6087d85584196e1569975a9e07 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:49 +1100 Subject: [PATCH 0313/3101] AP_Math: provide rand_float on embedded hardware --- libraries/AP_Math/AP_Math.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 6cb2dc6b3d5..9362f236d34 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -333,11 +333,15 @@ uint16_t get_random16(void) } -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED // generate a random float between -1 and 1, for use in SITL float rand_float(void) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6; +#else + return get_random16() / 65535.0; +#endif } Vector3f rand_vec3f(void) From 26d6c2413b0d4d0ba6f137cbeb57b818714ae326 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:49 +1100 Subject: [PATCH 0314/3101] AP_Scheduler: update simulation state on embedded --- libraries/AP_Scheduler/AP_Scheduler.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index e5534e3edfb..f77567f6f40 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include @@ -400,6 +401,10 @@ void AP_Scheduler::loop() perf_info.check_loop_time(sample_time_us - _loop_timer_start_us); _loop_timer_start_us = sample_time_us; + +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + hal.simstate->update(); +#endif } void AP_Scheduler::update_logging() From 8b2f85756afb4ed8c7869835ccf836c9b6f2102d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 1 Nov 2021 18:16:41 +1100 Subject: [PATCH 0315/3101] AP_HAL_ChibiOS: instantiate SIMState object in HAL --- libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 41f90b71c7c..fc90bd369a9 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -34,6 +34,7 @@ #include #endif #include +#include #include @@ -97,6 +98,10 @@ static ChibiOS::Scheduler schedulerInstance; static ChibiOS::Util utilInstance; static Empty::OpticalFlow opticalFlowDriver; +#if AP_SIM_ENABLED +static AP_HAL::SIMState xsimstate; +#endif + #if HAL_WITH_DSP static ChibiOS::DSP dspDriver; #else @@ -152,6 +157,9 @@ HAL_ChibiOS::HAL_ChibiOS() : &utilInstance, &opticalFlowDriver, &flashDriver, +#if AP_SIM_ENABLED + &xsimstate, +#endif &dspDriver, #if HAL_NUM_CAN_IFACES (AP_HAL::CANIface**)canDrivers From f7608c22da23f466607f4589919e0684553dffd7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 1 Nov 2021 18:17:10 +1100 Subject: [PATCH 0316/3101] AP_HAL_ChibiOS: set pwm output values in simstate object --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index ad8a5861052..40168cb385a 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -32,6 +32,10 @@ #include #endif +#if AP_SIM_ENABLED +#include +#endif + #if HAL_USE_PWM == TRUE #include @@ -512,11 +516,17 @@ void RCOutput::disable_ch(uint8_t chan) void RCOutput::write(uint8_t chan, uint16_t period_us) { + if (chan >= max_channels) { return; } last_sent[chan] = period_us; +#if AP_SIM_ENABLED + hal.simstate->pwm_output[chan] = period_us; + return; +#endif + #if HAL_WITH_IO_MCU // handle IO MCU channels if (AP_BoardConfig::io_enabled()) { From 650ef59be852656eb763a9554a93ceb2da6271d7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:48 +1100 Subject: [PATCH 0317/3101] AP_HAL: create HAL::SIMState object to hold simulation state --- libraries/AP_HAL/AP_HAL_Namespace.h | 2 + libraries/AP_HAL/HAL.h | 10 + libraries/AP_HAL/SIMState.cpp | 354 ++++++++++++++++++++++++++++ libraries/AP_HAL/SIMState.h | 234 ++++++++++++++++++ 4 files changed, 600 insertions(+) create mode 100644 libraries/AP_HAL/SIMState.cpp create mode 100644 libraries/AP_HAL/SIMState.h diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index e22e631cf52..83946aaf31b 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -65,6 +65,8 @@ namespace AP_HAL { SPIDevice_Type = -1, }; + class SIMState; + // Must be implemented by the concrete HALs. const HAL& get_HAL(); } diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index b6e5146c359..7b0887fbe7a 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -43,6 +43,9 @@ class AP_HAL::HAL { AP_HAL::Util* _util, AP_HAL::OpticalFlow*_opticalflow, AP_HAL::Flash* _flash, +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + class AP_HAL::SIMState* _simstate, +#endif AP_HAL::DSP* _dsp, #if HAL_NUM_CAN_IFACES > 0 AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES]) @@ -73,6 +76,9 @@ class AP_HAL::HAL { util(_util), opticalflow(_opticalflow), flash(_flash), +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + simstate(_simstate), +#endif dsp(_dsp) { #if HAL_NUM_CAN_IFACES > 0 @@ -144,4 +150,8 @@ class AP_HAL::HAL { UARTDriver* serial(uint8_t sernum) const; static constexpr uint8_t num_serial = 10; + +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + AP_HAL::SIMState *simstate; +#endif }; diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp new file mode 100644 index 00000000000..960ccfaaf92 --- /dev/null +++ b/libraries/AP_HAL/SIMState.cpp @@ -0,0 +1,354 @@ +#include "SIMState.h" + +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + +/* + * This is a very-much-cut-down AP_HAL_SITL object. We should make + * PA_HAL_SITL use this object - by moving a lot more code from over + * there into here. + */ + +#include + +extern const AP_HAL::HAL& hal; + +using namespace AP_HAL; + +#include + +void SIMState::update() +{ + static bool init_done; + if (!init_done) { + init_done = true; + sitl_model = SITL::MultiCopter::create("+"); + } + + _fdm_input_step(); +} + +/* + setup for SITL handling + */ +void SIMState::_sitl_setup(const char *home_str) +{ + _home_str = home_str; + + printf("Starting SITL input\n"); + + // find the barometer object if it exists + _barometer = AP_Baro::get_singleton(); +} + + +/* + step the FDM by one time step + */ +void SIMState::_fdm_input_step(void) +{ + fdm_input_local(); +} + +/* + get FDM input from a local model + */ +void SIMState::fdm_input_local(void) +{ + struct sitl_input input; + + // construct servos structure for FDM + _simulator_servos(input); + + // read servo inputs from ride along flight controllers + // ride_along.receive(input); + + // update the model + sitl_model->update_model(input); + + // get FDM output from the model + if (_sitl == nullptr) { + _sitl = AP::sitl(); + } + if (_sitl) { + sitl_model->fill_fdm(_sitl->state); + + if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) { + for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) { + pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000; + } + } + } + + // output JSON state to ride along flight controllers + // ride_along.send(_sitl->state,sitl_model->get_position_relhome()); + +#if HAL_SIM_GIMBAL_ENABLED + if (gimbal != nullptr) { + gimbal->update(); + } +#endif +#if HAL_SIM_ADSB_ENABLED + if (adsb != nullptr) { + adsb->update(); + } +#endif + if (vicon != nullptr) { + Quaternion attitude; + sitl_model->get_attitude(attitude); + vicon->update(sitl_model->get_location(), + sitl_model->get_position_relhome(), + sitl_model->get_velocity_ef(), + attitude); + } + if (benewake_tf02 != nullptr) { + benewake_tf02->update(sitl_model->rangefinder_range()); + } + if (benewake_tf03 != nullptr) { + benewake_tf03->update(sitl_model->rangefinder_range()); + } + if (benewake_tfmini != nullptr) { + benewake_tfmini->update(sitl_model->rangefinder_range()); + } + if (lightwareserial != nullptr) { + lightwareserial->update(sitl_model->rangefinder_range()); + } + if (lightwareserial_binary != nullptr) { + lightwareserial_binary->update(sitl_model->rangefinder_range()); + } + if (lanbao != nullptr) { + lanbao->update(sitl_model->rangefinder_range()); + } + if (blping != nullptr) { + blping->update(sitl_model->rangefinder_range()); + } + if (leddarone != nullptr) { + leddarone->update(sitl_model->rangefinder_range()); + } + if (USD1_v0 != nullptr) { + USD1_v0->update(sitl_model->rangefinder_range()); + } + if (USD1_v1 != nullptr) { + USD1_v1->update(sitl_model->rangefinder_range()); + } + if (maxsonarseriallv != nullptr) { + maxsonarseriallv->update(sitl_model->rangefinder_range()); + } + if (wasp != nullptr) { + wasp->update(sitl_model->rangefinder_range()); + } + if (nmea != nullptr) { + nmea->update(sitl_model->rangefinder_range()); + } + if (rf_mavlink != nullptr) { + rf_mavlink->update(sitl_model->rangefinder_range()); + } + if (gyus42v2 != nullptr) { + gyus42v2->update(sitl_model->rangefinder_range()); + } + if (efi_ms != nullptr) { + efi_ms->update(); + } + + if (frsky_d != nullptr) { + frsky_d->update(); + } + // if (frsky_sport != nullptr) { + // frsky_sport->update(); + // } + // if (frsky_sportpassthrough != nullptr) { + // frsky_sportpassthrough->update(); + // } + +#if AP_SIM_CRSF_ENABLED + if (crsf != nullptr) { + crsf->update(); + } +#endif + +#if HAL_SIM_PS_RPLIDARA2_ENABLED + if (rplidara2 != nullptr) { + rplidara2->update(sitl_model->get_location()); + } +#endif + +#if HAL_SIM_PS_TERARANGERTOWER_ENABLED + if (terarangertower != nullptr) { + terarangertower->update(sitl_model->get_location()); + } +#endif + +#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED + if (sf45b != nullptr) { + sf45b->update(sitl_model->get_location()); + } +#endif + + if (vectornav != nullptr) { + vectornav->update(); + } + + if (lord != nullptr) { + lord->update(); + } + +#if HAL_SIM_AIS_ENABLED + if (ais != nullptr) { + ais->update(); + } +#endif + for (uint8_t i=0; iupdate(); + } + } + + // update simulation time + if (_sitl) { + hal.scheduler->stop_clock(_sitl->state.timestamp_us); + } else { + hal.scheduler->stop_clock(AP_HAL::micros64()+100); + } + + set_height_agl(); + + _synthetic_clock_mode = true; + _update_count++; +} + +/* + create sitl_input structure for sending to FDM + */ +void SIMState::_simulator_servos(struct sitl_input &input) +{ + // output at chosen framerate + uint32_t now = AP_HAL::micros(); + // last_update_usec = now; + + float altitude = _barometer?_barometer->get_altitude():0; + float wind_speed = 0; + float wind_direction = 0; + float wind_dir_z = 0; + + // give 5 seconds to calibrate airspeed sensor at 0 wind speed + if (wind_start_delay_micros == 0) { + wind_start_delay_micros = now; + } else if (_sitl && (now - wind_start_delay_micros) > 5000000 ) { + // The EKF does not like step inputs so this LPF keeps it happy. + wind_speed = _sitl->wind_speed_active = (0.95f*_sitl->wind_speed_active) + (0.05f*_sitl->wind_speed); + wind_direction = _sitl->wind_direction_active = (0.95f*_sitl->wind_direction_active) + (0.05f*_sitl->wind_direction); + wind_dir_z = _sitl->wind_dir_z_active = (0.95f*_sitl->wind_dir_z_active) + (0.05f*_sitl->wind_dir_z); + + // pass wind into simulators using different wind types via param SIM_WIND_T*. + switch (_sitl->wind_type) { + case SITL::SIM::WIND_TYPE_SQRT: + if (altitude < _sitl->wind_type_alt) { + wind_speed *= sqrtf(MAX(altitude / _sitl->wind_type_alt, 0)); + } + break; + + case SITL::SIM::WIND_TYPE_COEF: + wind_speed += (altitude - _sitl->wind_type_alt) * _sitl->wind_type_coef; + break; + + case SITL::SIM::WIND_TYPE_NO_LIMIT: + default: + break; + } + + // never allow negative wind velocity + wind_speed = MAX(wind_speed, 0); + } + + input.wind.speed = wind_speed; + input.wind.direction = wind_direction; + input.wind.turbulence = _sitl?_sitl->wind_turbulance:0; + input.wind.dir_z = wind_dir_z; + + for (uint8_t i=0; ifetteconewireesc_sim.enabled()) { + _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input); + for (uint8_t i=0; istate.battery_voltage <= 0) { + } else { + // FDM provides voltage and current + voltage = _sitl->state.battery_voltage; + _current = _sitl->state.battery_current; + } + } + + // assume 3DR power brick + voltage_pin_value = ((voltage / 10.1f) / 5.0f) * 1024; + current_pin_value = ((_current / 17.0f) / 5.0f) * 1024; + // fake battery2 as just a 25% gain on the first one + voltage2_pin_value = ((voltage * 0.25f / 10.1f) / 5.0f) * 1024; + current2_pin_value = ((_current * 0.25f / 17.0f) / 5.0f) * 1024; +} + +/* + set height above the ground in meters + */ +void SIMState::set_height_agl(void) +{ + static float home_alt = -1; + + if (!_sitl) { + // in example program + return; + } + + if (is_equal(home_alt, -1.0f) && _sitl->state.altitude > 0) { + // remember home altitude as first non-zero altitude + home_alt = _sitl->state.altitude; + } + +#if AP_TERRAIN_AVAILABLE + if (_sitl != nullptr && + _sitl->terrain_enable) { + // get height above terrain from AP_Terrain. This assumes + // AP_Terrain is working + float terrain_height_amsl; + struct Location location; + location.lat = _sitl->state.latitude*1.0e7; + location.lng = _sitl->state.longitude*1.0e7; + + AP_Terrain *_terrain = AP_Terrain::get_singleton(); + if (_terrain != nullptr && + _terrain->height_amsl(location, terrain_height_amsl)) { + _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; + return; + } + } +#endif + + if (_sitl != nullptr) { + // fall back to flat earth model + _sitl->height_agl = _sitl->state.altitude - home_alt; + } +} + +#endif // AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h new file mode 100644 index 00000000000..af572105e06 --- /dev/null +++ b/libraries/AP_HAL/SIMState.h @@ -0,0 +1,234 @@ +#pragma once + +#include + +#if AP_SIM_ENABLED + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +class AP_HAL::SIMState { +public: + + // simulated airspeed, sonar and battery monitor + uint16_t sonar_pin_value; // pin 0 + uint16_t airspeed_pin_value; // pin 1 + uint16_t airspeed_2_pin_value; // pin 2 + uint16_t voltage_pin_value; // pin 13 + uint16_t current_pin_value; // pin 12 + uint16_t voltage2_pin_value; // pin 15 + uint16_t current2_pin_value; // pin 14 + + void update(); + + void set_gps0(SITL::GPS *_gps) { gps[0] = _gps; } + + uint16_t pwm_output[16]; // was SITL_NUM_CHANNELS + +private: + void _set_param_default(const char *parm); + void _sitl_setup(const char *home_str); + void _setup_timer(void); + void _setup_adc(void); + + void set_height_agl(void); + void _set_signal_handlers(void) const; + + void _update_airspeed(float airspeed); + void _simulator_servos(struct sitl_input &input); + void _fdm_input_step(void); + void fdm_input_local(void); + + void wait_clock(uint64_t wait_time_usec); + + uint16_t pwm_input[16]; // was SITL_RC_INPUT_CHANNELS + + // internal state + // enum vehicle_type _vehicle; + uint8_t _instance; + uint16_t _base_port; + pid_t _parent_pid; + uint32_t _update_count; + + AP_Baro *_barometer; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + SocketAPM _sitl_rc_in{true}; +#endif + SITL::SIM *_sitl; + uint16_t _rcin_port; + uint16_t _fg_view_port; + uint16_t _irlock_port; + float _current; + + bool _synthetic_clock_mode; + + bool _use_rtscts; + bool _use_fg_view; + + const char *_fg_address; + + // delay buffer variables + static const uint8_t wind_buffer_length = 50; + + // airspeed sensor delay buffer variables + struct readings_wind { + uint32_t time; + float data; + }; + uint8_t store_index_wind; + uint32_t last_store_time_wind; + VectorN buffer_wind; + VectorN buffer_wind_2; + uint32_t time_delta_wind; + uint32_t delayed_time_wind; + uint32_t wind_start_delay_micros; + + // internal SITL model + SITL::Aircraft *sitl_model; + +#if HAL_SIM_GIMBAL_ENABLED + // simulated gimbal + bool enable_gimbal; + SITL::Gimbal *gimbal; +#endif + +#if HAL_SIM_ADSB_ENABLED + // simulated ADSb + SITL::ADSB *adsb; +#endif + + // simulated vicon system: + SITL::Vicon *vicon; + + // simulated Benewake tf02 rangefinder: + SITL::RF_Benewake_TF02 *benewake_tf02; + // simulated Benewake tf03 rangefinder: + SITL::RF_Benewake_TF03 *benewake_tf03; + // simulated Benewake tfmini rangefinder: + SITL::RF_Benewake_TFmini *benewake_tfmini; + + // simulated LightWareSerial rangefinder - legacy protocol:: + SITL::RF_LightWareSerial *lightwareserial; + // simulated LightWareSerial rangefinder - binary protocol: + SITL::RF_LightWareSerialBinary *lightwareserial_binary; + // simulated Lanbao rangefinder: + SITL::RF_Lanbao *lanbao; + // simulated BLping rangefinder: + SITL::RF_BLping *blping; + // simulated LeddarOne rangefinder: + SITL::RF_LeddarOne *leddarone; + // simulated USD1 v0 rangefinder: + SITL::RF_USD1_v0 *USD1_v0; + // simulated USD1 v1 rangefinder: + SITL::RF_USD1_v1 *USD1_v1; + // simulated MaxsonarSerialLV rangefinder: + SITL::RF_MaxsonarSerialLV *maxsonarseriallv; + // simulated Wasp rangefinder: + SITL::RF_Wasp *wasp; + // simulated NMEA rangefinder: + SITL::RF_NMEA *nmea; + // simulated MAVLink rangefinder: + SITL::RF_MAVLink *rf_mavlink; + // simulated GYUS42v2 rangefinder: + SITL::RF_GYUS42v2 *gyus42v2; + + // simulated Frsky devices + SITL::Frsky_D *frsky_d; + // SITL::Frsky_SPort *frsky_sport; + // SITL::Frsky_SPortPassthrough *frsky_sportpassthrough; + +#if HAL_SIM_PS_RPLIDARA2_ENABLED + // simulated RPLidarA2: + SITL::PS_RPLidarA2 *rplidara2; +#endif + + // simulated FETtec OneWire ESCs: + SITL::FETtecOneWireESC *fetteconewireesc; + +#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED + // simulated SF45B proximity sensor: + SITL::PS_LightWare_SF45B *sf45b; +#endif + +#if HAL_SIM_PS_TERARANGERTOWER_ENABLED + SITL::PS_TeraRangerTower *terarangertower; +#endif + +#if AP_SIM_CRSF_ENABLED + // simulated CRSF devices + SITL::CRSF *crsf; +#endif + + // simulated VectorNav system: + SITL::VectorNav *vectornav; + + // simulated LORD Microstrain system + SITL::LORD *lord; + +#if HAL_SIM_JSON_MASTER_ENABLED + // Ride along instances via JSON SITL backend + SITL::JSON_Master ride_along; +#endif + +#if HAL_SIM_AIS_ENABLED + // simulated AIS stream + SITL::AIS *ais; +#endif + + // simulated EFI MegaSquirt device: + SITL::EFI_MegaSquirt *efi_ms; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + // output socket for flightgear viewing + SocketAPM fg_socket{true}; +#endif + + const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; + + const char *_home_str; + + // simulated GPS devices + SITL::GPS *gps[2]; // constrained by # of parameter sets +}; + +#endif // AP_SIM_ENABLED From a402dce95b2681b697e6ab0ef7cf2384199756c2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 28 Jan 2022 14:25:13 +1100 Subject: [PATCH 0318/3101] AP_HAL: permit double-maths whe simulating --- libraries/AP_HAL/AP_HAL_Macros.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL/AP_HAL_Macros.h b/libraries/AP_HAL/AP_HAL_Macros.h index 5c198fed060..25e124af666 100644 --- a/libraries/AP_HAL/AP_HAL_Macros.h +++ b/libraries/AP_HAL/AP_HAL_Macros.h @@ -6,7 +6,7 @@ macros to allow code to build on multiple platforms more easily */ -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || HAL_WITH_EKF_DOUBLE +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || HAL_WITH_EKF_DOUBLE || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED) /* allow double maths on Linux and SITL to avoid problems with system headers */ From da21b48f7e3372d0be20e40e2b9d02f13aeb6990 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:49 +1100 Subject: [PATCH 0319/3101] AP_InertialSensor: split AP_HAL_SITL and HAL_SIM_ENABLED --- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index fbbceff9dad..b851e76d27a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -48,7 +48,7 @@ #include #ifndef AP_SIM_INS_ENABLED -#define AP_SIM_INS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#define AP_SIM_INS_ENABLED AP_SIM_ENABLED #endif class AP_InertialSensor_Backend; From a53af5277c47b72f24b29c961df08881838d4dcf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 Mar 2022 11:12:29 +1100 Subject: [PATCH 0320/3101] AP_InertialSensor: assume accel cals are stored for sitl-on-hw Same as for SITL - they don't need to actually be in storage to be valid --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index b19b2614ad0..40fdb8878e4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -746,7 +746,7 @@ bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_ra _accel_id[_accel_count].set((int32_t) id); -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED) // assume this is the same sensor and save its ID to allow seamless // transition from when we didn't have the IDs. _accel_id_ok[_accel_count] = true; From daedebce2e67fbe0209cc3d9f2ea8028c0c54c1f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:47 +1100 Subject: [PATCH 0321/3101] AP_Baro: create and use HAL_SIM_BARO_ENABLED --- libraries/AP_Baro/AP_Baro.cpp | 29 ++++++++++++++++++++--------- libraries/AP_Baro/AP_Baro.h | 2 +- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index e2ce13be4d4..0447cae0bdd 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -535,7 +535,17 @@ void AP_Baro::init(void) sensors[i].bus_id.set(0); } -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if AP_SIM_BARO_ENABLED + SITL::SIM *sitl = AP::sitl(); + if (sitl == nullptr) { + AP_HAL::panic("No SITL pointer"); + } + for(uint8_t i = 0; i < sitl->baro_count; i++) { + ADD_BACKEND(new AP_Baro_SITL(*this)); + } +#endif + +#if HAL_ENABLE_LIBUAVCAN_DRIVERS && !AP_SIM_BARO_ENABLED // Detect UAVCAN Modules, try as many times as there are driver slots for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { ADD_BACKEND(AP_Baro_UAVCAN::probe(*this)); @@ -552,6 +562,15 @@ void AP_Baro::init(void) // macro for use by HAL_INS_PROBE_LIST #define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address) +#if AP_SIM_BARO_ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + ADD_BACKEND(AP_Baro_MS56XX::probe(*this, + std::move(GET_I2C_DEVICE(_ext_bus, HAL_BARO_MS5611_I2C_ADDR)))); +#endif + // do not probe for other drivers when using simulation: + return; +#endif + #if defined(HAL_BARO_PROBE_LIST) // probe list from BARO lines in hwdef.dat HAL_BARO_PROBE_LIST; @@ -630,14 +649,6 @@ void AP_Baro::init(void) default: break; } -#elif AP_SIM_BARO_ENABLED - SITL::SIM *sitl = AP::sitl(); - if (sitl == nullptr) { - AP_HAL::panic("No SITL pointer"); - } - for(uint8_t i = 0; i < sitl->baro_count; i++) { - ADD_BACKEND(new AP_Baro_SITL(*this)); - } #elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H_IMU_I2C ADD_BACKEND(AP_Baro_LPS2XH::probe_InvensenseIMU(*this, std::move(GET_I2C_DEVICE(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)), diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index c4f35520283..68280969b85 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -7,7 +7,7 @@ #include #ifndef AP_SIM_BARO_ENABLED -#define AP_SIM_BARO_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#define AP_SIM_BARO_ENABLED AP_SIM_ENABLED #endif #ifndef HAL_MSP_BARO_ENABLED From 57f8b7c2a8298834f35d34243f1cb9d06127d10d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:15:48 +1100 Subject: [PATCH 0322/3101] AP_Compass: split AP_HAL_SITL and AP_SIM_ENABLED --- libraries/AP_Compass/AP_Compass.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 0acfc7e9e35..d2040c14346 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -71,7 +71,7 @@ #define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES) #ifndef AP_SIM_COMPASS_ENABLED -#define AP_SIM_COMPASS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#define AP_SIM_COMPASS_ENABLED AP_SIM_ENABLED #endif #include "CompassCalibrator.h" From 0bcda3c57e69317f98652291ceb7bd7d0d58ae7c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Oct 2021 12:12:20 +1100 Subject: [PATCH 0323/3101] AP_Vehicle: split AP_HAL_SITL and AP_SIM_ENABLED --- libraries/AP_Vehicle/AP_Vehicle.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 8cc1a3ed7a1..58392ff97a8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -48,9 +48,7 @@ #include #include #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include -#endif #include class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -393,7 +391,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { ModeReason control_mode_reason = ModeReason::UNKNOWN; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED SITL::SIM sitl; #endif From df6efc37ac87f01079420058f084828b7594e5cd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Oct 2021 16:31:21 +1100 Subject: [PATCH 0324/3101] ArduCopter: split AP_HAL_SITL and AP_SIM_ENABLED --- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/Parameters.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index ff3c53e159d..205dd127743 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -426,7 +426,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { // @Param: EXTRA1 // @DisplayName: Extra data type 1 stream rate to ground station - // @Description: Stream rate of ATTITUDE, SIMSTATE (SITL only), AHRS2 and PID_TUNING to ground station + // @Description: Stream rate of ATTITUDE, SIMSTATE (SIM only), AHRS2 and PID_TUNING to ground station // @Units: Hz // @Range: 0 50 // @Increment: 1 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 1fd0bd11e72..7c365efe643 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -610,7 +610,7 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(sprayer, "SPRAY_", AC_Sprayer), #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp GOBJECT(sitl, "SIM_", SITL::SIM), From cfd9bfd5414f0f6ff79ad7984822ba2176241e35 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:37:05 +1100 Subject: [PATCH 0325/3101] AntennaTracker: split AP_HAL_SITL and HAL_SIM_ENABLED --- AntennaTracker/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 42be7948916..a7e728cbcb4 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -281,7 +281,7 @@ const AP_Param::Info Tracker::var_info[] = { // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp GOBJECT(sitl, "SIM_", SITL::SIM), From d93449ac285964d130cc6bfbb87550be2a83f4a7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:37:05 +1100 Subject: [PATCH 0326/3101] ArduPlane: split AP_HAL_SITL and HAL_SIM_ENABLED --- ArduPlane/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index c38483f0b1c..57fd778c60d 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -924,7 +924,7 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(can_mgr, "CAN_", AP_CANManager), #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp GOBJECT(sitl, "SIM_", SITL::SIM), From 07b299997d0b775bcc6918c88ba26f4340357e56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:37:05 +1100 Subject: [PATCH 0327/3101] ArduSub: split AP_HAL_SITL and HAL_SIM_ENABLED --- ArduSub/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 549c7af0686..234598351ec 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -521,7 +521,7 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(can_mgr, "CAN_", AP_CANManager), #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED GOBJECT(sitl, "SIM_", SITL::SIM), #endif From a501a86c8d3f370b88edee63356abd3189d18c73 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:37:05 +1100 Subject: [PATCH 0328/3101] Blimp: split AP_HAL_SITL and HAL_SIM_ENABLED --- Blimp/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 9ac54e3658b..884e9771baa 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -387,7 +387,7 @@ const AP_Param::Info Blimp::var_info[] = { GOBJECT(can_mgr, "CAN_", AP_CANManager), #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED GOBJECT(sitl, "SIM_", SITL::SIM), #endif From 318678ac96f25fb5d05eb6716579604d8b56ffbe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:37:06 +1100 Subject: [PATCH 0329/3101] Rover: split AP_HAL_SITL and HAL_SIM_ENABLED --- Rover/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 8df3cc7fdad..96dad7a978b 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -294,7 +294,7 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp GOBJECT(ins, "INS_", AP_InertialSensor), -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED // @Group: SIM_ // @Path: ../libraries/SITL/SITL.cpp GOBJECT(sitl, "SIM_", SITL::SIM), From 7ad9609fb70d5b40dec86b5a4a16837bd15610ee Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 11:38:13 +1100 Subject: [PATCH 0330/3101] AP_AHRS: split AP_HAL_SITL and HAL_SIM_ENABLED --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- libraries/AP_AHRS/AP_AHRS.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 9816e11b3c7..a37ac73bbd7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -3141,7 +3141,7 @@ void AP_AHRS::Log_Write() Write_AHRS2(); Write_POS(); -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_AHRS_SIM_ENABLED AP::sitl()->Log_Write_SIMSTATE(); #endif } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index c2da9f2185b..a58ab57df7a 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -33,7 +33,7 @@ #endif #ifndef AP_AHRS_SIM_ENABLED -#define AP_AHRS_SIM_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED #endif #if AP_AHRS_SIM_ENABLED From e0561b0ca16024fe99f1a52d58a3e51270a18781 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 30 Oct 2021 12:11:38 +1100 Subject: [PATCH 0331/3101] GCS_MAVLink: split AP_HAL_SITL and HAL_SIM_ENABLED --- libraries/GCS_MAVLink/GCS_Common.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e24e20cb394..955aac0d20e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3852,7 +3852,7 @@ void GCS_MAVLINK::send_banner() void GCS_MAVLINK::send_simstate() const { -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED SITL::SIM *sitl = AP::sitl(); if (sitl == nullptr) { return; @@ -3863,7 +3863,7 @@ void GCS_MAVLINK::send_simstate() const void GCS_MAVLINK::send_sim_state() const { -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AP_SIM_ENABLED SITL::SIM *sitl = AP::sitl(); if (sitl == nullptr) { return; From fa2e841387cc6007d6ba54e3b946d97170ab1481 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Jan 2022 13:19:01 +1100 Subject: [PATCH 0332/3101] AP_HAL: honour HAL_SIM_GPS_ENABLED --- libraries/AP_HAL/SIMState.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index af572105e06..118d27baf06 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -60,7 +60,9 @@ class AP_HAL::SIMState { void update(); +#if HAL_SIM_GPS_ENABLED void set_gps0(SITL::GPS *_gps) { gps[0] = _gps; } +#endif uint16_t pwm_output[16]; // was SITL_NUM_CHANNELS @@ -227,8 +229,10 @@ class AP_HAL::SIMState { const char *_home_str; +#if HAL_SIM_GPS_ENABLED // simulated GPS devices SITL::GPS *gps[2]; // constrained by # of parameter sets +#endif }; #endif // AP_SIM_ENABLED From ebe2205ba76f10023e82ff2f8dc2eed861f4795b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 8 Mar 2022 09:46:31 +1100 Subject: [PATCH 0333/3101] SITL: add on-hardware example files --- libraries/SITL/examples/on-hardware/README.md | 66 +++++++++++++++++++ .../SITL/examples/on-hardware/default.param | 59 +++++++++++++++++ .../on-hardware/extra-hwdef-sitl-on-hw.dat | 46 +++++++++++++ .../SITL/examples/on-hardware/sitl-on-hw.sh | 17 +++++ .../SITL/examples/on-hardware/some.script | 5 ++ 5 files changed, 193 insertions(+) create mode 100644 libraries/SITL/examples/on-hardware/README.md create mode 100644 libraries/SITL/examples/on-hardware/default.param create mode 100644 libraries/SITL/examples/on-hardware/extra-hwdef-sitl-on-hw.dat create mode 100755 libraries/SITL/examples/on-hardware/sitl-on-hw.sh create mode 100644 libraries/SITL/examples/on-hardware/some.script diff --git a/libraries/SITL/examples/on-hardware/README.md b/libraries/SITL/examples/on-hardware/README.md new file mode 100644 index 00000000000..7482e7cffa4 --- /dev/null +++ b/libraries/SITL/examples/on-hardware/README.md @@ -0,0 +1,66 @@ +# SITL-on-HW notes + +## Compiling and flashing + +Run the sitl-on-hw.sh script to compile and flash for MatekH743. Adjust for your own board if required before running. This script will configure a build ready for running SITL-on-hardware and attempt to upload it to a connected board. It includes a set of embedded parameters to configure the simulated sensors appropriately. + +:: + + cd $HOME/ardupilot + ./libraries/SITL/examples/on-hardware/sitl-on-hw.sh + +## Configuring + +Wipe the parameters on the board; this can be done with a mavlink command, or by setting the FORMAT_VERSION parameter to 0. + +For example: + +:: + + STABILIZE> wipe_parameters IREALLYMEAANIT + STABILIZE> Got COMMAND_ACK: PREFLIGHT_STORAGE: ACCEPTED + AP: All parameters reset, reboot board + reboot + +You may need to power-cycle the board at this point. + +:: + + Device /dev/serial/by-id/usb-ArduPilot_MatekH743_3A0019001051393036353035-if00 reopened OK + link 1 OK + heartbeat OK + disabling flow control on serial 2 + AP: Calibrating barometer + AP: Barometer 1 calibration complete + AP: Barometer 2 calibration complete + Init Gyro** + AP: ArduPilot Ready + Suggested EK3_BCOEF_* = 16.288, EK3_MCOEF = 0.208 + Home: -35.36326 149.1652 alt=584.0000m hdg=353.0000 + Smoothing reset at 0.001 + AP: RCOut: PWM:1-13 + AP: GPS 1: detected as SITL at 115200 baud + Time has wrapped + Time has wrapped 5577 368458 + AP: EKF3 IMU0 initialised + AP: EKF3 IMU1 initialised + AP: EKF3 IMU0 tilt alignment complete + AP: EKF3 IMU1 tilt alignment complete + AP: EKF3 IMU1 MAG0 initial yaw alignment complete + AP: EKF3 IMU0 MAG0 initial yaw alignment complete + AP: PERF: 0/3999 [2653:2349] F=400Hz sd=39 Ex=0 + AP: EKF3 IMU1 forced reset + AP: EKF3 IMU1 initialised + AP: EKF3 IMU0 forced reset + AP: EKF3 IMU0 initialised + AP: EKF3 IMU1 tilt alignment complete + AP: EKF3 IMU0 tilt alignment complete + AP: EKF3 IMU1 MAG0 initial yaw alignment complete + AP: EKF3 IMU0 MAG0 initial yaw alignment complete + AP: PreArm: 3D Accel calibration needed + AP: PERF: 0/4000 [2631:2369] F=400Hz sd=5 Ex=0 + AP: EKF3 IMU0 origin set + AP: EKF3 IMU1 origin set + AP: PERF: 0/4000 [2639:2362] F=400Hz sd=7 Ex=0 + +Force diff --git a/libraries/SITL/examples/on-hardware/default.param b/libraries/SITL/examples/on-hardware/default.param new file mode 100644 index 00000000000..657cdf4772b --- /dev/null +++ b/libraries/SITL/examples/on-hardware/default.param @@ -0,0 +1,59 @@ +AHRS_EKF_TYPE 10 + +ATC_RAT_YAW_P 0.09 +ATC_RAT_YAW_I 0.009 + +BATT_MONITOR 0 + +COMPASS_OFS_X 5 +COMPASS_OFS_Y 13 +COMPASS_OFS_Z -18 +COMPASS_OFS2_X 5 +COMPASS_OFS2_Y 13 +COMPASS_OFS2_Z -18 + +FENCE_RADIUS 150 + +FRAME_TYPE 0 +FRAME_CLASS 1 + +FS_THR_ENABLE 1 + +RC7_OPTION 7 + +FLTMODE1 7 +FLTMODE2 9 +FLTMODE3 6 +FLTMODE4 3 +FLTMODE5 5 +FLTMODE6 0 + +GPS_TYPE 100 + +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 +INS_ACC2OFFS_X 0.001 +INS_ACC2OFFS_Y 0.001 +INS_ACC2OFFS_Z 0.001 +INS_ACC2SCAL_X 1.001 +INS_ACC2SCAL_Y 1.001 +INS_ACC2SCAL_Z 1.001 + +MOT_THST_EXPO 0.65 +MOT_THST_HOVER 0.39 +MOT_BAT_VOLT_MIN 9.6 +MOT_BAT_VOLT_MAX 12.8 + +SCHED_DEBUG 0 + +SIM_MAG1_DEVID 97539 +SIM_BARO_RND 0 + +SIM_RATE_HZ 400 +SCHED_LOOP_RATE 400 + +BRD_RTC_TYPES 2 diff --git a/libraries/SITL/examples/on-hardware/extra-hwdef-sitl-on-hw.dat b/libraries/SITL/examples/on-hardware/extra-hwdef-sitl-on-hw.dat new file mode 100644 index 00000000000..fd0190862c7 --- /dev/null +++ b/libraries/SITL/examples/on-hardware/extra-hwdef-sitl-on-hw.dat @@ -0,0 +1,46 @@ +env SIM_ENABLED 1 + +define INS_MAX_INSTANCES 2 +define HAL_COMPASS_MAX_SENSORS 2 + +define HAL_NAVEKF2_AVAILABLE 0 +define EK3_FEATURE_BODY_ODOM 0 +define EK3_FEATURE_EXTERNAL_NAV 0 +define EK3_FEATURE_DRAG_FUSION 0 +define HAL_ADSB_ENABLED 0 +define HAL_MOUNT_ENABLED 0 +define HAL_PROXIMITY_ENABLED 0 +define HAL_VISUALODOM_ENABLED 0 +define HAL_GENERATOR_ENABLED 0 +# define HAL_LOGGING_ENABLED 0 +define HAL_CRSF_TELEM_ENABLED 0 +#define OSD_ENABLED 0 +define FRAME_HEXA 0 +define FRAME_OCTA 0 +define FRAME_DODECAHEXA 0 +define FRAME_Y6 0 +define FRAME_OCTAQUAD 0 + +define MODE_SMARTRTL_ENABLED 0 +define MODE_SPORT_ENABLED 0 +# define MODE_CIRCLE_ENABLED 0 +define MODE_THROW_ENABLED 0 +define MODE_TURTLE_ENABLED 0 +define MODE_ZIGZAG_ENABLED 0 +define MODE_FLOWHOLD 0 +define MODE_POSHOLD_ENABLED 0 +define MODE_SYSTEMID_ENABLED 0 +define MODE_ACRO_ENABLED 0 +define MODE_FOLLOW_ENABLED 0 +define MODE_FLIP_ENABLED 0 +define MODE_DRIFT_ENABLED 0 +define MODE_THROW_ENABLED 0 + +define LANDING_GEAR_ENABLED 0 +define HAL_MSP_OPTICALFLOW_ENABLED 0 +define HAL_SUPPORT_RCOUT_SERIAL 0 +define HAL_HOTT_TELEM_ENABLED 0 +# define HAL_WITH_DSP 0 + + +define HAL_HIGH_LATENCY2 0 diff --git a/libraries/SITL/examples/on-hardware/sitl-on-hw.sh b/libraries/SITL/examples/on-hardware/sitl-on-hw.sh new file mode 100755 index 00000000000..efa99028d6e --- /dev/null +++ b/libraries/SITL/examples/on-hardware/sitl-on-hw.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +set -e +set -x + +BOARD=NucleoH743 +BOARD=MatekH743 +#BOARD=F35Lightning + +THISDIR=$(dirname $0) + +./waf configure \ + --board=$BOARD \ + --extra-hwdef="$THISDIR/extra-hwdef-sitl-on-hw.dat" \ + --default-param="$THISDIR/default.param" + +./waf copter --upload diff --git a/libraries/SITL/examples/on-hardware/some.script b/libraries/SITL/examples/on-hardware/some.script new file mode 100644 index 00000000000..e6a63aa0023 --- /dev/null +++ b/libraries/SITL/examples/on-hardware/some.script @@ -0,0 +1,5 @@ +rc 1 1500 +rc 2 1500 +rc 3 1000 +rc 4 1500 +rc 5 2000 From 8e64d1cbd7e1804b04782c9b87da24cdd75f74f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 11:32:22 +1100 Subject: [PATCH 0334/3101] pymavlink: update submodule --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index b6ead7dfe02..2828d9dc216 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit b6ead7dfe02ea3732c8e82906895fb6b4ab88585 +Subproject commit 2828d9dc21682b83d6f67825a883a857b73879d3 From 8b16b5ca94bcf701ca70b32cc3e337c10e2cafd8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 29 Jan 2022 15:35:26 +1100 Subject: [PATCH 0335/3101] Plane: adjust throttle mix for auto landing use mix-max during landing approach and initial descent, use min in land final. As discussed with Leonard --- ArduPlane/quadplane.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3634c2633fb..4a484708350 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3608,7 +3608,17 @@ void QuadPlane::update_throttle_mix(void) // check for requested descent bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f; - if (large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { + bool use_mix_max = large_angle_request || large_angle_error || accel_moving || descent_not_demanded; + + /* + special case for auto landing, we want a high degree of + attitude control until LAND_FINAL + */ + if (in_vtol_land_sequence()) { + use_mix_max = !in_vtol_land_final(); + } + + if (use_mix_max) { attitude_control->set_throttle_mix_max(1.0); } else { attitude_control->set_throttle_mix_min(); From 3772029450ff7ca77c409747683062d8aac46cf0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Mar 2022 11:06:29 +1100 Subject: [PATCH 0336/3101] Plane: fixed pitch envelope after AIRBRAKE we need to setup last_fw_mode_ms and last_fw_nav_pitch_cd when we enter POSITION1 mode so that the expanding envelope pitch limit from Q_BACKTRANS_MS is applied correctly --- ArduPlane/quadplane.cpp | 50 +++++++++++++++++++++++++++-------------- ArduPlane/transition.h | 4 ++++ 2 files changed, 37 insertions(+), 17 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4a484708350..47326d88a34 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1633,16 +1633,14 @@ void SLT_Transition::update() quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); motors->output(); } - last_fw_mode_ms = now; - last_fw_nav_pitch_cd = plane.nav_pitch_cd; + set_last_fw_pitch(); in_forced_transition = false; return; } quadplane.motors_output(); - last_fw_mode_ms = now; - last_fw_nav_pitch_cd = plane.nav_pitch_cd; + set_last_fw_pitch(); } void SLT_Transition::VTOL_update() @@ -2130,8 +2128,13 @@ void QuadPlane::poscontrol_init_approach(void) if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist); poscontrol.set_state(QPOS_POSITION1); + transition->set_last_fw_pitch(); } else { - gcs().send_text(MAV_SEVERITY_INFO,"VTOL short d=%.1f", dist); + gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f", + plane.ahrs.groundspeed(), + dist, + stopping_distance(), + plane.relative_ground_altitude(plane.g.rangefinder_landing)); poscontrol.set_state(QPOS_AIRBRAKE); } } else { @@ -2317,6 +2320,7 @@ void QuadPlane::vtol_position_controller(void) stop_distance, plane.relative_ground_altitude(plane.g.rangefinder_landing)); poscontrol.set_state(QPOS_POSITION1); + transition->set_last_fw_pitch(); } else { gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f", groundspeed, @@ -2351,6 +2355,7 @@ void QuadPlane::vtol_position_controller(void) plane.relative_ground_altitude(plane.g.rangefinder_landing), desired_closing_speed); poscontrol.set_state(QPOS_POSITION1); + transition->set_last_fw_pitch(); // switch to vfwd for throttle control vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); @@ -2374,6 +2379,7 @@ void QuadPlane::vtol_position_controller(void) gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f", aspeed, aspeed_threshold); poscontrol.set_state(QPOS_POSITION1); + transition->set_last_fw_pitch(); } } else { poscontrol.thrust_loss_start_ms = 0; @@ -2385,6 +2391,7 @@ void QuadPlane::vtol_position_controller(void) gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 low speed as=%.1f at=%.1f", aspeed, aspeed_threshold); poscontrol.set_state(QPOS_POSITION1); + transition->set_last_fw_pitch(); } } @@ -3871,18 +3878,19 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ // disabled return false; } - uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms; + const uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms; - const uint32_t now = AP_HAL::millis(); - if (now - last_fw_mode_ms > limit_time_ms) { + const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms; + if (last_fw_mode_ms == 0 || dt > limit_time_ms) { + last_fw_mode_ms = 0; // past transition period, nothing to do return false; } // we limit pitch during initial transition - float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd), - now, - last_fw_mode_ms, last_fw_mode_ms+limit_time_ms); + const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd), + dt, + 0, limit_time_ms); if (pitch_cd > max_limit_cd) { pitch_cd = max_limit_cd; @@ -3900,9 +3908,9 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ to prevent inability to progress to position if moving from a loiter to landing */ - float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd), - now, - last_fw_mode_ms, last_fw_mode_ms+limit_time_ms); + const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd), + dt, + 0, limit_time_ms); if (plane.nav_pitch_cd < min_limit_cd) { plane.nav_pitch_cd = min_limit_cd; @@ -3912,14 +3920,22 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ return false; } +/* + remember last fixed wing pitch for pitch envelope in back transition + */ +void SLT_Transition::set_last_fw_pitch() +{ + last_fw_mode_ms = AP_HAL::millis(); + last_fw_nav_pitch_cd = plane.nav_pitch_cd; +} + void SLT_Transition::force_transistion_complete() { transition_state = TRANSITION_DONE; in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; - last_fw_mode_ms = AP_HAL::millis(); - last_fw_nav_pitch_cd = plane.nav_pitch_cd; -}; + set_last_fw_pitch(); +} MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const { diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h index 30c91d9615b..925dbb69727 100644 --- a/ArduPlane/transition.h +++ b/ArduPlane/transition.h @@ -54,6 +54,8 @@ class Transition virtual bool allow_weathervane() { return true; } + virtual void set_last_fw_pitch(void) {} + protected: // refences for convenience @@ -93,6 +95,8 @@ class SLT_Transition : public Transition bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) override; + void set_last_fw_pitch(void) override; + protected: enum { From cc6fc2b13066acabd70fc777151446babd2aea69 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Mar 2022 13:52:06 +1100 Subject: [PATCH 0337/3101] Plane: improvements to POSITION1 controller this improves 4 things in the POSITION1 controller based on logs from 4.2.0beta2. The changes are designed to increase the tolerance to an incorrect value for Q_TRANS_DECEL, reducing landing overshoot 1) we fix the initialisation of the acceleration. The init_xy_controller() function assumes zero accel, so we need to call set_accel_desired_xy_cmss() just after that init to get the correct accel. Thanks to Leonard for this fix 2) if we decel more than expected due to too low Q_TRANS_DECEL we need to reduce the target speed, rather than putting the nose down 3) lower the default Q_P_JERK_XY to a value more appropriate for most quadplanes (Leonard suggested a value of 2) 4) fixed the pitch envelope from Q_BACKTRANS_MS to start after the airbrake phase is complete --- ArduPlane/quadplane.cpp | 36 +++++++++++++++++++++++++++++++----- ArduPlane/quadplane.h | 3 ++- 2 files changed, 33 insertions(+), 6 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 47326d88a34..76f0b8f9aab 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -470,6 +470,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { { "Q_LOIT_SPEED", 500 }, { "Q_WP_SPEED", 500 }, { "Q_WP_ACCEL", 100 }, + { "Q_P_JERK_XY", 2 }, }; /* @@ -2175,7 +2176,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) // never do a rate reset, if attitude control is not active it will be automaticaly reset before running, see: last_att_control_ms // if it is active then the rate control should not be reset at all qp.attitude_control->reset_yaw_target_and_rate(false); - pos1_start_speed = plane.ahrs.groundspeed_vector().length(); + pos1_speed_limit = plane.ahrs.groundspeed_vector().length(); + done_accel_init = false; } else if (s == QPOS_AIRBRAKE) { // start with zero integrator on vertical throttle qp.pos_control->get_accel_z_pid().set_integrator(0); @@ -2422,11 +2424,13 @@ void QuadPlane::vtol_position_controller(void) float target_speed = stopping_speed; // maximum configured VTOL speed - const float wp_speed = wp_nav->get_default_speed_xy() * 0.01; + const float wp_speed = MAX(1.0, wp_nav->get_default_speed_xy() * 0.01); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); - // limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED - target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed); + // limit target speed to a the pos1 speed limit, which starts out at the initial speed + // but is adjusted if we start putting our nose down. We always allow at least twice + // the WP speed + target_speed = MIN(MAX(poscontrol.pos1_speed_limit, 2*wp_speed), target_speed); if (poscontrol.reached_wp_speed || rel_groundspeed_sq < sq(wp_speed) || @@ -2452,17 +2456,39 @@ void QuadPlane::vtol_position_controller(void) target_speed_xy_cms = diff_wp_norm * target_speed * 100; target_accel_cms = diff_wp_norm * (-target_accel*100); } + const float target_speed_ms = target_speed_xy_cms.length() * 0.01; target_speed_xy_cms += landing_velocity * 100; - poscontrol.target_speed = target_speed_xy_cms.length()*0.01; + poscontrol.target_speed = target_speed_ms; poscontrol.target_accel = target_accel; + if (!poscontrol.reached_wp_speed && + rel_groundspeed_sq < sq(target_speed_ms) && + rel_groundspeed_sq > sq(2*wp_speed) && + plane.nav_pitch_cd < 0) { + // we have slowed down more than expected, likely due to + // drag from the props and we're starting to put our nose + // down as a result. We want to accept the slowdown and + // re-calculate the target speed profile + poscontrol.pos1_speed_limit = sqrtf(rel_groundspeed_sq); + } + // use input shaping and abide by accel and jerk limits pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms); // run horizontal velocity controller run_xy_controller(MAX(target_accel, transition_decel)*1.5); + if (!poscontrol.done_accel_init) { + /* + the pos controller init assumes zero accel, we need to + override that so that we can start decelerating more + quickly at the start of POSITION1 + */ + poscontrol.done_accel_init = true; + pos_control->set_accel_desired_xy_cmss(target_accel_cms); + } + // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 68b3ad50350..33bbc2296b7 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -447,7 +447,8 @@ class QuadPlane uint32_t last_log_ms; bool reached_wp_speed; uint32_t last_run_ms; - float pos1_start_speed; + float pos1_speed_limit; + bool done_accel_init; Vector2f velocity_match; uint32_t last_velocity_match_ms; float target_speed; From ca4151be2c2e96a9a873b17ba37fb6cc15d6312a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:25:18 +1100 Subject: [PATCH 0338/3101] AP_Math: added angle_to_accel() and accel_to_angle() --- libraries/AP_Math/control.cpp | 16 ++++++++++++++++ libraries/AP_Math/control.h | 10 ++++++++++ 2 files changed, 26 insertions(+) diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp index 54431e5d6cb..03adce568b6 100644 --- a/libraries/AP_Math/control.cpp +++ b/libraries/AP_Math/control.cpp @@ -507,3 +507,19 @@ float input_expo(float input, float expo) } return input; } + +/* + convert a maximum lean angle in degrees to an accel limit in m/s/s + */ +float angle_to_accel(float angle_deg) +{ + return GRAVITY_MSS * tanf(radians(angle_deg)); +} + +/* + convert a maximum accel in m/s/s to a lean angle in degrees + */ +float accel_to_angle(float accel) +{ + return degrees(atanf((accel/GRAVITY_MSS))); +} diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h index 3ef5b0321b5..ecec883200c 100644 --- a/libraries/AP_Math/control.h +++ b/libraries/AP_Math/control.h @@ -145,3 +145,13 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m // The input must be in the range of -1 to 1. // The expo should be less than 1.0 but limited to be less than 0.95. float input_expo(float input, float expo); + +/* + convert a maximum lean angle in degrees to an accel limit in m/s/s + */ +float angle_to_accel(float angle_deg); + +/* + convert a maximum accel in m/s/s to a lean angle in degrees + */ +float accel_to_angle(float accel); From f824be726c4cc32e764bf0099c460e5b9565fd50 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:25:36 +1100 Subject: [PATCH 0339/3101] AC_WPNav: use angle/accel functions --- libraries/AC_WPNav/AC_Loiter.cpp | 2 +- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index edfd47ada61..0e66fdfe44f 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -211,7 +211,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on) float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f); gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN); - float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f)); + float pilot_acceleration_max = angle_to_accel(get_angle_max_cd()*0.01) * 100; // range check nav_dt if (nav_dt < 0) { diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 032a013d453..fb30863f56b 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -150,7 +150,7 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) // sanity check parameters // check _wp_accel_cmss is reasonable - _scurve_accel_corner = GRAVITY_MSS * 100.0f * tanf(ToRad(_pos_control.get_lean_angle_max_cd() * 0.01f)); + _scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100; const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner); _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss); From 7dcdeac7ab66bbe0b8aa3d3dde5bd6faafdffb1a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:25:53 +1100 Subject: [PATCH 0340/3101] AC_Autorotation: use accel_to_angle() --- libraries/AC_Autorotation/AC_Autorotation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 3e74e80ac1d..0018a06be73 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -366,7 +366,7 @@ void AC_Autorotation::update_forward_speed_controller(void) _accel_out_last = _accel_out; // update angle targets that will be passed to stabilize controller - _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI); + _pitch_target = accel_to_angle(-_accel_out*0.01) * 100; } From 7bb129aa3ee055a5e0f25192bbda935a039e2bcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:26:18 +1100 Subject: [PATCH 0341/3101] AC_AttitudeControl: added set_lean_angle_max_cd() --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 12 ++++++++---- libraries/AC_AttitudeControl/AC_PosControl.h | 9 +++++++++ 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index ec9c84922b3..8cda28b862c 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -483,6 +483,7 @@ void AC_PosControl::init_xy_controller() _pitch_target = att_target_euler_cd.y; _yaw_target = att_target_euler_cd.z; // todo: this should be thrust vector heading, not yaw. _yaw_rate_target = 0.0f; + _angle_max_override_cd = 0.0; _pos_target.xy() = _inav.get_position_xy_cm().topostype(); @@ -497,7 +498,7 @@ void AC_PosControl::init_xy_controller() // limit acceleration using maximum lean angles float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd()); - float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)); + float accel_max = angle_to_accel(angle_max * 0.01) * 100.0; _accel_target.xy().limit_length(accel_max); // initialise I terms from lean angles @@ -634,7 +635,7 @@ void AC_PosControl::update_xy_controller() // limit acceleration using maximum lean angles float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd()); - float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)); + float accel_max = angle_to_accel(angle_max * 0.01) * 100; // Define the limit vector before we constrain _accel_target _limit_vector.xy() = _accel_target.xy(); if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) { @@ -986,6 +987,9 @@ void AC_PosControl::update_z_controller() /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request float AC_PosControl::get_lean_angle_max_cd() const { + if (is_positive(_angle_max_override_cd)) { + return _angle_max_override_cd; + } if (!is_positive(_lean_angle_max)) { return _attitude_control.lean_angle_max_cd(); } @@ -1166,9 +1170,9 @@ void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, const float accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller - pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI); + pitch_target = accel_to_angle(-accel_forward * 0.01) * 100; float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f); - roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI); + roll_target = accel_to_angle((accel_right * cos_pitch_target)*0.01) * 100; } // lean_angles_to_accel_xy - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 7ba799ff657..29c4a6f62ca 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -343,6 +343,12 @@ class AC_PosControl /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request float get_lean_angle_max_cd() const; + /* + set_lean_angle_max_cd - set the maximum lean angle. A value of zero means to use the ANGLE_MAX parameter. + This is reset to zero on init_xy_controller() + */ + void set_lean_angle_max_cd(float angle_max_cd) { _angle_max_override_cd = angle_max_cd; } + /// Other @@ -466,6 +472,9 @@ class AC_PosControl // high vibration handling bool _vibe_comp_enabled; // true when high vibration compensation is on + // angle max override, if zero then use ANGLE_MAX parameter + float _angle_max_override_cd; + // return true if on a real vehicle or SITL with lock-step scheduling bool has_good_timing(void) const; }; From eabdaae1e7da9169b5b4da5317a1679906da3565 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 15 Mar 2022 09:31:13 +1100 Subject: [PATCH 0342/3101] Plane: use set_lean_angle_max_cd() allows for a wider range of Q_TRANS_DECEL while landing on the desired landing point --- ArduPlane/quadplane.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 76f0b8f9aab..a696ecb5bf7 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2109,6 +2109,7 @@ void QuadPlane::run_xy_controller(float accel_limit) if (!pos_control->is_active_xy()) { pos_control->init_xy_controller(); } + pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_to_angle(accel_limit)*100, aparm.angle_max))); pos_control->update_xy_controller(); } @@ -3159,6 +3160,7 @@ bool QuadPlane::verify_vtol_land(void) (vel_ned.xy() - target_vel).length() < descend_speed_threshold) { poscontrol.set_state(QPOS_LAND_DESCEND); poscontrol.pilot_correction_done = false; + pos_control->set_lean_angle_max_cd(0); poscontrol.xy_correction.zero(); #if AC_FENCE == ENABLED plane.fence.auto_disable_fence_for_landing(); @@ -3909,7 +3911,9 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms; if (last_fw_mode_ms == 0 || dt > limit_time_ms) { last_fw_mode_ms = 0; - // past transition period, nothing to do + // past transition period, only constrain roll + int16_t limit_cd = MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd); + roll_cd = constrain_int32(roll_cd, -limit_cd, limit_cd); return false; } @@ -4001,6 +4005,9 @@ bool QuadPlane::in_vtol_takeoff(void) const // called when we change mode (for any mode, not just Q modes) void QuadPlane::mode_enter(void) { + if (available()) { + pos_control->set_lean_angle_max_cd(0); + } poscontrol.xy_correction.zero(); poscontrol.velocity_match.zero(); poscontrol.last_velocity_match_ms = 0; From 6ebefbdb1680b61efc30d74ebcd95861f8adf02a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 08:12:20 +1100 Subject: [PATCH 0343/3101] Plane: added airspeed based pitch limit check prevent using too much pitch when at low airspeed, which can lead to severe instability in SLT quadplanes --- ArduPlane/quadplane.cpp | 68 +++++++++++++++++++++++++++++++++++------ 1 file changed, 59 insertions(+), 9 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a696ecb5bf7..819fe2b6ba8 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3900,25 +3900,75 @@ bool SLT_Transition::active() const return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER)); } +/* + limit VTOL roll/pitch in POSITION1, POSITION2 and waypoint controller. This serves three roles: + 1) an expanding envelope limit on pitch to prevent sudden pitch at the start of a back transition + + 2) limiting roll and pitch down to the Q_ANGLE_MAX, as the accel limits may push us beyond that for pitch up. + This is needed as the position controller doesn't have separate limits for pitch and roll + + 3) preventing us pitching up a lot when our airspeed may be low + enough that the real airspeed may be negative, which would result + in reversed control surfaces + */ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_cd) { + bool ret = false; + const int16_t angle_max = quadplane.aparm.angle_max; + + /* + we always limit roll to Q_ANGLE_MAX + */ + int32_t new_roll_cd = constrain_int32(roll_cd, -angle_max, angle_max); + if (new_roll_cd != roll_cd) { + roll_cd = new_roll_cd; + ret = true; + } + + /* + always limit pitch down to Q_ANGLE_MAX. We need to do this as + the position controller accel limits may exceed this limit + */ + if (pitch_cd < -angle_max) { + pitch_cd = -angle_max; + ret = true; + } + + /* + prevent trying to fly backwards (negative airspeed) at high + pitch angles, which can result in a high degree of instability + in SLT aircraft. This can happen with a tailwind in a back + transition, where the position controller (which is unaware of + airspeed) demands high pitch to hit the desired landing point + */ + float airspeed; + if (pitch_cd > angle_max && + plane.ahrs.airspeed_estimate(airspeed) && airspeed < 0.5 * plane.aparm.airspeed_min) { + const float max_limit_cd = linear_interpolate(angle_max, 4500, + airspeed, + 0, 0.5 * plane.aparm.airspeed_min); + if (pitch_cd > max_limit_cd) { + pitch_cd = max_limit_cd; + ret = true; + } + } + if (quadplane.back_trans_pitch_limit_ms <= 0) { - // disabled - return false; + // time based pitch envelope disabled + return ret; } + const uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms; const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms; if (last_fw_mode_ms == 0 || dt > limit_time_ms) { + // we are beyond the time limit, don't apply envelope last_fw_mode_ms = 0; - // past transition period, only constrain roll - int16_t limit_cd = MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd); - roll_cd = constrain_int32(roll_cd, -limit_cd, limit_cd); - return false; + return ret; } // we limit pitch during initial transition - const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd), + const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max_cd), dt, 0, limit_time_ms); @@ -3938,7 +3988,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ to prevent inability to progress to position if moving from a loiter to landing */ - const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd), + const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min_cd), dt, 0, limit_time_ms); @@ -3947,7 +3997,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ return true; } - return false; + return ret; } /* From c48b7319bc848ca28d88770dbc16543a27c2e7fa Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 11 Mar 2022 19:03:13 -0600 Subject: [PATCH 0344/3101] AP_RSSI: convert floating point divides into multiplys --- libraries/AP_RSSI/AP_RSSI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_RSSI/AP_RSSI.cpp b/libraries/AP_RSSI/AP_RSSI.cpp index 105da81a59a..594317fe8b2 100644 --- a/libraries/AP_RSSI/AP_RSSI.cpp +++ b/libraries/AP_RSSI/AP_RSSI.cpp @@ -141,7 +141,7 @@ float AP_RSSI::read_receiver_rssi() case RssiType::RECEIVER: { int16_t rssi = RC_Channels::get_receiver_rssi(); if (rssi != -1) { - return rssi / 255.0; + return rssi * (1/255.0); } return 0.0f; } From 14700063d248fb4cdecedb815d4f6388f848faae Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 11 Mar 2022 19:03:13 -0600 Subject: [PATCH 0345/3101] ArduPlane: convert floating point divides into multiplys --- ArduPlane/navigation.cpp | 2 +- ArduPlane/quadplane.cpp | 6 +++--- ArduPlane/servos.cpp | 2 +- ArduPlane/system.cpp | 2 +- ArduPlane/tiltrotor.cpp | 26 +++++++++++++------------- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 1f2c8b744e3..fadfcb62a6a 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -380,7 +380,7 @@ void Plane::update_fbwb_speed_height(void) target_altitude.last_elev_check_us = now; - float elevator_input = channel_pitch->get_control_in() / 4500.0f; + float elevator_input = channel_pitch->get_control_in() * (1/4500.0); if (g.flybywire_elev_reverse) { elevator_input = -elevator_input; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 819fe2b6ba8..37adb002012 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1202,7 +1202,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return plane.channel_rudder->get_control_in() * max_rate / 45; + return plane.channel_rudder->get_control_in() * max_rate * (1/45.0); } /* @@ -3206,7 +3206,7 @@ void QuadPlane::Log_Write_QControl_Tuning() float des_alt_m = 0.0f; int16_t target_climb_rate_cms = 0; if (plane.control_mode != &plane.mode_qstabilize) { - des_alt_m = pos_control->get_pos_target_z_cm() / 100.0f; + des_alt_m = pos_control->get_pos_target_z_cm() * 0.01f; target_climb_rate_cms = pos_control->get_vel_target_z_cms(); } @@ -3391,7 +3391,7 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) wp_nav->get_pitch(), is_takeoff, in_vtol_land_sequence())) { - return constrain_float(wv_output / 45, -100.0, 100.0) * yaw_rate_max * 0.5; + return constrain_float(wv_output * (1/45.0), -100.0, 100.0) * yaw_rate_max * 0.5; } return 0.0; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 2e52219587e..9511c188532 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -710,7 +710,7 @@ void Plane::set_landing_gear(void) void Plane::servos_twin_engine_mix(void) { float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); - float rud_gain = float(plane.g2.rudd_dt_gain) / 100; + float rud_gain = float(plane.g2.rudd_dt_gain) * 0.01f; rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / SERVO_MAX; #if ADVANCED_FAILSAFE == ENABLED diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 205dda44459..1c4570e3c06 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -474,7 +474,7 @@ void Plane::update_dynamic_notch() float rpm; if (rpm_sensor.get_rpm(0, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref / 60.0f)); + ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref * (1/60.0))); } else { ins.update_harmonic_notch_freq_hz(ref_freq); } diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 3f0660d1660..fc8561f4c51 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -175,7 +175,7 @@ float Tiltrotor::tilt_max_change(bool up, bool in_flap_range) const rate = MAX(rate, 90); } } - return rate * plane.G_Dt / 90.0f; + return rate * plane.G_Dt * (1/90.0); } /* @@ -196,13 +196,13 @@ void Tiltrotor::slew(float newtilt) // tilt wings can sustain forward flight with some amount of wing tilt float Tiltrotor::get_fully_forward_tilt() const { - return 1.0 - (flap_angle_deg / 90.0); + return 1.0 - (flap_angle_deg * (1/90.0)); } // return the target tilt value for forward flight float Tiltrotor::get_forward_flight_tilt() const { - return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01); + return 1.0 - ((flap_angle_deg * (1/90.0)) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01); } /* @@ -305,8 +305,8 @@ void Tiltrotor::continuous_update(void) // Q_TILT_MAX. Anything above 50% throttle gets // Q_TILT_MAX. Below 50% throttle we decrease linearly. This // relies heavily on Q_VFWD_GAIN being set appropriately. - float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) / 50.0f, 0, 1); - slew(MIN(settilt * max_angle_deg / 90.0f, get_forward_flight_tilt())); + float settilt = constrain_float((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)-MAX(plane.aparm.throttle_min.get(),0)) * 0.02, 0, 1); + slew(MIN(settilt * max_angle_deg * (1/90.0), get_forward_flight_tilt())); } } @@ -527,9 +527,9 @@ void Tiltrotor::vectoring(void) // base the tilt on elevon mixing, which means it // takes account of the MIXING_GAIN. The rear tilt is // based on elevator - const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0; - const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0; - const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0; + const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0); + const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0); + const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0); // front tilt is effective canards, so need to swap and use negative. Rear motors are treated live elevons. SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1)); @@ -547,9 +547,9 @@ void Tiltrotor::vectoring(void) // we don't want tilt impacted by airspeed const float scaler = plane.control_mode == &plane.mode_manual?1:(quadplane.FW_vector_throttle_scaling() / plane.get_speed_scaler()); const float gain = fixed_gain * fixed_tilt_limit * scaler; - const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) / 4500.0; - const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) / 4500.0; - const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0; + const float right = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_right) * (1/4500.0); + const float left = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevon_left) * (1/4500.0); + const float mid = gain * SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (1/4500.0); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft,1000 * constrain_float(base_output - right,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight,1000 * constrain_float(base_output - left,0,1)); SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRearLeft,1000 * constrain_float(base_output + left,0,1)); @@ -607,10 +607,10 @@ void Tiltrotor::bicopter_output(void) float tilt_right = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorRight); if (is_negative(tilt_left)) { - tilt_left *= tilt_yaw_angle / 90.0f; + tilt_left *= tilt_yaw_angle * (1/90.0); } if (is_negative(tilt_right)) { - tilt_right *= tilt_yaw_angle / 90.0f; + tilt_right *= tilt_yaw_angle * (1/90.0); } // reduce authority of bicopter as motors are tilted forwards From 2c22cbc50e6e35921ad3581837adc8ef9172cc7d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 20:29:33 +1100 Subject: [PATCH 0346/3101] Plane: update release notes for 4.2.0beta3 --- ArduPlane/ReleaseNotes.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index a79485d870f..8db9c6e89f4 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,20 @@ +Release 4.2.0beta3 18th March 2022 +---------------------------------- + +This is the 3rd beta of the 4.2.0 major release. This beta should be +close to the final 4.2.0 release. + + - fixed pitch envelope constraint after AIRBRAKE + - improvements to POSITION1 quadplane landing controller + - added arming check for Q_ASSIST_SPEED + - added warning if arming with ARMING_CHECK=0 + - added arming check for DO_LAND_START with RTL_AUTOLAND=0 + - improved throttle mix for quadplane autoland + - added fence breach message on GCS + - constrain indexing on declination tables + +Happy flying! + Release 4.2.0beta2 10th March 2022 ---------------------------------- From 116c692e2b835ab8fc086dddb6f9e8d44cde9514 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 18:02:49 +1100 Subject: [PATCH 0347/3101] AP_Periph: rename ReleaseNotes.txt be consistent with plane and copter --- Tools/AP_Periph/{release-notes.txt => ReleaseNotes.txt} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename Tools/AP_Periph/{release-notes.txt => ReleaseNotes.txt} (100%) diff --git a/Tools/AP_Periph/release-notes.txt b/Tools/AP_Periph/ReleaseNotes.txt similarity index 100% rename from Tools/AP_Periph/release-notes.txt rename to Tools/AP_Periph/ReleaseNotes.txt From 59456b4f8278a4e4ce9f8bffc121379fbadb2e89 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 18:17:55 +1100 Subject: [PATCH 0348/3101] AP_Periph: updated release notes for 1.3.0 --- Tools/AP_Periph/ReleaseNotes.txt | 38 ++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 455b4efabbd..3f317063e3a 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,41 @@ +Release 1.3.0 18th Mar 2022 +--------------------------- + +This is a major release with several significant bug fixes and +improvements: + + - added new peripherals: BirdCANdy, MatekL431, CubeOrange-periph, + G4-ESC, HerePro, Hitec-Airspeed, HolybroG4GPS, HolybroGPS, + Sierra-F405, Sierra-F421, Sierra-F9P, Sierra-L431, + f103-QiotekPeriph, f405-MatekGPS, f405-MatekAirspeed, mRo-M10095, + ARK_GPS, HitecMosaic, MatekH743-periph, Pixracer-periph + + - support dshot for CAN ESC outputs + + - support a wider range of notify options + + - numerous small bug fixes + + - support lua scripting in peripherals + + - switched to DroneCAN compiler and libraries + + - support logging in peripherals + + - support dual CAN bus + + - support BLHeli monitoring of ESC telemetry + + - support mavlink in peripherals + + - support moving baseline yaw dual-GPS on dual-CAN GPS + + - support MPPT battery driver + + - fixed MSP GPS yaw + +Note that the next major release will add CANFD support. + Release 1.2.0 6th Jan 2020 -------------------------- From 994e1e0c0aedab59ebcfb342df4d0c9139dd6a0f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 21:16:13 +1100 Subject: [PATCH 0349/3101] AP_Periph: mark version as 1.4.0dev --- Tools/AP_Periph/version.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 3d0f687fbbf..b105e3cbc97 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -1,12 +1,12 @@ #pragma once -#define THISFIRMWARE "AP_Periph V1.3dev" +#define THISFIRMWARE "AP_Periph V1.4dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,3,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,4,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 1 -#define FW_MINOR 3 +#define FW_MINOR 4 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV From 35ed87a5db041495c727b855e6652eaf1b4e6fa8 Mon Sep 17 00:00:00 2001 From: mateksys Date: Sun, 27 Feb 2022 17:15:06 +0800 Subject: [PATCH 0350/3101] AP_HAL_ChibiOS: add MatekL431 AP_Periph hwdef --- .../hwdef/MatekL431-Airspeed/hwdef-bl.dat | 3 + .../hwdef/MatekL431-Airspeed/hwdef.dat | 15 +++ .../hwdef/MatekL431-Periph/hwdef-bl.dat | 3 + .../hwdef/MatekL431-Periph/hwdef.dat | 85 +++++++++++++++ .../hwdef/MatekL431/hwdef-bl.inc | 83 ++++++++++++++ .../AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc | 103 ++++++++++++++++++ 6 files changed, 292 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef-bl.inc create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef-bl.dat new file mode 100644 index 00000000000..887b18aeacc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef-bl.dat @@ -0,0 +1,3 @@ +include ../MatekL431/hwdef-bl.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Airspeed" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef.dat new file mode 100644 index 00000000000..302d5c8d88f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef.dat @@ -0,0 +1,15 @@ +include ../MatekL431/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Airspeed" + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE + +# ------------------ I2C airspeed ------------------------- +define HAL_PERIPH_ENABLE_AIRSPEED + +# 10" DLVR sensor by default +define HAL_AIRSPEED_TYPE_DEFAULT 9 +define AIRSPEED_MAX_SENSORS 1 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef-bl.dat new file mode 100644 index 00000000000..dd6b5cc8873 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef-bl.dat @@ -0,0 +1,3 @@ +include ../MatekL431/hwdef-bl.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Periph" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat new file mode 100644 index 00000000000..ace89f7764c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Periph/hwdef.dat @@ -0,0 +1,85 @@ +include ../MatekL431/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Periph" + + +# --------------------- PWM ----------------------- +PA8 TIM1_CH1 TIM1 PWM(1) GPIO(50) +PA9 TIM1_CH2 TIM1 PWM(2) GPIO(51) +PA10 TIM1_CH3 TIM1 PWM(3) GPIO(52) +PA11 TIM1_CH4 TIM1 PWM(4) GPIO(53) +PA15 TIM2_CH1 TIM2 PWM(5) GPIO(54) + +# Beeper +PA6 TIM16_CH1 TIM16 GPIO(32) ALARM + +# ----------------------- GPS ---------------------------- +define HAL_PERIPH_ENABLE_GPS +define GPS_MAX_RATE_MS 200 + +define GPS_MAX_RECEIVERS 1 +define GPS_MAX_INSTANCES 1 + +define HAL_PERIPH_GPS_PORT_DEFAULT 2 + + +# ---------------------- COMPASS --------------------------- +define HAL_PERIPH_ENABLE_MAG + +SPIDEV rm3100 SPI1 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ +COMPASS RM3100 SPI:rm3100 false ROTATION_PITCH_180 + +define HAL_COMPASS_MAX_SENSORS 1 + +# added QMC5883L for different board varients +COMPASS QMC5883L I2C:0:0xd false ROTATION_PITCH_180_YAW_90 + + +# --------------------- Barometer --------------------------- +define HAL_PERIPH_ENABLE_BARO +define HAL_BARO_ALLOW_INIT_NO_BARO + +BARO SPL06 I2C:0:0x76 + +# ------------------ I2C airspeed ------------------------- +define HAL_PERIPH_ENABLE_AIRSPEED + +# MS4525 sensor by default +define HAL_AIRSPEED_TYPE_DEFAULT 1 +define AIRSPEED_MAX_SENSORS 1 + +# -------------------- MSP -------------------------------- +define HAL_PERIPH_ENABLE_MSP +define HAL_MSP_ENABLED 1 +define AP_PERIPH_MSP_PORT_DEFAULT 1 + +# ------------------ BATTERY Monitor ----------------------- +define HAL_PERIPH_ENABLE_BATTERY + +define HAL_USE_ADC TRUE +define STM32_ADC_USE_ADC1 TRUE + +PA0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PA1 BATT_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 0 +define HAL_BATT_VOLT_PIN 5 +define HAL_BATT_VOLT_SCALE 21.0 +define HAL_BATT_CURR_PIN 6 +define HAL_BATT_CURR_SCALE 40.0 + + +PB0 BATT2_VOLTAGE_SENS ADC1 SCALE(1) +PB1 BATT2_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT2_MONITOR_DEFAULT 0 +define HAL_BATT2_VOLT_PIN 15 +define HAL_BATT2_VOLT_SCALE 21.0 +define HAL_BATT2_CURR_PIN 16 +define HAL_BATT2_CURR_SCALE 40.0 + + +# -------------------- Buzzer+NeoPixels --------------d------ +define HAL_PERIPH_ENABLE_RC_OUT +define HAL_PERIPH_ENABLE_NOTIFY + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef-bl.inc b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef-bl.inc new file mode 100644 index 00000000000..1ef579243f5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef-bl.inc @@ -0,0 +1,83 @@ +# hw definition file Matek L431 CAN node + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# crystal frequency +OSCILLATOR_HZ 8000000 + +# board ID for firmware load +APJ_BOARD_ID 1062 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 36 +FLASH_SIZE_KB 256 + +# reserve some space for params +APP_START_OFFSET_KB 4 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +# --------------------------------------------- +PB3 LED_BOOTLOADER OUTPUT LOW # blue +define HAL_LED_ON 0 + +# order of UARTs +SERIAL_ORDER USART1 USART2 USART3 + +PB6 USART1_TX USART1 NODMA +PB7 USART1_RX USART1 NODMA + +PA2 USART2_TX USART2 NODMA +PA3 USART2_RX USART2 NODMA + +PB10 USART3_TX USART3 NODMA +PB11 USART3_RX USART3 NODMA + +define HAL_USE_SERIAL TRUE +define STM32_SERIAL_USE_USART1 TRUE +define STM32_SERIAL_USE_USART2 TRUE +define STM32_SERIAL_USE_USART3 TRUE + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +# PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +CAN_ORDER 1 + +# --------------------------------------------- + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a small bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 1000 + +define HAL_USE_SERIAL TRUE +define STM32_SERIAL_USE_USART1 TRUE +define STM32_SERIAL_USE_USART2 TRUE + +define HAL_NO_GPIO_IRQ +define SERIAL_BUFFERS_SIZE 32 +define HAL_USE_EMPTY_IO TRUE +define PORT_INT_REQUIRED_STACK 64 +define DMA_RESERVE_SIZE 0 + +MAIN_STACK 0x800 +PROCESS_STACK 0x800 + +define HAL_DISABLE_LOOP_DELAY + +# Add CS pins to ensure they are high in bootloader +PA4 MAG_CS CS +PB2 SPARE_CS CS + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc new file mode 100644 index 00000000000..82492ac3fca --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc @@ -0,0 +1,103 @@ +# hw definition file for Matek L431 CAN node + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# bootloader starts firmware at 36k + 4k (STORAGE_FLASH) +FLASH_RESERVE_START_KB 40 +FLASH_SIZE_KB 256 + +# store parameters in pages 18 and 19 +STORAGE_FLASH_PAGE 18 +define HAL_STORAGE_SIZE 800 + +# ChibiOS system timer +STM32_ST_USE_TIMER 15 +define CH_CFG_ST_RESOLUTION 16 + +# board ID for firmware load +APJ_BOARD_ID 1062 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +env AP_PERIPH 1 + +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 57600 + +define HAL_NO_GPIO_IRQ +define SERIAL_BUFFERS_SIZE 512 +define PORT_INT_REQUIRED_STACK 64 + +define DMA_RESERVE_SIZE 0 + +# MAIN_STACK is stack for ISR handlers +MAIN_STACK 0x300 + +# PROCESS_STACK controls stack for main thread +PROCESS_STACK 0xA00 + +define HAL_DISABLE_LOOP_DELAY + +define HAL_GCS_ENABLED 0 +define HAL_NO_MONITOR_THREAD +define HAL_NO_LOGGING +define HAL_MINIMIZE_FEATURES 0 + +# we setup a small defaults.parm +define AP_PARAM_MAX_EMBEDDED_PARAM 512 + +# blue LED0 marked as ACT +PB3 LED OUTPUT HIGH +define HAL_LED_ON 1 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# --------------------- SPI1 RM3100 ----------------------- +PA5 SPI1_SCK SPI1 +PB4 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA4 MAG_CS CS +PB2 SPARE_CS CS + + +# ---------------------- I2C bus ------------------------ +I2C_ORDER I2C2 + +PB13 I2C2_SCL I2C2 +PB14 I2C2_SDA I2C2 + +define HAL_I2C_CLEAR_ON_TIMEOUT 0 +define HAL_I2C_INTERNAL_MASK 1 + + +# ---------------------- CAN bus ------------------------- +CAN_ORDER 1 + +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 +# PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW + +define HAL_CAN_POOL_SIZE 6000 + +# ---------------------- UARTs --------------------------- +# | sr0 | MSP | GPS | +SERIAL_ORDER USART1 USART2 USART3 + +# USART1 for debug +PB6 USART1_TX USART1 SPEED_HIGH +PB7 USART1_RX USART1 SPEED_HIGH + +# USART2 for MSP +PA2 USART2_TX USART2 SPEED_HIGH +PA3 USART2_RX USART2 SPEED_HIGH + +# USART3 for GPS +PB10 USART3_TX USART3 SPEED_HIGH NODMA +PB11 USART3_RX USART3 SPEED_HIGH NODMA + +# allow for reboot command for faster development +define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 From b3f7556ac2d1571b37bab7ba92ff1833dc4ebdc8 Mon Sep 17 00:00:00 2001 From: mateksys Date: Sun, 27 Feb 2022 17:19:09 +0800 Subject: [PATCH 0351/3101] Tools: add MatekL431 bootloader --- Tools/AP_Bootloader/board_types.txt | 1 + Tools/bootloaders/MatekL431-Airspeed_bl.bin | Bin 0 -> 21108 bytes Tools/bootloaders/MatekL431-Airspeed_bl.hex | 1322 +++++++++++++++++++ Tools/bootloaders/MatekL431-Periph_bl.bin | Bin 0 -> 21100 bytes Tools/bootloaders/MatekL431-Periph_bl.hex | 1321 ++++++++++++++++++ 5 files changed, 2644 insertions(+) create mode 100644 Tools/bootloaders/MatekL431-Airspeed_bl.bin create mode 100644 Tools/bootloaders/MatekL431-Airspeed_bl.hex create mode 100644 Tools/bootloaders/MatekL431-Periph_bl.bin create mode 100644 Tools/bootloaders/MatekL431-Periph_bl.hex diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 37a5149a405..7b3c0d7a6ba 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -160,6 +160,7 @@ AP_HW_KakuteH7Mini 1058 AP_HW_JHEMCUGSF405A 1059 AP_HW_SPRACINGH7 1060 AP_HW_DEVEBOXH7 1061 +AP_HW_MatekL431 1062 AP_HW_CUBEORANGE_PERIPH 1400 AP_HW_CUBEBLACK_PERIPH 1401 diff --git a/Tools/bootloaders/MatekL431-Airspeed_bl.bin b/Tools/bootloaders/MatekL431-Airspeed_bl.bin new file mode 100644 index 0000000000000000000000000000000000000000..defa6d416371edcd858eb4af21cd35b6383dbf0d GIT binary patch literal 21108 zcmd74dwf*Y^*6lFWoAw$Loy)&LlR&z6EH9VhXgeOKb=g5laK*|A}!UnP7BJ_zXtvvkp5lw*p1(he>d;n z!~4sK@E+V(a1&fa_%FDBhxzyT?rB75hx-_g=B@r8%G|&B|Gl1@0J8+H0`BzxP{+Tw z`+sv>lTHNkoxkO6Wo=}J;3K(NzH(We17{=hrnVy?!EjFx|KCK`DxWBP_a{B9g>R}Z znbjkg2>W^Q*CMel3G8(25j|Zkk!}6AH8T-*;GK|(F-4e=A;Jz9^>Rsj?B%^#rP+qJ zqso=3k;lVAbmAqX7i*)s&_0L0E31#=v?0~zm8aJ1bJ+W{YS^2roOfot*m5+g$U@Xu z;Oym`B&rL)6ock@5>gAivcBdeKU7bHw&9&lA@fv!y#ZS48w)gLiWrK)cT>f zSWLh;04eLD#uR5S<4lT@(cdqmkg;*hVbA=2A?ZKi6~q_&l6ZSBXMZO6$B;>P)NYRG zh8Op822(^g$V9XQS;a#XlZyz#&CwynWS~7sh^EJSAVibU12&@VPmJ|K2&wbD@(%9~ z;OG(Wp%5!nIOBa8k`v;6DI(FN(O<{LwzTUg<=_X_p{QFhPseM!vo zj`zjii$6HK-2ap2qkRHffixzf3!Bl8kbP<~aCIM<9>*)@p6j&Jna_@J1H0`^@l}i+ zjpN{kL_=|}wiqp6dDg*4*g=X3jzjB(*PVIRF$=9%IU6${!`z3j=%Nz~Kq*DqXwty0 zG3z_iGEqi@8{U{#>Kzbv+=OoezDutN zxBhoOVFb6mpOBB>mR^~AUp&P(Q2wNEVEvOc=VZT(oE>AZS98i`LjL)+QNQ%cg0V7M zlXoC(2Q9_R1w*`?47X^gTwYlu?%HYH-}_yA`<( zI3-9-*DOl^4h%1UrSS5Wp|r(mOI~$w7j9Sh%pD~|K_V9^DT`CSm&VKIkAne#Pm>fl z0)D{2NBoOh5k}hMaCLBVZz84x3=v-Zj>GydpY@e-=PAYD?~wM3?Oz&qVzoUsO2^6~pMB$^xR#R+*<4ZfF#AZ%DI+9YBTuLmc z^FB_l9*K{C++vHA+H8@S`4)E-LRAP=xdm6|=7P2=$}r&_5UJuWn~ED8v(9jGv&zXG zDnWgJS0=hxQ+hQcuNa!(YPg4(8&>aI-*ET7^AcrBz3L)$CbOEY zKeS(BV9q!d4^aL ztCzd?ijCqFu|Z@olFLd?^KV_560s6FYa1P#S>fuqMq*_y5P8oi4SjWyLjFbJ8nfq5 zv|j3DUbs!M$i#PEAi@aT?gS#-1plg=rYAx;kq9Yp8{rz^PQlUqvt#~30}<{YEB6?} z|3u!^I4#4P)NtP@$+gST8q&yS9}kga!Ivd!4uMt*jwor^G5gg0LC``qk>6K!Q~EB_ zG69tN+|S&ixsLRA*ZkuPoy_US8B=f2STyx~w&oC#{pvN{KP=_5hk|$FdnNWA2$suP z38kRo0W~4SCur*8uy3kup}ge&0AnKHz$LwjiFZYIMZsnCjQqYby*W`%x6#j-ZeB{{lPYVnfpQ8J)Fa##*)c@heNW}Z zx@H>kt3rqoNNy-_94io`O=Zz0yViD;IfBtBlPq zOEvuU13`<6Oq*Pt?K&?0#C@~)68dz@;GM6>d$o1&2d`(w=$FWs)qE3?bJeC0Nl>qp zy*^3ibXy0-*OSVgk`nqfT)pF#E4RGfcGIjDO!dm_ z*O^AHfm0YObK_S*O2db+zW#l!@kkfo3^X6^ec)zjgyQCw!Evuoot4u0UdY6jEz}>L zCUYd$S0>gjVxCHO68V_QGZYhy*1vWFbGGiaLd==2Pam7J)v=b!Vs+g08b+!6q4Uj! zw+tS9Z5hVoMQxfox*8UZ>O;If6kA7XI6nqof?m9g{yO~>M~QyHtbZ9CNlpYN=lSEj zi1t$rn@6%rIr;ubYEx`K0*%dh?JAAsyhgF4IivxW46zlkbuc5=D|NUgHe;!;sbOZM zsDFozlb1&{z?wr{ir!_a3&f^>X~o5#)0L+F>$te(^BctnKc_T9Y3snR7?MzyDlO{w zkbNzg1`=foQr%ity5#I%Rza(%s&c>XJ0T8>wq0*+w&u+7FC!F788AZMX#5;tzFwhbr`xY{xTt?R=^l3T4Okqz(m>WVt!NQn7?qTyaa}8KKFSgh$r2gX` zev{_l*&bq|2Z(p`m4fH$Klu2GlLf@|G2BOHVybQ(C#Hd8v$9iJ z*2c~@2AI60!1!1VkOnP`N!Oj5XY^}2nY;wQCNJ5KGVQj!Xdp2L^JOsZe=+8NHs*I@ zU#4x^CqHkRUNbZCYTM7Qc{jUer}*{sq757VG3~E^kgu_o-K2T;2dS;RXb}a`1Z*&M ztXv*3KgjImW?IBtA7|zi{yI}OYi8x*NF~yBv2-2MHP@N5Ii%khdEtBM+Uusp(yb9@ zFCP*(qd7O5m2)D6CbkJXBBA@=k;~jemLta3?EUwT_ln74&LNUL!%gJ&ky7-+hWR`` z!ax@IfeSooLsPV0PIuRTp(BjtSNM(#Rg!{jYM7{we!i+;yY6thxMUG8OW?hx1ly_^ zqD}mjvY<{4?^e?eLuL^Lb1v}w>5yQ4K7Cy1R*uFl4vh=lhFRr9Mux`E4wZ=$4)dY( zSnQ*riHGgTD~!e78%jS+AvecjZ=+;ItfV&*{$mJVVHQGf+ zen=e$jK@~9?CqwQZ9 z?OZ7%Pd_1pZs$U_Ed&iSuJmdu^PfaLa%+JrYs(`|bDQ&BN#=|il4BAzO>+BnWk%^% zi9O792$orkW|xlZ<5*7KJgO0GZP&W2rFy_J^5)3rLB`X3dF3tIF0*TTX<{t(p2**V z#k=hN(;f8=oj+@l!6ej75>4obJM!n?)H2q|hK$@k2Y+UQE34FqGM@#FH~l)pB&IE5 z2Q=U@YU(q4z&pWqrt`&f6rhaEp(OhSz}Mt&C@2frY~|(x4Sq(<+zmyuY!93PH?N?J@A|1F2KcTx=H)GS2UotA7gwZ_l|(&Vb+=% zxU`e8rUw`~VOT4w;a`t%ke=VK{)gg!>S$E%*Iu6Z0E^T4#7OPHX@%G>JzUUs$VJQ< z)z946PEX%Y9^CnL3p4+yLla^&iCyz2PjU7T@*83i%#5tWMmJCWGN=os5jFht5sW&N z#uQHGvOGN_7|NK6rWJM%qRo7|l|u8Ijau9g7q{lhmQjlup7U9l=XG zbFI_;#!kjM#jmlR2+&j5_C=4lK9JbK?#k5S58X6{_(p;@>#KpPhqo?a3f647*i!H4 z5xKyp4_mi1I9B^V7M~4fi*}4yH@Yu)a3?drvE{UNPv@p3v$sq?p$Qo{QZJYbph=+Z zca3h$>Tl^HQ$z~c;Lu!1%Ae^ZPtj3+js2FGDMuuh!|q3vzuMkYI&Ic1iEWo2wr(WT zFy4gulLhk(u~ht153^##&B$M=^M3(+=?^=~dq2<$T~SFeTb*+N!N~tmSBT#P|2Xhx z?LQyJ$^j<1`h!?Glji1P)i8U+7 zzu(E;{6@<_R->_z=n5_hFZXqD`^9mgbp6!Gj)C&t_nD&(2K+^TM&BpIMoeNQD@Lg_ z_+{{E<*(RxS|aB$B3{gFi^t!L#S5IzfVOE01F5(vQr`QUMA{S79NM+;JkA;kUHZNq z4A-}VFlHiO8>MxA6C6l)ga`|K<ha)ZzF2>no*%y4gYw^DkYRP>ah>kP~4{lukIc1fe(^y1Z5YOy zkMN+kPoh{~8=Im7{cJRq<1bIFHb!b?gDc(5$st8(PPSNUxF%aWUEg*`GNC-}pW}$| zF5+}wK9WKN+7~(=x~^w^#qLV(Up1qvhdlPg;`*j1mj1e&&i;*9$?l_M+Q&V~E0Ugm zLUAr*x8Q5qC)uTcC<|UleuaA{Q!188aZ)`a8zS2#6@>^#o=$q1%Ab+QA&2G@TF*5j zI~B6*e6Rs4Q)eapZ+Mvkq}UQ)d+w`ycM;2m@_#6U&+F@-z+RD9ztIg0C*CPO@fF#{ zOdA)P%<53z6H3;yCzQ!eL|!(!U&*o&t6lz$m)1TJT!-W?3{;lmY%??;{O^{tjNCD_ z4VWs8VM>2_;xDdZD+63;zI$c)Zxzz?VaTXIf%93KuRydyD%u%II`o8+Yg5C{=$j#J zaxS!-T7A-Ct}l_f4KzOkl>Fh4{xDacZei>7A%-n+ooV^Fhb-rxGKv2cTUCtgkKT+k zPVG=pS<#YIsUzd)z13A5^I`|v#^$p_c8L_ zLrJegBG`D7h#t!}w+*9sE}Cx9+1UE)Ac>tE_%pHhuSos4hpQ&_4$&2>?>ABF7Axpk ziCDJ~>(;?3uS2r^lHEnQD{biq!==FsIEmHD0`~#@ABjeD5~Sz{u+LB7gje*j8JimY z%@-U=K3t3Me#`~t+NN(zs!w|PlO7?qsv{$|G1&{cpxED48I01^Ra&ETMc2=}a0lkb zX`a&JiwKAQ_|hrABqaY1VZxJAp;c2#K0D>B*RLOSw<*#bc1-8ei<35MUCWhhyG8oMqj53f ztC%6%Tkev0JG2dY4ybNr<&7f=OB0sZyd)=AD%epA%_S?Z9Yxp5@7jfUF4C`4&Tdm-APa1{lp@-Y5fk5C-J0b3u~a`KxE)jUPG&)(2-)UL+7NTs%B)$< zL}7l+iI;=vqNY19XPYCdn(5k`_0wv51|4S>=77y*eJr>iG*cC^Izb~ejd2O_lvtec zv1Q9Wl$w`QD5T+>WT}V7@1q~TdplDw6r^%EC$geHWBd@4NoGU>)g`QxLvL6=&ik1W zXmdeV#Z#B0iBBszCY`uLyzRt4lr}%Zs0%%Zr8^nJxx-Q>Bb<9xN|U{}{{(pIP)$@0tYLBzC7ezS z#CqI!k(BouvX1$RWuCiJvrsYE5~bwG_Tg07Tb}OI^ckkWWliz@UGdH-mRat<;U{kS zNZCF|)5lNPCB3ISKF5XcjyZpeK``7$@JXL@LQ%iOO?Gm9&Q#6`8DhFnFiylRlwpO! z_Z7pfUN{ukK6lpU$<=^PgC35X_TV%*QDZH(+^2@`QMJDpur>PbLf8K!fphV4FL zX5H*YhkKj5x&-uWCYF;inUEK`8Ur}dH^396Yq=HDH)=45m<3y^%qMpYcp1T$B32A+ zC%s+G?9B( zC^+FTi{U`JWO8RdXu0o`o+JzQHVb1pq6jvtFTpx$(g70^^t3zB-k(Qzxgu}lNrK9%As_6z%$$ds#I2d?DIbD5CGu_`F7~x%26_m;qoGa-0(f~(R5xcRK zhlIzAWmy^3YWRiN3=d*$Qe2W5%ltE3Gt2)F4PD;nc-*JAq*zu?X^jpHbJ48j@3vT* z*<5hbPRdQ>M-3tMhBq#I*N4x%+xKo#Qi$WqdoLw)4E%|E5z;YE(|YWo_d{=sdp*sH zJ0d%j)~Is1^>W45Q@$kJlv2pg0)jX!sY`S8$>2Ea`KMPkC^OF5&!RI^Fpq&+Uq3 z*%R)R2a9B$Nx6?H;H-TCTe}JP+t@l;EcQ$k8>BNjUQ9UOATind-0uw7SzTrUJPwIyM7PDP30qQN}Z&$M8A;WfXa-^jsByCqL z#=`8I+PgBXXaHwy5|=OlBqd6wxg)w2Yld1 zJ{k0?fTz{oIt4iW%kV!G=Sg7>$pI(QkiLcB7I*2A&Rx_R&IfwP#@{-Q7U|9udw#6s zR6pmoK1h22E&{oXfknV)fp|~a-XcCa$3m+bJ~FI^d!VDCT=K|U^p3FDqY?K?MDAYJ z6zaY!S)A(s0@ClkVa?^*+l_O{bZuxmr|BzB%>DPLZIQ{n@|#R&#TI^Zqfy9(%ySW>H#MJ@E2YB(5^ z{R*q6HB)_@8a}F;sujS^>N#Fy5GROJMK4eA1(HM7~N zCh#1b48u`2I}Nv81Mnxn|0YVIin0E-$7a_E+$Fz-*^1xK(LH>{j;5!T(>BJ+gHulw zNq^M)aQP1vi%H*BqFBgZAu+$J7`)Rry@@kYLZGPM5BcH5@U3DEdc9=iPYS7vm;Z@B zzM7uHxc(Zg^rT7_?8vEZ8lpW&bys=FwRcs-p+1VL;V+@NA@X+b7r^ra&=uDH=uI&p zBqU(J%%;24%qV*Zx}M>WmGlE^#DqQ_2aQA09`P-lOa>RGyMN@a6gg;KF9-gn5NOOz zH$J2Ari}aC*L@Or8slA$eUM!H2tsSHQ<%gf3aLJ#u+>i}Jh<$wBUPT0ZICe?8Sa$s zLWz?yu_mA`%54Y~N@{q%O4IViOsNR_9m#z%5Z_rU%o*ZZ_yR? zzIPPEcpA6IQl0r#9>(0EWC8MwLh>z`jd$Qv98sx>VyF%l0>2K@H4M2#o)KBr%W$m7 z`W;g$LWJQV+Z&q`(a+M6nCvNMwT;1o^1d8P8{dYzal}6xi|gBxkKr@`8=%zIb>j9f zw1k_y2Xj~Zg@!ODDhF+BeMDk*uV~KPai>mq&fi0BXryz{aH_>79x9BiODK@kvq(U1TEFfW>Lu|zr|sJ zWMhO(G5$d_v@OSn_%W#lrI?FrJ~ZAbB*d6PL1QcTOI}QEBN8kB z5SnHxyAg|T;LayIpkcfdHX(ODSsM#gm(Vbg=oj)f_E0MCT~;+11s~%r->3&1+Af=T z59qT;o#xENiHY)@OR6b@o=0gJyR^3%C5RgCL`lO~$usJ9hfUn$yGJ6^)bNKYvmnoD zqUSAA*lG|psN+wl+jur!@`TDeCFxjH4v}bzllF%(y&CHyPKemGseGRXz4JJfdk;0| z`grkN^Ko&dVpvOgTcc`o=DO|T!|3PJYC*;`4z6>Zn{qh0X07|h%=yw`x5@d1n>AgA zt<6C2Y9F2(>w^KkfAD|k!K_VoObEAmuAtWje4r81%OI~cwhJ+O&8eCyEjLSVJfZECiOd5FzX zO-{NV~(%RVc%|JqpWTt#{lk&}grj~a#ti7nE5O1g2g^ScxHZ{Jylk@?Y+y$mB2Sa$ab~IzoK;Zo@YYasR}w{ zF6=1ol8%9$VtMb`hq2N&Pwcg53w<|3yJ8jZklyY)v(=2%s`rzb=@(u!9CKU~*zZUV zjC21zm@0l9e955;u(H?r&VYA=V_~BI_U3Pb!kMpvA1TC`=>9l(%)$7NIsVm|Aw46K zng0q*a(Be$hT1POvW_qBJujG@8d(Q zL2oZ(5-dgi*(}pyCuD}V1vY{O$9>CU_!)!-Bfe&zNdKLK1Pa^JpvM5ug&;x*E0r~; zN}Ri62lE(H(Acui(cvxhaWzJf@{=7+Cg=smi4t@Jqv0)}75-^8{4bRiS2nK&4-j4h zpZw^UBiHS5TX3f&Xs!f|IA2vm4g~JoKG<1>-R*;I#`brm3@OXq;V#B4M)9^X_ct*- zDdm&J4|gVu9RmrBc1Df(6r@HQZgXF0!EGJ?Y>Q_0)QDF}?yOtLg&0nIHo8;qtQ<(- zqT3i#Mt6on+@Igh*m{F?3qxm~4`~y#EWeaaK)=mJ$Df!Q*>#2FEew=`o*xIj7q)G7 zua{Xu&r;Z51z0Og*Sa^mkF+3`xNxBncLoAyo}3EoV1sF)cLtd=aHpVH{RkYQxlz8*+-+H0gA;2$JsMfL+p>WzC<;(y$iju zFUXjK!IKF6R5ZDH@l6w|TSHet6(ZC4i9y}%He zN#sYpdCf_%R3h>=uP)EpDTFBho1|(mN{msHD{|lJanm3hjfVG+n#H7jlvj_29~rI6 zvTmYU^*kphPvxrQTR6)G(g=-wkq10@26}YoC^PM1kl;27*hvt+HqI-Ty@^Z3C1N`4 z4Y7a3EF9kV$7{cLl+2zNU>fJwu;!#jHYDKgMQiwBq$Xq(k2zRqyw9{Sovf(q&Qh2} zhPoTHGv8?WrVG6=GV)aBm_vZnV^2JuUevoN#RXY{LywqzdP}gyJ#W)Zu^ThvD0BTW z$E=peWF~>m+VQ|+LEL}^*UOA%dr)befHPzc#>+#79t~SYwT)Ho*2`q7uJI-JT;H06 z@+;~FOC=9%{kDZzaGQpk9nO92wyJFhw<+yfH&Z~*_5Hv-w>h)T)k3WA`xvTUBV;qx z`-RIt!R>jwVp}%u+5JkPyZAr+c4WqfLwQLU}|1@=UzycO>MgCN;GL5FeI*9xZ7D6*& zg^>u`L#EgoVf#~x)5X&*YM37tuD8$P#J*VVYB*_>6G11x&By70?x@|P1)G_Ii!D>F zJAA}aPz{QJR)0M#jfN<`$Q8OLmp{_t1xn-v-k8|YHyrXth>%Bd-?n>*eL?L9U+t9Ao@Ks!lBOwSZEs< zYpn05TZ|%CkMr;d*C-U&t*rP3dR~N{HsihsJ%5y*Yv`U>JK7dvlC=TrclvLw*2MZx z@$$u3|LIr;Fjpr%q?xx>mvneCq<1RbkutvJGcjB8BEOREowPJfoQ&4UwFzx>X9&R7 z{Dq9wunqo=0yc)=R;S!G9?G$Hh@S>=vpte1y^nQs zHIL?Wf`ZQlcZdlRwfppLpMt$vF5lHO4tQNwmj+DYG>22mq#8;iM4nKye9A4j%}S|Z zW^w__CG2t{ZV^B35ekmuj-|D)Be}e{L&u#Zro`Bot&^T}bV8Fk&$l&GZMhb=p`D5m zH(9^%UpH$hY{+cdKRB=&xIEvL7TCyAurFZs<=3+MW76VJ4GGK9mtP4|=Hhzji&)K zGicAV`f=N}x?;+^p=|cWP&PLmZxl4>!!(piDV|jPp>F(n#{43zfZv6_f<>ws5HHwo za};@rd*#rhfHH;!WwHudS~Y5C!h(f!W{B+4mFjI31f3uZ`UA1 zc5|W`yF5KVKNr>D7Fd}1S(kAp*O|G4>T&-WG|ha%M{T$=B^DN4tfdp@lnWZ)LQ**BpI+Kv!mUY8&cdyZeAv>`jCT z{x+Kilao+D1};w{)j?o2Y-;Bp%1P{9*q~p6A-;Kx0gI&BvoU zv77FiByQFB{y_4~ba^YRZ}qUH{V+J+^DHFKTO@MbP~d8r4Jbo$Kl2~-%y++3@d-3# zWI8xgHC|RE;oU-rizJQy9-Pn;y5o2T)r9b$r`};&(uSl>>iDjo$x8a829n z$#fk5yWTfY@7aqpw=nZQ4G!%*1*w#oubFY7WqD`rWMa{UG7Js4Z8a)PpFv@|nK?{> zcE-h)!2q{UznkBexEpttp$x{byU@oMJSdIZJ=eDab_2wcR3F_*u?Fdp%W%eVMs*Gg zOcZlh@wUZ1bIb9a%)A+sK_ij*yN++Gb#Q@$QXHQq(61U^IEryV3f=x4%+BAn*OBH+ zfwpn~=UX6+{=u&`G2L0VD%@w2lW&1PDuILyVX`K~8%Q6QpxL`0I|0*2lHLwwCLic* za6H=uxomy{oCX%Kn)!zPdize7hiCnRfbIoiyX3dLkB{7 ziR!KNZln3a&ugKhM(Oz3k&9@Ek#t{u7m?(;Q%W5qRcZbZ?aTd9W3$$Eu)^4`aYcjl#R&8q@%qrGM7)Jaf@7s(DW#ZkbIs|odrB7ewZ-yD zXxGqsA!Gku+)2J4FD3RHTXPoYETPsq!x6HF52X?FB4U9aQhs~Lw217%xhYggrn-n+ z7@6XFx96Jcr!1z>{~CkN9D{yoDC1k`bULTjD?h{Qs?nSuFB{EM{VgHy zT~qvCWtGj=J{~P6`cv@zD3C<;+Jj!aIB8ftED@5i{rK%A$8cd}V`Hadg-llR$9ry+20e2tvvnFV-Jd24OuEmT{i&|hS0tO5gE8OExT%jj zpZE=TF4khtXq>7))#%U1Q=tRCkLLVsEQeaxf-CFOWRp-Bn6I4(8NrmG2^H#1TI2c0 zK!el@{q7->d&31Go0jyAUMqXK0(ZRT2eoPEE4AatYCqqTinECju&yg;HUj!u*{)N= zzaPDE4EmCG()m~JP4ygYh9q0E%#&V1TR8go)51nRMJijD#!HgC48%V8ub{&dh`ETv2H^bft^=$hlp8J@cQ^JkjPH2`^PI zN%Nh3u-%|#N)Y=OWcFIA-VsLqIG3Yde-H1z0d*&yp)l90Rj9WL_2!DSW)7O#ab5W$ zvwI|bt&00`>GjNc;vCd%iq+i{q@^+thMjAx-br#gD%i&DZrp;ClRcW0Q*O3`t2DL? zt^OWOemyiW+oWx6+axA`oBNI|N5!OSw4dML$nkYZ#T5jvE)GQK%?-Vwr@FRkyvyQo zvZTD^chc{g4@xiKOmNV>JnLY^gw1A8JxX4Z4pv<9B*Hovn*XtPYPxP}!%1Whc9l;r zJc82Ixa$s=)4Hw^_lkZcs}6Lcr*lP5vlGPC%`;tw?^V`wllw(ciDP^WW>$xF&goP3aPg)Ae$9dIcTtaCqhk z+9zA9D=TryqfNYM^jkgpGUgCYyO?vTW8`tVr12#~R?kY04*QfE?o#o(6y+x00(<#Z z!;+#3tEWb4z<6M>(itE7E+4f;!aFESUIimsHc=ahLi=zy1RFnEmWRz+rl(G7gx%(z z*u6;{KgL!8?;}mn1eo2oyBV{kB4d*pu2UJ&49{X|U%2c@^epZVxL zEp#S6s)shV8WyWzhf3dg>KA#b*OJOEn9{oEM8 z5Rk7QD<`xsf{f+B45Wj4Qr*RF9=XxCDaVY@1BZFh`ZFZfz0Cci3ObiByiLcdadDwE zwxdU52J9S4+q&^ckF{Ifnzi&KPKos94)4$+1>$lg?YahNWo(u2^)NXz-TFM-kxkOu z{@9~gL9HwBj_}PnYl<^2r5EC@WW96{_KU(KypN1*g#E@IhXHi_s@v*bB9P>KgP`X1Im+ z9fjaYfVVJrFBDo6QKlYc{-P$Jj6)`lJ1R0HNNu-e#LL8d#J^W|Yi@%Nb_KSRY|M%j z_auaJGvYN>mu$^Nt*`?8N+p*`6^lKa5Ae2H*r?#nBW@#4w&qf+lc|7efF;2TYLaWf zoKeBz&K!N{J;}!WL2v#Y;Cqj$s2%=$Sl#o7)bQ_cr$N^+&XM3E;LsQgwePg|!V&>e zEN<@nvt9Q7%jOJs-1_p7bZgnu4nxpwptiKT}}^m-i*c5(xfLN%LyNTcL|tsPOY) zjkBQGP45tqUdKu_y}@DdC&I?HbVG)lTJ0NLS#VPU{*pF8g0@D}k$p_#& z+v)reUZCUJjIbB(mvFEWZB5*q+nkBnN&uHltOho%!+y#Sg=*X-_=u0rEp{XHn=B*p z?~hlvZfjD*&yFN5g&m=sF_bUWId1eB$`TjdDy2!YeX!CgV&>fH<|lC?Ec)>RjM-g) z-aR?O?4$NjxI6Yejymh$;&p8vF~;f|RN{3dm5uw4x@b)t?keMTc~BRu!ZzSMBu2w$ zMrf^9>om5qPAx3!>u}3uYluZ!BlkZNM5b?RCfGlLhSYWQsx?*n6R z`RUt1HQbN8VU0hY%j}^x;naqK()LxSb1m#&fmH!&b=#poZ*VWZ@{t2^7lW5hfU*($ zGr$*zm6h4dxyr7*0!pkdp=mUa+HU8!u_3R6z0BvIQTCW@O}gPU$60{z{FtB%$IzIGgE@nT|LSw+|01@s~wry^|8}#J-Dv7xZK=O2~L}Tp_1CB-%)kJeFx+c@I{IjUyP1m{|-op z(3;{oD<#^yT22=?>QX9n=dbR>pN>f!0?sfwF1m9ztl3xDum&|&?p@Ql#x3heXJzLa zGUlhRom7>+HUn<&TDR<7wzq2UT3D3D{LZyqRh?@uz@@L7v})4247h#w6v^IO_u)4k zVm^M?0c*mTGHBmA#YZ^+RtA38p$@<6P-mh(k+%+UkWBBu4lp(btc9Cg^U4}lgKoj= z>vY5C(L3y+<7Go~G%&ZU9B1Yf_pP2qn&ES?Jt3dMJ;e>5pS1`Yph52jy*m$3-4!WF zcgSv*<~w8|=F|GF)^!#X?HD{+Nh#Y@b|D7k1y9V7b~D50CzoZ=Jx8W{OolWbDYPFG z@LMJw$`0t7D5W`M9{MId>}zx^KQj_&06fRi-my9&pS z!PNuK|1I3F{sWG`S|84s%RE=>+wdQ7^koBWw;pi*Z{dFWA8^L2^{qjDYp&K;_AT5* zyb1WRhacR$;f{)fTK;@$Sv?^AfOI2fEK6OBa*Qyoz} zO&&}8pq=W<;%T3crJZc2`XkCaXN{%&0cQnmHeOobj`@eGHd|*J4KIf;z%PfN0N({) z55ENdHSmk!=fb~5)n#YFpRXFSr@_Aw{&e^SDy0xZ_O%GlipkyBmq%f1s;J@9)Wt01 z)UXR}6N{#7lE@Sot?%@Z6x_D63bpa@QMyWmT~9i8=V`|&PcA&pG-V=Wl9;dH&w##;<(eGMQZ=TW+$?I{2+Ar z)1-lSjc~nTR-w-@jm*-D%*B@- z9LhuUXMzqzKB#1a`=yD`w3*81!(xWsfWwAZDRj?>g>F=evn&A$Pt%`LE(I;nwq0{B z(aLpM+*!_PZoaz;zu#l0_UTY@78|fiT(iYp*q(>{sWS>= zS|q+};@GL8u-&n4eDiHew$0T>Y0Rd~1f9gs>qNfhIAhiOf0V@qC_H1reC0TlX?AKi zNle|rIKAL~3FJ*mKlH6xy#16|4~~4V2gFDFfZv{J<3v!BmHq+?nK=aNXG*bFKyx*?B^8@iuU$O1V9u-}&xVcFt5>h0w7hh^siL~t zw9#|7o;WI2nSSm6SPqP(no)`DNAJdbP_3P2BXTv=|uh_6!PwX#z7Fs3k)}n9A zZ(mZ7zj*E~w;=r8w_CKI#YIaO{j8#DwaIhus@0ok6E9%C^Pw^FpoIusW+Lp*#P24; z1tt^WdHAhxzk_=jt{LtK+)x$~+Ti~SziD_H{>O;pUH$*#-;$g0TSsvB!&SiD1NS4i zg>ZA=YT)jv@Z3u)@T^+5n)+`+PhyA8WHWdcgZNMEXaj$Ile(40>ECod?h`i`3yjWW Y$Qh)mW66Wdh(Pg?2f7(c`akCVFO^(hLjV8( literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/MatekL431-Airspeed_bl.hex b/Tools/bootloaders/MatekL431-Airspeed_bl.hex new file mode 100644 index 00000000000..2c48620e5ae --- /dev/null +++ b/Tools/bootloaders/MatekL431-Airspeed_bl.hex @@ -0,0 +1,1322 @@ +:020000040800F2 +:1000000000090020B5040008D5290008552900087A +:10001000AD2900085529000881290008B704000807 +:10002000B7040008B7040008B704000889370008BF +:10003000B7040008B7040008B7040008B7040008B4 +:10004000B7040008B7040008B7040008B7040008A4 +:10005000B7040008B70400089D4A0008C54A000814 +:10006000ED4A0008154B00083D4B0008B704000896 +:10007000B7040008B7040008B7040008B704000874 +:10008000B7040008B7040008B70400089125000869 +:10009000BD250008CD250008B7040008654B000801 +:1000A000B7040008B7040008B7040008B704000844 +:1000B000754C0008B7040008B7040008B70400082E +:1000C000B7040008B7040008B7040008B704000824 +:1000D000B7040008394C00084D4C0008614C00087A +:1000E000C94B0008B7040008B7040008B7040008AB +:1000F000B7040008B7040008B7040008B7040008F4 +:10010000B7040008B7040008B7040008B7040008E3 +:10011000B7040008B7040008B7040008B7040008D3 +:10012000B7040008B7040008B7040008B7040008C3 +:10013000B7040008B7040008B7040008B7040008B3 +:10014000B7040008B7040008B7040008B7040008A3 +:10015000B7040008B7040008B7040008B704000893 +:10016000B7040008B7040008B7040008B704000883 +:10017000B7040008B7040008B7040008B704000873 +:10018000B7040008B7040008B7040008B704000863 +:10019000B7040008B7040008B7040008B704000853 +:1001A000051800080000000000000000000000002A +:1001B00053B94AB9002908BF00281CBF4FF0FF31CE +:1001C0004FF0FF3000F074B9ADF1080C6DE904CECA +:1001D00000F006F8DDF804E0DDE9022304B0704722 +:1001E0002DE9F047089D04468E46002B4DD18A42EA +:1001F000944669D9B2FA82F252B101FA02F3C2F11D +:10020000200120FA01F10CFA02FC41EA030E9440AD +:100210004FEA1C48210CBEFBF8F61FFA8CF708FBCE +:1002200016E341EA034306FB07F199420AD91CEBA6 +:10023000030306F1FF3080F01F81994240F21C81D8 +:10024000023E63445B1AA4B2B3FBF8F008FB103320 +:1002500044EA034400FB07F7A7420AD91CEB040455 +:1002600000F1FF3380F00A81A74240F20781644425 +:10027000023840EA0640E41B00261DB1D4400023AA +:10028000C5E900433146BDE8F0878B4209D9002D0E +:1002900000F0EF800026C5E9000130463146BDE898 +:1002A000F087B3FA83F6002E4AD18B4202D3824202 +:1002B00000F2F980841A61EB030301209E46002DB1 +:1002C000E0D0C5E9004EDDE702B9FFDEB2FA82F206 +:1002D000002A40F09280A1EB0C014FEA1C471FFA64 +:1002E0008CFE0126200CB1FBF7F307FB131140EA4B +:1002F00001410EFB03F0884208D91CEB010103F118 +:10030000FF3802D2884200F2CB804346091AA4B2D9 +:10031000B1FBF7F007FB101144EA01440EFB00FEAD +:10032000A64508D91CEB040400F1FF3102D2A64512 +:1003300000F2BB800846A4EB0E0440EA03409CE7B1 +:10034000C6F12007B34022FA07FC4CEA030C20FA5E +:1003500007F401FA06F31C43F9404FEA1C4900FA7E +:1003600006F3B1FBF9F8200C1FFA8CFE09FB1811FB +:1003700040EA014108FB0EF0884202FA06F20BD96E +:100380001CEB010108F1FF3A80F08880884240F2BE +:100390008580A8F102086144091AA4B2B1FBF9F002 +:1003A00009FB101144EA014100FB0EFE8E4508D9FD +:1003B0001CEB010100F1FF346CD28E456AD9023882 +:1003C000614440EA0840A0FB0294A1EB0E01A14267 +:1003D000C846A64656D353D05DB1B3EB080261EBD5 +:1003E0000E0101FA07F722FA06F3F1401F43C5E9AF +:1003F000007100263146BDE8F087C2F12003D840E5 +:100400000CFA02FC21FA03F3914001434FEA1C4726 +:100410001FFA8CFEB3FBF7F007FB10360B0C43EA18 +:10042000064300FB0EF69E4204FA02F408D91CEBC8 +:10043000030300F1FF382FD29E422DD902386344C6 +:100440009B1B89B2B3FBF7F607FB163341EA034166 +:1004500006FB0EF38B4208D91CEB010106F1FF38B5 +:1004600016D28B4214D9023E6144C91A46EA0046AC +:1004700038E72E46284605E70646E3E61846F8E63E +:100480004B45A9D2B9EB020864EB0C0E0138A3E787 +:100490004646EAE7204694E74046D1E7D0467BE768 +:1004A000023B614432E7304609E76444023842E7E0 +:1004B000704700BF02E000F000F8FEE772B63A486D +:1004C00080F30888394880F3098839484EF6085186 +:1004D000CEF20001086040F20000CCF200004EF6BF +:1004E0003471CEF200010860BFF34F8FBFF36F8FFE +:1004F00040F20000C0F2F0004EF68851CEF200014A +:100500000860BFF34F8FBFF36F8F4FF00000E1EE35 +:10051000100A4EF63C71CEF200010860062080F30E +:100520001488BFF36F8F04F033F904F00FF904F06F +:1005300049F94FF055301F491B4A91423CBF41F8E1 +:10054000040BFAE71C49194A91423CBF41F8040BDD +:10055000FAE71A491A4A1B4B9A423EBF51F8040B5C +:1005600042F8040BF8E700201749184A91423CBFB3 +:1005700041F8040BFAE704F0EDF804F07BF9144CB1 +:10058000144DAC4203DA54F8041B8847F9E700F035 +:1005900041F8114C114DAC4203DA54F8041B884762 +:1005A000F9E704F0D5B80000000900200011002090 +:1005B000000000080001002000090020E8510008A8 +:1005C000001100208C1100209011002034390020EF +:1005D000A0010008A4010008A4010008A40100086B +:1005E0002DE9F04F2DED108AC1F80CD0C3689D465F +:1005F000BDEC108ABDE8F08F002383F311882846F4 +:10060000A047002003F0ACFDFEE703F025FD00DF6E +:10061000FEE70000F8B504F03BF8074604F08CF85C +:100620000546A0BB1F4B9F4231D001339F4231D0C2 +:100630001D4B27F0FF029A422FD1F8B200F030FD97 +:100640002E4642F2107400F035FF08B10024264611 +:1006500000F02CFD08B90646044635B1134B9F4205 +:1006600003D004F061F800242646002004F01AF8B4 +:100670000EB100F063F801F051FA00F03FFF01F015 +:100680003DF9204600F0A8F800F058F8F9E72E46AA +:100690000024D8E704460126D5E706464FF47A74CD +:1006A000D1E700BF010007B0000008B0263A09B04A +:1006B00008B501F0EFF8A0F120035842584108BDF9 +:1006C00007B541F21203022101A8ADF8043001F090 +:1006D000FFF803B05DF804FB10B5202383F3118805 +:1006E0001248C3680BB103F0B3FD114A0F48002351 +:1006F0004FF47A7103F070FD002383F311880D4CE1 +:10070000236813B12368013B2360636813B16368F6 +:10071000013B6360084B1B7833B9636823B902203F +:1007200001F08AF93223636010BD00BF90110020F0 +:10073000D9060008AC120020A4110020214B224A47 +:1007400010B51C46196801313BD004339342F9D1EE +:100750006268A24235D31D4B9B6803F1006303F529 +:1007600020439A422DD203F0B1FF03F0C3FF0020D3 +:1007700001F0DEF8164B0220187001F055F9154B08 +:100780009A6D00229A65996F9A67996FD96DDA65AB +:10079000D96FDA67D96F196E1A66D3F88010C3F86B +:1007A0008020D3F8803072B64FF0E0232021C3F8C8 +:1007B000084DD4E9003281F311889D4683F30888FF +:1007C000104710BD00A0000820A000080011002064 +:1007D000A4110020001002402DE9F04F93B0AC4B63 +:1007E00000902022FF210AA89D6801F041F9A94A42 +:1007F0001378A3B9A8480121C3601170202383F3A3 +:100800001188C3680BB103F023FDA44AA24800235A +:100810004FF47A7103F0E0FC002383F31188009B0E +:100820009F4A03B113609F49009C00230B705360E3 +:1008300098469B461E469A46012001F0F5F824B1E1 +:10084000974B1B68002B00F01C82002001F022F85F +:100850000390039B002B01DA00F094FE039B002B16 +:10086000EDDB012001F0D6F8039B213B162BE3D8EA +:1008700001A252F823F000BFD5080008FD080008C7 +:1008800091090008390800083908000839080008EB +:10089000250A0008F70B0008110B0008730B00086D +:1008A0009B0B0008C10B000839080008D30B000897 +:1008B00039080008450C00087509000839080008C7 +:1008C000890C0008E10800087509000839080008CB +:1008D000730B00080220FFF7EBFE002840F0FB81BD +:1008E000009B0221B8F1000F08BF1C4605A841F289 +:1008F0001233ADF8143000F0EBFF9DE74FF47A703F +:1009000000F0C8FF071EEBDB0220FFF7D1FE002836 +:10091000E6D0013F052F00F2E081DFE807F0030A8F +:100920000D10133605230593042105A800F0D0FF10 +:1009300017E057480421F9E75B480421F6E75B48D4 +:100940000421F3E74FF01C09484600F0F3FF09F1DA +:1009500004090590042105A800F0BAFFB9F12C0F95 +:10096000F2D1012000FA07F747EA0B0B5FFA8BFB85 +:100970004FF0000A01F0C8F826B10BF00B030B2B67 +:1009800008BF0024FFF79CFE56E749480421CDE745 +:10099000002EA5D00BF00B030B2BA1D10220FFF7EB +:1009A00087FE074600289BD001203E4E00F0C0FF86 +:1009B0000220307001F038F84FF000085FFA88F933 +:1009C000484600F0C5FF044690B1484600F0D0FF0D +:1009D00008F101080028F1D1B846044641F212138B +:1009E000022105A8ADF814303E4600F071FF23E760 +:1009F00001230220337001F00DF82546244B9B683B +:100A0000AB4207D9284600F095FF013040F06881DD +:100A10000435F3E7234B00251D70214BB8465D607C +:100A20003E46A7E7002E3FF45BAF0BF00B030B2B0A +:100A30007FF456AF1B4B0220187000F0F5FF3220F8 +:100A400000F028FFB0F10009FFF64AAF19F00307E4 +:100A50007FF446AF0E4A926809EB050393423FF6D6 +:100A60003FAFB9F5807F3FF73BAF124B0193B945DC +:100A700022DD4FF47A7000F00DFF0390039A002AF4 +:100A8000FFF62EAF019B039A03F8012B0137EDE728 +:100A900000110020A812002090110020D9060008A3 +:100AA000AC120020A4110020041100200811002025 +:100AB0000C110020A8110020C820FFF7F9FD0746FF +:100AC00000283FF40DAF1F2D11D8C5F120024A4573 +:100AD0000AAB25F0030028BF4A4683490192184417 +:100AE00000F0B4FF019A8048FF2100F0C1FF4FEAF7 +:100AF000A9037D490193C9F38702284600F0C0FF8E +:100B0000064600283FF46AAF019B05EB830531E7F9 +:100B10000220FFF7CDFD00283FF4E2AE00F044FFD5 +:100B200000283FF4DDAE0027B946704B9B68BB42FE +:100B300018D91F2F11D80A9B01330ED027F00303B9 +:100B400012AA134453F8203C05934846042205A9F1 +:100B500001F04EFF04378146E7E7384600F0EAFE31 +:100B60000590F2E7CDF81490042105A800F0B0FE3E +:100B700000E70023642104A8049300F09FFE0028EE +:100B80007FF4AEAE0220FFF793FD00283FF4A8AE3D +:100B9000049800F0FFFE0590E6E70023642104A816 +:100BA000049300F08BFE00287FF49AAE0220FFF73A +:100BB0007FFD00283FF494AE049800F0EDFEEAE7D4 +:100BC0000220FFF775FD00283FF48AAE00F0FCFE1E +:100BD000E1E70220FFF76CFD00283FF481AE05A994 +:100BE000142000F0F7FE04210746049004A800F04A +:100BF0006FFE3946B9E7322000F04CFE071EFFF6C3 +:100C00006FAEBB077FF46CAE384A926807EB0A03FD +:100C100093423FF665AE0220FFF74AFD00283FF4FD +:100C20005FAE27F003075744BA453FF4A3AE5046E2 +:100C300000F080FE0421059005A800F049FE0AF1AD +:100C4000040AF1E74FF47A70FFF732FD00283FF411 +:100C500047AE00F0A9FE002844D00A9B01330BD018 +:100C600008220AA9002000F00BFF00283AD0202219 +:100C7000FF210AA800F0FCFEFFF722FD1C4803F04C +:100C800075FA13B0BDE8F08F002E3FF429AE0BF0DB +:100C90000B030B2B7FF424AE0023642105A80593DE +:100CA00000F00CFE074600287FF41AAE0220FFF782 +:100CB000FFFC814600283FF413AEFFF701FD41F22F +:100CC000883003F053FA059800F042FF4E4600F0DA +:100CD0001BFF3C46B0E506464CE64FF0000AFFE538 +:100CE000B8467BE6374679E6A811002000110020BF +:100CF000A0860100094A136849F2690099B21B0CE9 +:100D000000FB01331360064B186844F2506182B255 +:100D1000000C01FB0200186080B270471411002023 +:100D20001011002010B500211022044600F0A0FE92 +:100D3000034B03CB206061601868A06010BD00BF4A +:100D40009075FF1F2DE9F043224DBBB001F04AFE24 +:100D5000AB6840F2ED22C31A934232D906AFA860C5 +:100D60002B4628220021384602F0D6FA05F10E0063 +:100D700000F076FE002604465FFA80F905F10E08C1 +:100D8000F3B2F100994501F1280107D908EB0603F8 +:100D90000822384602F0C0FA0136F1E708230122A2 +:100DA000CDE9023205340C4B0193A4B230230093F9 +:100DB000CDE9047405A3D3E90023297B074802F099 +:100DC000C3F83BB0BDE8F083AFF3008078F6339F03 +:100DD00093CACD8DC0330020CD330020CC2200201B +:100DE00070B50D4614461E4602F044F850B9022E66 +:100DF00010D1012C0ED112A3D3E90023C5E90023A1 +:100E0000012007E0282C10D005D8012C09D0052C92 +:100E10000FD0002070BD302CFBD10BA3D3E90023F1 +:100E2000ECE70BA3D3E90023E8E70BA3D3E9002306 +:100E3000E4E70BA3D3E90023E0E700BFAFF30080B2 +:100E4000401DA12026812A0B78F6339F93CACD8DB1 +:100E50009E6AC421818A46EE26417272DF25D7B789 +:100E6000F017304A39059E5613B504462346084606 +:100E700020220021019002F04FFA22790198032AE2 +:100E8000234628BF032203F8042F2021022202F068 +:100E900043FA62790198072A234628BF072203F8FC +:100EA000052F2221032202F037FAA2790198072A9E +:100EB000234628BF072203F8062F2521032202F02C +:100EC0002BFA019804F108031022282102F024FAD9 +:100ED000382002B010BD00002DE9F04FADF5037DC4 +:100EE00023AD10AE40F2751280460F4624A80021B3 +:100EF000296000F0BDFD48220021304600F0B8FD19 +:100F000001F070FD564B4FF47A72B0FBF2F01860AE +:100F100093E80700012386E807000DF162003382A1 +:100F2000FFF700FF42F20463338407AB18464D49D4 +:100F300004F068F84FF0200930642946304686F8FE +:100F40003C90FFF791FF14AB044601460822284667 +:100F500002F0E2F90822A1180DF15103284602F02F +:100F6000DBF90DF15203082204F11001284602F0CA +:100F7000D3F94A4615AB04F11801284602F0CCF922 +:100F800016AB402204F13801284602F0C5F918AB2F +:100F9000082204F17801284602F0BEF90DF1610340 +:100FA000082204F18001284602F0B6F904F1880A0B +:100FB0000DF1620904F5847B4B4651460822284610 +:100FC0000AF1080A02F0A8F9D34509F10109F3D1A1 +:100FD0001DAB08225946284602F09EF904F5887494 +:100FE0004FF0000996F834304B450AD9B36B2146CF +:100FF0004B440822284602F08FF9083409F1010910 +:10100000F0E74FF0000996F83C304B4504EBC9017E +:1010100008D9336C08224B44284602F07DF909F1C7 +:101020000109F0E7CB1DC3F3CF03CDE9045300233F +:101030000393BB7E029307F11903019301230093ED +:10104000F97E05A3D3E90023404601F07DFF0DF5AD +:10105000037DBDE8F08F00BF9E6AC421818A46EE01 +:10106000B41200203450000810B50A4B0A4A1A6026 +:1010700003F5805393F860203AB9DC6D2CB120461B +:1010800001F02EFB204603F04FFEBDE81040034860 +:1010900001F026BBF0220020345100083833002034 +:1010A000014B1870704700BFC0120020F0B5334BE1 +:1010B0001C7B85B034B1324B0E221A8100242046AD +:1010C00005B0F0BD2F4A1068516802AB03C3082376 +:1010D0002D492E480DEB030203F078FE054630B98A +:1010E000274B2B480A221A8101F078FAE6E70169BA +:1010F000B1F5583F06D9224B26480B221A8101F040 +:101100006DFADCE7438B40F22642934207D01C493C +:101110000C2008811946204801F060FACFE71F4AE9 +:10112000024402F11003994204D2154B1C481022CC +:101130001A81E4E710398E1A2046144901F058FC50 +:101140003246074605F11801204601F051FCAB6814 +:101150009F4202D1EB6898420AD0094B0D221A81B6 +:101160000090D5E902123B460E4801F037FAA5E798 +:101170000D4801F033FA0124A1E700BFC03300207D +:10118000B4120020E9500008DC5F030000A0000852 +:101190005850000864500008765000080860FFF7B7 +:1011A00094500008B1500008DA5000082DE9F04FC3 +:1011B000ADB006AF80460C4601F05CFE0546002847 +:1011C0005AD1237E022B1BD1E38A012B18D101F0C7 +:1011D00009FC0646FFF78EFD03464FF4C870DFF8A2 +:1011E000D092B3FBF0F206F5167602FB103316FA36 +:1011F00083F3C9F80030E37E33B9A84B00221A709C +:101200009C37BD46BDE8F08FA38AEEB2013BB342E6 +:1012100005F101050BD93B1D1E44E9000096002392 +:10122000082201F0F801204601F03AFFECE707F14F +:101230001400FFF777FD324607F11401381D03F063 +:10124000B5FD0028D9D10F2E08D8944B1E70D9F8BF +:101250000030A3F51673C9F80030D1E7FB1CF87015 +:101260000146009307220346204601F019FFF97852 +:10127000404601F0F7FDC3E7E38A282B26D010D8BB +:10128000012B1ED0052BBBD1BFF34F8F8449854B5B +:10129000CA6802F4E0621343CB60BFF34F8F00BF14 +:1012A000FDE7302BACD1637E7F4D01336A7BDBB22F +:1012B0009342E94603D1E27E2B7B9A4265D0CD462C +:1012C0009EE721464046FFF707FE99E7A38A013BC8 +:1012D0009BB2C92B94D8744D2E7B26BB05F10C0311 +:1012E0000093082233463146204601F0D9FE731C94 +:1012F000F2B2D9001E46A38A013B9A4205DA0E32A9 +:101300002A44009200230822EEE700230022C5E9C8 +:1013100000230023AB6085F8D730C5F8D8302B7B8D +:101320000BB9E37E2B73002507F114093B1D08223E +:1013300029464846C7E90155FD6001F0EDFF3B7ABB +:1013400005F1010AAB424FEACA0608D9FB68082238 +:101350002B443146484601F0DFFF5546EFE7C6F320 +:10136000CF06E17ECDE9049600230393A37E02938A +:10137000193428230093019446A3D3E9002340465F +:1013800001F0E2FDFFF7DEFC3AE74FF0000807F15D +:101390001403A7F81480102200934146012320462D +:1013A00001F07EFEA68A023EB6B2F31C9B109B00A3 +:1013B0000733DB08A9EBC3039D460DF1180A1FFA9A +:1013C00088F34FEAC801B34201F110010AD20AEBD7 +:1013D0000803009308220023204601F061FE08F173 +:1013E0000108ECE795F8D70000F0B2FAD5F8D8304C +:1013F00004461BB995F8D70000F0BAFAD5F8D830F2 +:1014000033449C4204D295F8D700013000F0B0FA82 +:101410004FEA960B4FF000081FFA88F18B45D5E98B +:10142000003209D90AEB880103EB8800012200F0A1 +:1014300027FB08F10108EFE7F31842F10002C5E9C4 +:101440000032D5F8D83095F8D70006EB0308C5F878 +:10145000D88000F07DFA804509D395F8D730D5F8CB +:10146000D8000133001B85F8D730C5F8D800FF2E0F +:1014700008D800232B7300F097FAFFF717FE08B186 +:10148000FFF75CF92B68094A9B0A0133138100239B +:10149000AB6014E726417272DF25D7B7C522002062 +:1014A00000ED00E00400FA05C0330020B412002073 +:1014B000C822002010B54FF000540C4B22689A420D +:1014C00011D10B4B627D1A700A48237D03730A49C0 +:1014D000C9220E3000F0BAFAE0220021204600F0C6 +:1014E000C7FA012010BD0020FCE700BF9AAD44C53B +:1014F000C0120020C03300201600002037B5184D60 +:1015000018491948022301226B7100F035FF0023AE +:101510000193164B164900931648174B4FF480520F +:1015200001F07AFC154B197811B1124801F09AFCC0 +:1015300001F058FA0446FFF7DDFB4FF4C873B0FB27 +:10154000F3F202FB130304F5167010FA83F00C4B50 +:10155000186003F0F1F808B10F232B8103B030BD00 +:10156000B412002040420F00F0220020E10D0008DC +:10157000C4120020CC220020AD110008C0120020AF +:10158000C82200202DE9F04F2DED028B0FF26429C7 +:10159000D9E900898D4C93B08D4E8E4F204601F0D5 +:1015A00037FD034660B30025CDE90F550E958DF844 +:1015B0004450027B8DF8442099684068DFF83CA2D3 +:1015C0000FAA03C21B6843F000430E9301F00CFA0C +:1015D000821941F1000300950EA9384600F0FAFA8D +:1015E000A84205DD204601F017FD8AF80050D5E736 +:1015F0009AF80030072B00F2B68001338AF80030E9 +:101600000BAE9FED6E8B0023724FDFF8F4A10A93AF +:101610008DF834300B9373604FF0000B5B468DED0B +:10162000008B01250DF11D0207A938468DF81C50CD +:101630008DF81DB000F050FE9DF81C30002B40F0DE +:101640009680204601F016FC0646002845D1624FE0 +:1016500001F0C8F93B6898423FD301F0C3F98246D4 +:10166000FFF748FB4FF4C8730AF5167AB0FBF3F2A4 +:1016700002FB13031AFA83F33B60584F97F800B04C +:10168000CBF1100ABBF1000F14BF33462B465FFAB3 +:101690008AFA0EA88DF82830FFF744FBBAF1060F3E +:1016A00028BF4FF0060A0EAB03EB0B0152460DF1BB +:1016B000290000F0CBF90AAB0393182302930AF137 +:1016C0000102474BD2B2CDE90053049220463DA31C +:1016D000D3E9002301F0D0FB3E7001F083F9414AC9 +:1016E000414D1368C31AB3F57A7F2FD3106001F010 +:1016F0007BF902460B46204601F098FC204601F09B +:10170000B9FB18B32B7B394E002B14BF03230223E4 +:10171000737101F067F90EAF4FF47A733946B0FB7D +:10172000F3F030603046FFF79FFB18230730029339 +:101730002F4B0193C0F3CF0040F25513CDE9037056 +:10174000009342464B46204601F096FB2B7B2BB183 +:10175000FFF7F8FA2B7B002B7FF41EAF13B0BDEC24 +:10176000028BBDE8F08F204601F056FC48E7DAF81E +:10177000143083F00803CAF81430594610220EA81A +:1017800000F076F90DF11E0308AA0AA9384600F008 +:101790001FFB96E803000FAB83E803009DF834308D +:1017A0008DF844300A9B0E930EA9DDE908232046EC +:1017B00001F0FAFD30E700BFAFF300800000000049 +:1017C00000000000401DA12026812A0BCC22002011 +:1017D00040420F00F0220020C8220020C522002035 +:1017E000C4220020A0340020C0330020B412002006 +:1017F000A4340020F1C6A7C1D068080FA53400208A +:101800000004004808B5054800F07CFBBDE808402E +:10181000034A0449002003F081BA00BFF0220020EF +:10182000F8340020691000082DE9F84F4FF47A735E +:10183000DFF85880DFF8589006460D4602FB03F7A4 +:10184000002498F900305A1C5FFA84FA01D0A342B0 +:1018500010D159F8240003682A46D3F820B0314645 +:101860003B46D847854205D1074B012083F800A0AD +:10187000BDE8F88F0134032CE3D14FF4FA7002F085 +:1018800075FC0020F4E700BFEC34002018110020A4 +:101890000051000807B50023024601210DF10700A1 +:1018A0008DF80730FFF7C0FF20B19DF8070003B0A7 +:1018B0005DF804FB4FF0FF30F9E700000A4608B579 +:1018C0000421FFF7B1FF80F00100C0B2404208BD23 +:1018D00030B4074B0A461978064B53F82140236869 +:1018E000DD69054B0146AC46204630BC604700BF71 +:1018F000EC34002000510008A086010070B502F011 +:1019000073FD094E094D30800024286833888342D6 +:1019100008D902F065FD2B6804440133B4F5204F6B +:101920002B60F2D370BD00BFEE340020A83400203D +:1019300002F008BE00F1006000F52040006870472A +:1019400000F10060920000F5204002F089BD000027 +:10195000054B1A68054B1B889B1A834202D9104419 +:1019600002F03EBD00207047A8340020EE34002075 +:1019700038B5074D04462868204402F037FD28B9E1 +:1019800028682044BDE8384002F042BD38BD00BFA1 +:10199000A83400200020704700F10050A0F510404E +:1019A000D0F8900570470000064991F8243033B113 +:1019B0000023086A81F824300822FFF7C1BF012004 +:1019C000704700BFAC340020014B1868704700BF5F +:1019D000002004E070B50E4B5C6893F90860421E6D +:1019E0000A44013C0B46934207D214F9015F581C8C +:1019F0002DB100F8015C0346F5E7184605E02C24FC +:101A000082421C7001D9981C5E70401A70BD00BFE4 +:101A10001C110020022802BF024B4FF400229A61E1 +:101A2000704700BF00040048022802BF014B082293 +:101A30009A61704700040048022801BF024A5369B6 +:101A400083F00803536170470004004810B5002379 +:101A5000934203D0CC5CC4540133F9E710BD0000BD +:101A600003460246D01A12F9011B0029FAD1704729 +:101A700002440346934202D003F8011BFAE7704781 +:101A80002DE9F8431F4D144695F824200746884653 +:101A900052BBDFF870909CB395F824302BB920220C +:101AA000FF2148462F62FFF7E3FF95F82400C0F1BD +:101AB0000802A24228BF2246D6B24146920005EB58 +:101AC0008000FFF7C3FF95F82430A41B1E44F6B234 +:101AD000082E17449044E4B285F82460DBD1FFF768 +:101AE00063FF0028D7D108E02B6A03EB820383420F +:101AF000CFD0FFF759FF0028CBD10020BDE8F883F5 +:101B00000120FBE7AC340020024B1A78024B1A701C +:101B1000704700BFEC3400201811002008B50849B8 +:101B200008484FF461430B6002F0BAF904490648D3 +:101B300002F0B6F9BDE808400149044802F0B0B926 +:101B4000D4340020FC34002068350020D435002037 +:101B5000094B10B51822044600211846FFF788FFEC +:101B6000064A074B127804600146BDE8104053F85E +:101B7000220002F095B900BFD4340020EC340020DC +:101B800000510008202383F3118862B670470000DB +:101B9000002383F3118862B670470000012070476C +:101BA0000020704700207047704700007047000019 +:101BB000002070470E20704700F5805090F8C80054 +:101BC000C0F340007047000000F5805090F9C90054 +:101BD000704700002DE9F0410C68BDF8187014F052 +:101BE00000541E466FD10B7B082B6CD8FFF7CAFF41 +:101BF0004569AB685B010CD4AB681B0108D4AC68C9 +:101C000014F080545DD1FFF7C3FF2046BDE8F0819A +:101C100001240B6804F1180C002BB8BFDB004FEA5D +:101C20000C1CB4BF43F004035B0545F80C300B6893 +:101C30000FFA84FC13F0804F18BF05EB0C1E05EB68 +:101C40000C1C1EBFDEF8803143F00203CEF8803159 +:101C50000B7BCCF8843105EB04158B68C5F88C310F +:101C60004B68C5F88831DCF8803143F00103CCF8CB +:101C7000803100EB441541F268031D4403EB44132B +:101C80000344C5E9002608330D4601F10C0C55F854 +:101C900004EB43F804EB6545F9D184342D781D70CD +:101CA00000EB441407F00303257925F00B052B43C3 +:101CB0002371FFF76DFF06973346BDE8F04100F052 +:101CC000AFBC0224A5E74FF0FF309FE713B500F546 +:101CD00080540191E06D00F039FD1F280AD9019967 +:101CE000E06D202200F0A8FDA0F1200358425841E9 +:101CF00002B010BD0020FBE708B500F58050FFF7EB +:101D000041FFC06D00F0F6FCBDE80840FFF740BFA2 +:101D100000220260027342608260704710B50022A8 +:101D20000023C0E900230023044603810C30FFF7A1 +:101D3000EFFF204610BD0000F0B5054600F58050CD +:101D40000C4690F8C83013F0040FC3F3800108BFAD +:101D5000114661F3820304F1840680F8C83005EB74 +:101D6000461389B01B79D8072ED57AB319072DD41D +:101D70006846FFF7D3FF05EB441303F5835303F1E4 +:101D8000180703AA103318685968144603C40833A7 +:101D9000BB422246F7D1186820601B792371DDE928 +:101DA0000E23CDE900230123ADF808302B686946E6 +:101DB000DB6B2846984705EB46152B791A075CBF65 +:101DC00043F008032B7101E0002AF4D109B0F0BD03 +:101DD0002DE9F047074688B007F5805468469A46D3 +:101DE0008846FFF7CFFE9146FFF798FFE06D00F0C1 +:101DF00093FC1F2829D9E06D2022694600F09EFD42 +:101E0000202822D103AD444605AB2E4603CE9E4288 +:101E100020606160354604F10804F6D13068206026 +:101E200033792371DDE90023C9E90023BDF80830C7 +:101E3000AAF80030FFF7ACFE4A4653464146384602 +:101E400008B0BDE8F04700F0D9BBFFF7A1FE0020C5 +:101E500008B0BDE8F08700002DE9F84F0023C0E985 +:101E60000133254B044640F8183B0F46FFF750FF5F +:101E700004F12800FFF752FF04F1480804F58255E9 +:101E80004646083530462036FFF748FFAE42F9D1C6 +:101E900004F580554FF480534FF00009C5E913391C +:101EA000C5F848800123EE6504F5875804F584568B +:101EB000C5F8549085F8583085F86030083608F138 +:101EC00008084FF0000A4FF0000B46E908ABA6F1F6 +:101ED0001800FFF71DFF203646F8289C4645F4D130 +:101EE00085F8C97017B1054800F076FD044B6361B1 +:101EF0002046BDE8F88F00BF345100080C5100089F +:101F00000064004010B5044B197804464A1C1A704E +:101F1000FFF7A2FF204610BDF43400202DE9F04762 +:101F2000002950D0294B2A4FB7FBF1F599428CBFBD +:101F30000A231123581EB5FBF3FC03FB1C53C4B248 +:101F40002BB102280346F5D80020BDE8F0870CF13C +:101F5000FF36B6F5806FF7D2C4EBC40E0EF1030363 +:101F60004FEAE309C3F3C703A4EB030809F1010A2D +:101F70004FF47A755FFA88F009FB05555AFA88F82C +:101F8000B5FBF8F5B5F5617FC1BF0EF1FF33C3F3C3 +:101F9000C703E01AC0B25C1C50FA84F40CFB04F4D2 +:101FA000B7FBF4F4A142CFD1013BDBB20F2BCBD86E +:101FB0000138C0B20728C7D800211071168091706F +:101FC000D3700120C1E70846BFE700BF3F420F00C2 +:101FD00000B4C40470B505460E464FF47A746B69BC +:101FE0005B6803F00103B34207D04FF47A7002F04C +:101FF000BDF8013CF3D1204670BD0120FCE7000094 +:1020000030B54269936913F0700F16D000230B4C62 +:10201000936103F1840200EB421211794D0709D557 +:10202000890707D5416954F823508D60117941F033 +:10203000040111710133032BEBD130BD2051000895 +:1020400073B51D46436916469A68D207044609D5FA +:102050009A6801219960C2F34002CDE90065002130 +:10206000FFF76AFE63699A68D1050BD59A684FF449 +:1020700080719960C2F34022CDE9006501212046BC +:10208000FFF75AFE63699A68D2030BD59A684FF43A +:1020900080319960C2F34042CDE9006502212046BB +:1020A000FFF74AFE204602B0BDE87040FFF7A8BF28 +:1020B000F8B50446466900296CD106F10C0738686A +:1020C00080076AD006EB01153868D5F8B00110F02A +:1020D000040FD5F8B0011ABFC00840F00040400D11 +:1020E000A061D5F8B0C11CF0020F1CBF40F08040C9 +:1020F000A061D5F8B40106EB011100F00F0084F8DF +:102100002400D1F8B8012077D1F8B801000A60772F +:10211000D1F8B801000CA077D1F8B801000EE07733 +:10212000D1F8BC0184F82000D1F8BC01000A84F881 +:102130002100D1F8BC01000C84F82200D1F8BC11B8 +:10214000090E84F823103821396004F1340004F1B9 +:10215000180104F1240551F8046B40F8046BA942FE +:10216000F9D109780170C4E90A23214600232386A6 +:1021700051F8283B2046DB6B984704F580522046F7 +:1021800092F8C83043F0040382F8C830BDE8F84044 +:10219000FFF736BF06F1100791E7F8BD10B504460A +:1021A00000F022FC02460B4652EA030102D0013A3B +:1021B00063F100030449086820B12146BDE81040DE +:1021C000FFF776BF10BD00BFF0340020F8B500F572 +:1021D00083511E46FFF7D6FCDFF844C008310024C7 +:1021E00004F1840500EB45152B795F070ED4DB065F +:1021F0000CD5D1E900739742B34107D243695CF82B +:1022000024709F602B7943F004032B710134032C5D +:1022100001F12001E4D1BDE8F840FFF7B9BC00BFEF +:102220002051000808B5FFF7ADFCFFF7E9FEBDE857 +:102230000840FFF7ADBC0000F8B543690546986853 +:1022400000F0E050B0F1E05F0F461FD0E8B1FFF7BB +:1022500099FC05F583541034002606F1840305EB40 +:1022600043131B791A0706D50136032E04F1200407 +:10227000F3D1012007E05B07F6D42146384600F091 +:1022800009FA0028F0D1FFF783FCF8BD0120FCE734 +:1022900000F5805008B5FFF775FCC06D00F03CFA02 +:1022A000FFF776FC43090CBF0120002008BD0000A9 +:1022B000F8B51D46002313700F4606461446FFF777 +:1022C000E7FF80F00100387025B129463046FFF75E +:1022D000B3FF2070F8BD00002DE9B8410C4615464B +:1022E0001F46804600F080FB0B462178024609B964 +:1022F000287850B14046FFF769FFFFF793FF3B4650 +:102300002A462146FFF7D4FF0120BDE8B88100002E +:1023100010B5FFF737FC174B9A6D42F000729A65C3 +:102320009A6B42F000729A639A6B00F5805422F027 +:1023300000729A63FFF72CFC94F8C830DB0718D4BE +:10234000B9B102211320FFF71DFC01F0F7FB0221B8 +:10235000142001F0F3FB0221152001F0EFFB94F8AB +:10236000C83043F0010384F8C830BDE81040FFF7DF +:102370000FBC10BD001002402DE9F04700F580555C +:1023800088B095F8C930012B04468846164600F2FD +:10239000FB807E4F57F823200AB947F82300D7F86F +:1023A00000A0C4F80C802674BAF1000F09D141F2E4 +:1023B000D00002F0BBFC51468146FFF74DFDC7F847 +:1023C000009095F8C930012B5DD001212046FFF720 +:1023D0009FFFFFF7D7FB6269136823F002031360C6 +:1023E0006269136843F001031360636900275F614A +:1023F00001212046FFF7CCFBFFF7ECFD002800F0A1 +:102400008480E86D00F076F904F58359BA4609F145 +:102410000809202200216846FFF72AFB02A8FFF7DF +:1024200077FCCDF818A06A4609EB07030DF1180EEA +:102430009446BCE80300F44518605960624603F115 +:102440000803F5D1DCF80000186020379CF8042060 +:102450001A71602FDDD195F8C8306FF38203002721 +:1024600085F8C8306A4641462046ADF80070ADF8A0 +:1024700002708DF80470FFF751FD6369C0B94FF425 +:1024800000421A6011E0386803681B6B98470146E8 +:1024900000289AD13868FFF73BFF38680368324656 +:1024A0001B684146984700288FD108B0BDE8F087E7 +:1024B00061221A609DF802309DF803201B06120469 +:1024C00002F4702203F040731343BDF80020C2F3FE +:1024D000090213439DF804201205022E02F4E002C3 +:1024E0000CBF4FF000410021134362690B43D361DD +:1024F000636913225A616269136823F00103136050 +:1025000039462046FFF766FD08B96369B7E795F8D5 +:10251000C93093BB6169D1F8002242F00102C1F8D1 +:1025200000226169D1F8002222F47C5222F00E02CE +:10253000C1F800226169D1F8002242F46062C1F85A +:1025400000226269C2F814326269C2F80432626918 +:1025500041F6FF71C2F80C126269C2F8403262693A +:10256000C2F8443263690122C3F81C226269D2F8BE +:10257000003223F00103C2F8003295F8C83043F06E +:10258000020385F8C83090E700208EE7F034002081 +:1025900008B500F029FA50EA0103024602D0421EB3 +:1025A00061F10001044B186810B10B46FFF748FDBC +:1025B000BDE8084001F01CB9F034002008B5002047 +:1025C000FFF7ECFDBDE8084001F012B908B50120A5 +:1025D000FFF7E4FDBDE8084001F00AB90FB40020A0 +:1025E00004B0704713B56C4684E80600031D94E8F8 +:1025F000030083E80500012002B010BD73B58568B3 +:10260000019155B11B885B0707D4D0E900369B6B5D +:102610009847019AC1B23046A847012002B070BD68 +:10262000F0B5866889B005460C465EB1BDF8383015 +:102630005B070AD4D0E900379B6B98472246C1B2AA +:102640003846B047012009B0F0BD00220023CDE993 +:1026500000230023ADF808300A4603AB01F1080659 +:10266000106851681C4603C40832B2422346F7D1B1 +:10267000106820601279227100F0B8F90423ADF8D7 +:1026800008302B68CDE90001DB6B69462846984786 +:10269000D8E7000030B503680968DD0FB5EBD17FDE +:1026A00023F0604421F060424FEAD1700BD0002B40 +:1026B000B8BFA40C0029B8BF920C944202D034BF1A +:1026C0000120002030BD944205D1C1F38070C3F3D6 +:1026D00080738342F6D194422CBF00200120F1E7A1 +:1026E00010B5037C044613B9006802F057FB20467E +:1026F00010BD00000023BFF35B8FC360BFF35B8F8F +:10270000BFF35B8F8360BFF35B8F7047BFF35B8F5B +:102710000068BFF35B8F704770B505460C30FFF75C +:10272000F5FF05F1080604463046FFF7EFFFA0422B +:1027300006D930466D68FFF7E9FF2544281A70BDB9 +:102740003046FFF7E3FF201AF9E7000070B50546B1 +:10275000406898B105F10800FFF7D8FF05F10C06B5 +:1027600004463046FFF7D2FF8442304694BF6D687E +:102770000025FFF7CBFF013C2C44201A70BD000060 +:1027800038B50C460546FFF7C7FFA04210D305F148 +:102790000800FFF7BBFF04446868B4FBF0F100FBDE +:1027A0001144BFF35B8F0120AC60BFF35B8F38BD7A +:1027B0000020FCE72DE9F041144607460D46FFF7DF +:1027C000C5FF844228BF0446D4B1B84658F80C6B04 +:1027D0004046FFF79BFF3044286040467E68FFF785 +:1027E00095FF331A9C4203D86C600120BDE8F0814C +:1027F0006B60A41B3B68AB602044E8600220F5E7F7 +:102800002046F3E738B50C460546FFF79FFFA04288 +:1028100010D305F10C00FFF779FF04446868B4FB9E +:10282000F0F100FB1144BFF35B8F0120EC60BFF3BC +:102830005B8F38BD0020FCE72DE9FF4188466946E3 +:102840000746FFF7B7FF6C4606B204EBC606002545 +:10285000B44209D06268206808EB0501FFF7F6F87A +:10286000636808341D44F3E729463846FFF7CAFF7A +:10287000284604B0BDE8F081F8B505460C300F4697 +:10288000FFF744FF05F1080604463046FFF73EFF18 +:10289000A042304688BF6C68FFF738FF201A3860C6 +:1028A00020B130462C68FFF731FF2044F8BD00000E +:1028B00073B5144606460D46FFF72EFF844228BF27 +:1028C00004460190DCB101A93046FFF7D5FF019B1A +:1028D00033B93268C5E90233C5E9002401200CE0B0 +:1028E0009C4238BF01942860019868608442F5D901 +:1028F0003368AB60241AEC60022002B070BD204641 +:10290000FBE700002DE9FF410F466946FFF7D0FFC6 +:102910006C4600B204EBC0050026AC4209D0D4F8E6 +:10292000048054F8081BB8194246FFF78FF8464454 +:10293000F3E7304604B0BDE8F081000038B5054645 +:10294000FFF7E0FF044601462846FFF719FF20463F +:1029500038BD000000B59BB0EFF3098168226846DE +:10296000FFF774F8EFF30583044B9A6BDA6A9A6AFF +:102970009A6A9A6A9A6A9A6A9B6AFEE700ED00E090 +:1029800000B59BB0EFF3098168226846FFF75EF857 +:10299000EFF30583044B9A6B9A6A9A6A9A6A9A6A69 +:1029A0009A6A9B6AFEE700BF00ED00E000B59BB0AD +:1029B000EFF3098168226846FFF748F8EFF30583D3 +:1029C000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE7FA +:1029D00000ED00E0FEE700000FB408B5029801F03A +:1029E0005FFBFEE701F0EEBD01F0C6BD01F0C4BD26 +:1029F00030B5094D0A4491420DD011F8013B5840C1 +:102A0000082340F30004013B2C4013F0FF0384EA49 +:102A10005000F6D1EFE730BD2083B8ED2DE9F0414D +:102A2000C56915B9C161BDE8F0814B6823F0604705 +:102A3000C3F38A464FEAD37EC3F3807816EA2306AF +:102A400038BF3E46AC462B465A68BEEBD27F22F0DA +:102A500060440AD0002A18DAA40CB44217D19D426F +:102A60000FD10D60DEE71346EEE7A74207D102F073 +:102A70008044C2F3807242450BD054B1EFE708D2D4 +:102A8000EDE7CCF800100B60CDE7B44201D0B442C2 +:102A9000E5D81A689C46002AE5D11960C3E7000012 +:102AA0002DE9F047089D01F007044FEAD5082244BC +:102AB00005F0070500EBD1004FF47F49944201D1A6 +:102AC000BDE8F08704F0070705F0070A57453E46C2 +:102AD00038BF5646C6F10806111B8E4228BF0E4667 +:102AE000E10808EBD50E415C13F80EC0B94029FA95 +:102AF00006F721FA0AF1FFB28CEA010147FA0AF758 +:102B000039408CEA010C03F80EC034443544D5E753 +:102B100080EA0120082341F2210201B2400000298D +:102B200080B203F1FF33B8BF504013F0FF03F4D17C +:102B30007047000038B50C468D18A54200D138BD4D +:102B400014F8011BFFF7E4FFF7E7000002684AB141 +:102B500013680360C388018901339BB29942C38023 +:102B600038BF03811046704770B588B020220446F4 +:102B70000D4668460021FEF77BFF20460495FFF7CF +:102B8000E5FF024658B16B46054608AE1C4603CC2D +:102B9000B44228606960234605F10805F6D1104665 +:102BA00008B070BD10B54B6823B9CA8A63F3090237 +:102BB000CA8210BDC4681A681C60C360438A013BA6 +:102BC00043824A60EFE700002DE9F84F1D46CB8AAB +:102BD0000F46C3F309010629814692460B4630D0C1 +:102BE0000020AAB207F119049EB2052E1FFA80F840 +:102BF0000FD8904503F1010306D3FB8A0A4462F320 +:102C00000903FB8201201AE01AF80060E654013043 +:102C1000EAE79045F1D2A1F1060B1C237C68BBFBCF +:102C2000F3F203FB12BB1FFA8BF6002C45D148468A +:102C3000FFF78CFF044638B978606FF00200BDE8FA +:102C4000F88F4FF00008E6E7002606607860ADB226 +:102C50004FF0000B454510D90AEB0803221D13F86D +:102C6000011B9155B1B208F101081B291FFA88F820 +:102C70002BD0454506F10106F1D8FB8AC3F30902C2 +:102C8000154465F30903BCE7013292B21C46236880 +:102C9000002BF9D1AB1F0B441C21B3FBF1F3013323 +:102CA0009BB29A42D3D2BBF1000FD0D14846FFF776 +:102CB0004DFF20B9C4F800B0BFE70122E7E7C0F834 +:102CC00000B05E4620600446C1E74545D5D9484678 +:102CD000FFF73CFF08B92060AFE7C0F800B000265E +:102CE00020600446B6E700002DE9F04F2DED028B81 +:102CF00083B0CDE90013BDF83C4080469246002ADF +:102D000000F087802CB10E9B002B00F08280072CF6 +:102D10002BD808F10C00FFF719FF054638B96FF002 +:102D20000205284603B0BDEC028BBDE8F08F1422EB +:102D30000021FEF79DFE22460E9905F10800FEF7E0 +:102D400085FE631C2B749AF800302C4403F01F039B +:102D500063F03F032372009B43F0004169604046EB +:102D60002946FFF75BFE0125DBE700F10C034FF07E +:102D7000000908EE103A4FF0800B4E464D4618EE13 +:102D8000100AFFF7E3FE07460028C8D014220021EE +:102D9000FEF76EFE002E3AD1019B3B8102220E9B74 +:102DA00002F1080103EB060C57FA81F11046B44218 +:102DB00002F10102D2B201D8024607E01CF8010B71 +:102DC00001F8010B0136062AB6B2EFD99AF80010C5 +:102DD00001F01F01B44208BF4FF0400BB81841EAA0 +:102DE00049114BEA01030372009B013243F0004397 +:102DF0007B603A7439464046FFF710FE0135B44215 +:102E00002DB289F001094FF0000BB8D189E70022FB +:102E1000C5E76FF0010584E7F8B515460E46242294 +:102E2000002104461F46FEF723FE069B6360B5F5AE +:102E3000001F079BA76034BF6A094FF6FF72236229 +:102E400004F10C0097B200239A4205D800230360D6 +:102E500027826382A382F8BD0660013330462036A4 +:102E6000F2E7000003781BB94BB2002BC8BF01701A +:102E700070470000007870472DE9F74FDDF83C906F +:102E8000BDF830500D9E9DF83840BDF8407080462A +:102E900092469B46B9F1000F01D1002F51D11F2C52 +:102EA0004FD898F80000B0B9072F47D835F0030382 +:102EB00047D13A4649464FF6FF70FFF73BFE20F0F8 +:102EC00001002D02400445EA0464400C44EA402419 +:102ED0004FF6FF7321E040EA0520072F40EA046423 +:102EE000F6D900254FF6FF73C5F12000A5F12002A9 +:102EF0002AFA05F10BFA00F001432BFA02F2114312 +:102F00001846C9B2FFF704FE0835402D0346EBD141 +:102F10003A464946FFF70EFE0346CDE90097324692 +:102F200021464046FFF7E0FE33780133DBB21F2B2A +:102F300088BF0023337003B0BDE8F08F6FF003004B +:102F4000F9E76FF00100F6E72DE9F04F85B0924602 +:102F5000DDF848800F9D9DF840209DF84490BDF815 +:102F60004C7006469B46B8F1000F01D1002F48D1A6 +:102F70001F2A46D83378002B46D00C0244EA02645C +:102F80009DF8381044EAC93444EA01441C43072F31 +:102F900044F0800432D900234FF6FF72C3F1200CB5 +:102FA000A3F120002AFA03F10BFA0CFC41EA0C0110 +:102FB0002BFA00F00143C9B210460393FFF7A8FDB6 +:102FC000039B0833402B0246E8D13A464146FFF7BF +:102FD000B1FD0346CDE900872A4621463046FFF77A +:102FE00083FEB9F1010F06D12B780133DBB21F2B21 +:102FF00088BF00232B7005B0BDE8F08F4FF6FF733C +:10300000E8E76FF00100F6E76FF00300F3E7000078 +:10301000C06900B104307047C3691A68C261C268F0 +:103020001A60C360438A013B438270472DE9F04137 +:10303000D0F81880194E14461D464146002709B99C +:10304000BDE8F081D1E90223A21A65EB03039642A1 +:1030500077EB03031ED283698B420DD1FFF7A2FDEC +:1030600083691B688361C3680B60438AC16081699F +:10307000013B43828846E2E7FFF794FD0B68C8F8FE +:103080000030C3680B60438AC160013B4382D8F8BB +:103090000010D4E788460968D1E700BF80841E008D +:1030A0002DE9F04F8BB00D46DDF8509014469B464D +:1030B0008046002800F01981B9F1000F00F0158159 +:1030C000531E3F2B00F21181012A03D1BBF1000FE7 +:1030D00040F00B810023CDE90833B8F81430B5EB8C +:1030E000C30F4FEAC30703D300200BB0BDE8F08F36 +:1030F0002B199F42D8F80C303ABF7F1BFFB22746EE +:103100001BB9D8F81030002B7AD02F2D4ED8C5F12E +:103110003006B7424FF000032CBFF6B23E46009394 +:103120002946D8F8080008AB3246FFF7B9FCA7EBF0 +:10313000060A35445FFA8AFAB8F8143003F10053EE +:10314000063BDB000493D8F80C3003933021039B3B +:1031500013B1BAF1000F2CD1D8F8100040B1BAF178 +:10316000000F05D0009608AB5246691AFFF798FC8D +:1031700038B2002FB8D066070AD00AAB03EBD401EF +:10318000624211F8083C02F00702134101F8083CC2 +:10319000082C3CD9102C40F2B580202C40F2B7808E +:1031A000BBF1000F00F09C80082334E0BA460026F3 +:1031B000C2E7049BE02B28BFE02306930B44AB42FD +:1031C000059314D95A1B03980096924534BF524672 +:1031D000D2B2691A08AB04300792FFF761FC079A74 +:1031E0001644AAEB020A1544F6B25FFA8AFA049B67 +:1031F000069A05999B1A0493039B1B680393A6E701 +:103200000093D8F8080008AB3A462946AEE7BBF170 +:10321000000F13D00123B4EBC30F6CD0082C12D8CD +:103220009DF82030621E23FA02F2D50706D54FF032 +:10323000FF3202FA04F423438DF820309DF8203049 +:1032400089F8003051E7102C12D8BDF82030621EEA +:1032500023FA02F2D10706D54FF0FF3202FA04F446 +:103260002343ADF82030BDF82030A9F800303CE70A +:10327000202C0FD80899631E21FA03F3DA0705D52D +:103280004FF0FF3202FA04F40C430894089BC9F88B +:1032900000302AE7402C2BD0DDE90865611EC4F11F +:1032A0002102A4F1210326FA01F105FA02F225FA1E +:1032B00003F311431943CB0712D50122A4F12003D4 +:1032C000C4F1200102FA03F322FA01F1A2405242B2 +:1032D00043EA010363EB430332432B43CDE9082365 +:1032E000DDE90823C9E90023FFE66FF00100FCE6F1 +:1032F0006FF00800F9E6082CA0D9102CB3D9202CC7 +:10330000EED8C3E7BBF1000FADD0022383E7BBF1DA +:10331000000FBBD004237EE730B5012A144638BF26 +:103320000124402C85B028BF40240025012ACDE986 +:10333000025518D81B788DF8083063070AD004AB03 +:1033400003EBD405624215F8083C02F007029340F3 +:1033500005F8083C009103462246002102A8FFF729 +:103360009FFB05B030BD082AE4D9102A03D81B887A +:10337000ADF80830E1E7202A8DBFD3E900231B68B0 +:103380000293CDE90223D8E710B5CB681BB98B6057 +:103390000B618B8210BDC4681A681C60C360438ACD +:1033A000013B4382CA60F0E72DE9F04FD1F800807D +:1033B00093B018F0800FCDE90323C8F3C01219BFF2 +:1033C000C8F3C03BC8F306264FF0020B1646B8F10F +:1033D000000F04460D4680F2C58118F0C0430593E6 +:1033E00040F0C0810B7B002B00F0BC81BBF1020FD1 +:1033F00003D00178B14240F0B88108F07F01069116 +:103400006AB3C8F3074A2B44069A93F803907606EA +:1034100046EA0B4646EA82465FEAD91346EA0A06BE +:10342000079300F0908000220023CDE90A23069B39 +:10343000009367685B4652460AA92046B8470028B1 +:103440007ED0A7699FB9314604F10C00FFF78CFBD1 +:103450000746E0B96FF0020013B0BDE8F08FC8F383 +:103460000F2A18F07F0F08BF0AF0030ACBE73B6969 +:103470009E420DD03F68002FF9D1314604F10C0077 +:10348000FFF772FB07460028E4D0A3693B60A76101 +:10349000DDE90A2300264FF6FF70C6F1200E22FA5E +:1034A00006F103FA0EFEA6F1200C23FA0CFC41EA09 +:1034B0000E0141EA0C01C9B2083609920893FFF7E0 +:1034C00027FB402EDDE90832E7D1B882FB7D09F009 +:1034D0001F06C3F38403F31AD7E9022198B2002B25 +:1034E000BCBF00F120031BB252EA0100C8F304681C +:1034F0000FD00398821A049860EB0101A148904212 +:103500004FF000028A4104D3079A002A55D0012BBC +:1035100023DDFA7D4FEA890302F0030203F07C0306 +:103520001343FB7539462046FFF73CFB079BA3B9C5 +:10353000FB7DC3F38402013262F38603FB7504E072 +:103540006FF00B0088E7A76917B96FF00C0083E7ED +:103550003B699E42BAD03F68F6E719F0400F2CD085 +:10356000039BBB60049BFB60142200210DA8FEF7A7 +:103570007FFA039B0A93049B0B932B1D0C932B7BCD +:10358000ADF83EA0013BDBB2ADF83C30069B8DF8B8 +:1035900043308DF840B0A3688DF841608DF84280CB +:1035A0000AA920469847FB7DC3F38403013303F047 +:1035B0001F039B02FB8200204EE7FB7DC9F34012F4 +:1035C000B2EBD31F40F0D480C3F38403B34240F086 +:1035D000D28007992B7B4FEA9912002934D0D20769 +:1035E00041D4032B40F2CA80039BBB60049BFB6069 +:1035F0002B7BAE1D033BDBB23246394604F10C0097 +:10360000FFF7E2FA00280DDA20463946FFF7CAFA3A +:10361000FB7DC3F38403013303F01F039B02FB8292 +:10362000032019E7AB883B832A7B033AB88AD2B2DE +:103630003146FFF77FFAFB7DB882DA43C2F3C0124E +:1036400062F3C713FB75B6E76AB92E1D013BDBB207 +:103650003246394604F10C00FFF7B6FA0028D3DBF6 +:103660002A7B013AE2E7F98AC1F30901013B052906 +:10367000DAB253D8281D002307F11A0C9A4208D950 +:1036800010F801EB0CF801E0013101330629DBB23F +:10369000F4D103990A9104990B91934207F11A010D +:1036A0000C9138BF043379680D9134BF55FA83F318 +:1036B00000230E93FB8AADF83EA0C3F309031A441E +:1036C000069B8DF843300023ADF83C208DF840B0C8 +:1036D0008DF841608DF842807B602A7BB88A013A80 +:1036E000291DFFF727FA3B8BB882834203D1A368D9 +:1036F0000AA92046984720460AA9FFF745FEFB7D08 +:10370000B88AC3F38403013303F01F039B02FB82D7 +:103710003B8B984214BF112000209DE67B68002B54 +:10372000B7D0062001E01C306346D3F800C0BCF1DE +:10373000000FF8D1091A081D05F1040C00EB03096C +:1037400005989DF8143001EB000EBEF11B0FA0D8B8 +:103750009A429ED91CF8013B09F8013B059B0133B5 +:103760000593EDE76FF0090076E66FF00A0073E667 +:103770006FF00D0070E66FF00E006DE66FF00F0059 +:103780006AE600BF80841E00EFF3098305494A6B97 +:1037900022F001024A63683383F30988002383F32C +:1037A0001188704700EF00E0202080F3118862B696 +:1037B0000C4B0D4AD96821F4E0610904090C0A4355 +:1037C000DA60D3F8FC20094942F08072C3F8FC208B +:1037D0000A6842F001020A601022DA7783F82200B8 +:1037E000704700BF00ED00E00003FA05001000E0A4 +:1037F00010B5202383F311880E4B5B6813F400632C +:1038000014D0F1EE103AEFF30984683C4FF0807366 +:10381000E361094BDB6B236684F3098800F0F2FB5C +:1038200010B1064BA36110BD054BFBE783F3118874 +:10383000F9E700BF00ED00E000EF00E00B06000834 +:103840000E060008026843681143016003B118477F +:1038500070470000024A136843F0C00313607047CA +:1038600000380140024A136843F0C00313607047F8 +:1038700000440040024A136843F0C00313607047DD +:103880000048004037B5244C244D204600F0FCFA97 +:10389000009404F1140022490023202200F0BEF914 +:1038A00020490094202204F138001F4B00F038FA20 +:1038B0001E4BC4E917351E4C204600F0E5FA009473 +:1038C00004F114001B490023202200F0A7F91A4933 +:1038D0000094202204F13800184B00F021FA184B14 +:1038E000C4E91735174C204600F0CEFA04F1140055 +:1038F000154900940023202200F090F9134B14493D +:103900000094202204F1380000F00AFA114BC4E9B7 +:10391000173503B030BD00BFFC34002000B4C40430 +:1039200040360020A03600205538000800380140FD +:103930006835002060360020C03600206538000859 +:1039400000440040D435002080360020753800083F +:10395000E03600200048004030B5037C304C0029A0 +:1039600018BF0C46012B0FD12E4B984239D12E4B4C +:103970001A6E42F480421A66D3F8802042F48042E4 +:10398000C3F88020D3F880302268036EC16D84664E +:1039900003EB5203B3FBF2F36268150442BF23F05A +:1039A000070503F0070343EA4503CB60A36843F030 +:1039B00040034B60E36843F001038B6042F496736D +:1039C00043F001030B604FF0FF330B62510505D547 +:1039D00012F010221FD0B2F1805F1ED080F8643048 +:1039E00030BD124B98420BD0114B9842CCD10E4BAC +:1039F0009A6D42F480229A659A6F42F4802207E021 +:103A0000094B9A6D42F400329A659A6F42F4003283 +:103A10009A679B6FB8E77F23E0E73F23DEE700BFAD +:103A200074510008FC34002000100240683500206A +:103A3000D43500202DE9F047C66D3768F46934624B +:103A40002107054618D014F0080118BF8021E207AD +:103A500048BF41F02001A30748BF41F04001600783 +:103A600048BF41F48071202383F31188281DFFF79C +:103A7000E9FE002383F31188E2050AD5202383F3AE +:103A800011884FF40071281DFFF7DCFE002383F33B +:103A900011884FF020094FF0000A14F0200838D1A7 +:103AA0003B0616D54FF0200905F1380A200610D53F +:103AB00089F31188504600F067F9002836DA0821AA +:103AC000281DFFF7BFFE27F080033360002383F338 +:103AD0001188790614D5620612D5202383F3118844 +:103AE000D5E913239A4208D12B6C33B11021281D3C +:103AF00027F04007FFF7A6FE3760002383F3118805 +:103B0000E30619D5AA6E1369B3B1BDE8F047506951 +:103B1000184789F31188B38C95F86410284619402A +:103B200000F0CCF98AF31188F469B6E780B23085E9 +:103B300088F31188F469B9E7BDE8F08700F16043C4 +:103B400003F561430901C9B283F80013012200F0B3 +:103B50001F039A4043099B0003F1604303F561434F +:103B6000C3F880211A607047F8B515468268066967 +:103B7000AA420B46816938BF8568761AB542044669 +:103B80000BD218462A46FDF761FFA3692B44A361B7 +:103B9000A3685B1BA3602846F8BD0CD918463246C3 +:103BA000FDF754FFAF1BE1683A463044FDF74EFF86 +:103BB000E3683B44EBE718462A46FDF747FFE36816 +:103BC000E5E7000083689342F7B51546044638BF21 +:103BD0008568D0E90460361AB5420BD22A46FDF753 +:103BE00035FF63692B446361A36828465B1BA360B0 +:103BF00003B0F0BD0DD932460191FDF727FF0199C1 +:103C0000E068AF1B3A463144FDF720FFE3683B44D0 +:103C1000E9E72A46FDF71AFFE368E4E710B50A442E +:103C20000024C361029B8460C0E90000C0E9051163 +:103C3000C1600261036210BD08B5D0E9053293424C +:103C400001D1826882B98268013282605A1C426165 +:103C50001970D0E904329A4224BFC368436100213D +:103C600000F0A0FA002008BD4FF0FF30FBE7000095 +:103C700070B5202304460E4683F31188A568A5B1CC +:103C8000A368A269013BA360531CA3611578226954 +:103C9000934224BFE368A361E3690BB120469847D0 +:103CA000002383F31188284607E03146204600F0C0 +:103CB00069FA0028E2DA85F3118870BD2DE9F74F23 +:103CC00004460E4617469846D0F81C904FF0200A3E +:103CD0008AF311884FF0000B154665B12A4631462C +:103CE0002046FFF741FF034660B94146204600F0F9 +:103CF00049FA0028F1D0002383F31188781B03B020 +:103D0000BDE8F08FB9F1000F03D001902046C847FD +:103D1000019B8BF31188ED1A1E448AF31188DCE7AE +:103D2000C0E90511C160C3611144009B8260C0E914 +:103D30000000016103627047F8B504460D4616465F +:103D4000202383F31188A768A7B1A368013BA36070 +:103D500063695A1C62611D70D4E904329A4224BF1F +:103D6000E3686361E3690BB120469847002080F364 +:103D7000118807E03146204600F004FA0028E2DA14 +:103D800087F31188F8BD0000D0E905239A4210B5E9 +:103D900001D182687AB98268013282605A1C8261DC +:103DA0001C7803699A4224BFC3688361002100F034 +:103DB000F9F9204610BD4FF0FF30FBE72DE9F74F32 +:103DC00004460E4617469846D0F81C904FF0200A3D +:103DD0008AF311884FF0000B154665B12A4631462B +:103DE0002046FFF7EFFE034660B94146204600F04B +:103DF000C9F90028F1D0002383F31188781B03B0A0 +:103E0000BDE8F08FB9F1000F03D001902046C847FC +:103E1000019B8BF31188ED1A1E448AF31188DCE7AD +:103E2000026843681143016003B1184770470000FE +:103E30001430FFF743BF00004FF0FF331430FFF79B +:103E40003DBF00003830FFF7B9BF00004FF0FF332F +:103E50003830FFF7B3BF00001430FFF709BF000090 +:103E60004FF0FF311430FFF703BF00003830FFF789 +:103E700063BF00004FF0FF323830FFF75DBF000036 +:103E800000207047FFF7FEBC044B03600023C0E92D +:103E90000233436001230374704700BF8C51000854 +:103EA00010B52023044683F31188FFF755FD022344 +:103EB0002374002383F3118810BD000038B5C36953 +:103EC00004460D461BB904210844FFF7A9FF294603 +:103ED00004F11400FFF7B0FE002806DA201D4FF4AD +:103EE0008061BDE83840FFF79BBF38BD024B002220 +:103EF000C3E900339A607047003700200023037441 +:103F00008268054B1B6899689142FBD25A6803602E +:103F100042601060586070470037002008B52023C9 +:103F200083F31188037C032B05D0042B0DD02BB910 +:103F300083F3118808BD436900221A604FF0FF33F4 +:103F40004361FFF7DBFF0023F2E7D0E900321360A3 +:103F50005A60F3E7002303748268054B1B68996875 +:103F60009142FBD85A680360426010605860704705 +:103F700000370020054B19690874186802681A6038 +:103F80005360186101230374FCF72ABB003700203B +:103F900030B54B1C0B4D87B0044610D02B690A4A34 +:103FA00001A800F019F92046FFF7E4FF049B13B1C4 +:103FB00001A800F04DF92B69586907B030BDFFF733 +:103FC000D9FFF8E7003700201D3F000838B50C4D39 +:103FD00041612B6981689A689142044603D8BDE823 +:103FE0003840FFF78BBF1846FFF7B4FF01232C6161 +:103FF000014623742046BDE83840FCF7F1BA00BF03 +:1040000000370020044B1A681B6990689B6898422F +:1040100094BF0020012070470037002010B5084CE5 +:10402000236820691A6822605460012223611A748F +:10403000FFF790FF01462069BDE81040FCF7D0BAB9 +:104040000037002008B5FFF7DDFF18B1BDE80840D4 +:10405000FFF7E4BF08BD0000FFF7E0BFFEE7000088 +:1040600010B50C4CFFF742FF00F0A8F80A49802277 +:10407000204600F03DF8012344F8180C0374FFF7C4 +:1040800093FB002383F3118862B60448BDE8104017 +:1040900000F04EB828370020B4510008C451000881 +:1040A00008B572B6034B586200F060FA00F00CFBE2 +:1040B000FEE700BF0037002000F004B9EFF31180E5 +:1040C00020B9EFF30583202282F3118870470000A6 +:1040D00010B530B9EFF30584C4F3080414B180F3CC +:1040E000118810BDFFF7AEFF84F31188F9E70000D7 +:1040F00082600222028270478368A3F17C0243F847 +:104100000C2C026943F83C2C426943F8382C074ACE +:1041100043F81C2CC26843F8102C022203F8082C28 +:10412000002203F8072CA3F118007047F9050008D6 +:1041300010B5202383F31188FFF7DEFF002104462A +:10414000FFF744FF002383F31188204610BD0000D1 +:10415000024B1B6958610F20FFF70CBF003700208E +:10416000202383F31188FFF7F3BF000008B5014651 +:10417000202383F311880820FFF70AFF002383F32D +:10418000118808BD49B1064B42681B6918605A6026 +:10419000136043600420FFF7FBBE4FF0FF30704711 +:1041A000003700200368984206D01A680260506009 +:1041B00059611846FFF7A2BE70470000054B03F196 +:1041C0001402C3E905224FF0FF310022C3E90712B0 +:1041D000704700BF0037002070B51C4EC0E90323B4 +:1041E00005460C4600F0EAFA334653F8142F9A427B +:1041F0000DD13062C5E901242A600A2C2CBF0019B8 +:104200000A30C6E90555BDE8704000F0C5BA316A0C +:10421000431AE31838BF1C469368A34202D9081911 +:1042200000F0C8FA73699A6894420CD85A68AC6076 +:104230002B606A6015609A685D60121B9A604FF08F +:10424000FF33F36170BD1B68A41AECE70037002050 +:1042500038B51B4C636998420DD0D0E90032136029 +:104260005A6000228168C2609A680A449A604FF0DE +:10427000FF33E36138BD2246036842F8143F002152 +:1042800093425A60C16003D1BDE8384000F08CBA57 +:104290009A688168256A0A449A6000F08FFA636917 +:1042A0009A68411B8A42E5D9AB181D1A092D206A6C +:1042B00098BF01F10A02BDE83840104400F07ABA14 +:1042C000003700202DE9F041184C002704F11406B6 +:1042D000656900F073FA236AAA68C11A8A4215D880 +:1042E00013442362D5E9003213605A606369D5F83C +:1042F0000C80EF60B34201D100F056FA87F31188C9 +:104300002869C047202383F31188E1E76169B1423E +:1043100009D013441B1ABDE8F0410A2B2CBFC0186A +:104320000A3000F047BABDE8F08100BF0037002036 +:104330000C2303604FF0FF307047000000207047EF +:10434000FEE70000704700004FF0FF3070470000AC +:10435000BFF34F8F024A1369DB03FCD4704700BFE1 +:104360000020024008B5094B1B7873B9FFF7F0FF36 +:10437000074B5A69002ABFBF064A9A6002F1883289 +:104380009A601A6822F480621A6008BD883800209A +:10439000002002402301674508B50B4B1B7893B9F9 +:1043A000FFF7D6FF094B5A6942F000425A611A687A +:1043B00042F480521A601A6822F480521A601A6815 +:1043C00042F480621A6008BD883800200020024054 +:1043D0007F289ABF00F58030C0020020704700009F +:1043E0004FF4006070470000802070477F2808B5B8 +:1043F0000BD8FFF7EDFF00F500630268013204D12E +:1044000004308342F9D1012008BD0020FCE7000000 +:104410007F2810B504461FD8FFF79AFFFFF7A2FFC9 +:104420000E4BF3221A6102225A615A6942EAC00213 +:104430005A615A6942F480325A61FFF789FF4FF49A +:104440000061FFF7C5FF00F03BF9FFF7A5FF20462D +:10445000BDE81040FFF7CABF002010BD0020024099 +:104460002DE9F84340EA020313F00703144606D08F +:104470002E4B40F2F5221A600020BDE8F883851823 +:104480002B4A95420CD9294A40F2FA211160F3E7F0 +:10449000031D1B684A68934208D1083C0830083164 +:1044A000072C14D902680B689A42F1D0FFF75AFF23 +:1044B000FFF74EFF1F4E08314FF001084FF0000983 +:1044C000012CA1F1080706D8FFF766FF01E0002CD8 +:1044D000ECD10120D1E7C6F81480054651F8083C1C +:1044E00045F8043B51F8043C4360FFF731FFC6F840 +:1044F0001490026851F8083C9A420CD00B4B4FF4D0 +:1045000048721A600C4B18600C4B1C600C4B1F60FF +:10451000FFF742FFB0E72D6851F8043C9D4201F1DE +:104520000801EBD1083C0830CAE700BF84380020FE +:104530000000040800200240783800208038002065 +:104540007C380020084908B50B7828B11BB9FFF763 +:1045500009FF01230B7008BD002BFCD0BDE808400B +:104560000870FFF719BF00BF883800204FF4803172 +:104570004FF0005000F0A6B870B582B0FFF79EFD76 +:104580000E4E054600F01AF93268904237BF0C4AC9 +:104590000B49516814682EBFD1E9004101315160C7 +:1045A0000419034641F10001284601913360FFF7E9 +:1045B0008FFD0199204602B070BD00BF8C380020ED +:1045C0009038002070B582B0FFF778FD104E054698 +:1045D00000F0F4F83268904237BF0E4A0D49516836 +:1045E00014682EBFD1E9004101315160041941F135 +:1045F00000010346284601913360FFF769FD0199E8 +:104600004FF47A7200232046FBF7D2FD02B070BD52 +:104610008C3800209038002010B50244064BD2B2EE +:10462000904200D110BD441C00B253F8200041F864 +:10463000040BE0B2F4E700BF50280040114B30B546 +:10464000D3F89040240409D4D3F89040C3F89040A4 +:10465000D3F8904044F40044C3F890400A4C2368D7 +:1046600043F4807323600244084BD2B2904200D1DD +:1046700030BD441C00B251F8045B43F82050E0B256 +:10468000F4E700BF001002400070004050280040D6 +:1046900007B5012201A90020FFF7BEFF019803B072 +:1046A0005DF804FB13B50446FFF7F2FFA04205D006 +:1046B000012201A900200194FFF7C0FF02B010BD44 +:1046C000704700007047000070470000074B45F23C +:1046D00055521A6002225A6040F6FF729A604CF6F8 +:1046E000CC421A60024B01221A7070470030004021 +:1046F0009C380020034B1B781BB1034B4AF6AA22BF +:104700001A6070479C38002000300040054B1A6842 +:1047100032B902F1804202F50432D2F894201A60D4 +:10472000704700BF98380020024B4FF40002C3F8D6 +:10473000942070470010024008B5FFF7E7FF024BD6 +:104740001868C0F3407008BD98380020704700001A +:10475000FEE700000A4B0B480B4A90420BD30B4B71 +:10476000DA1C121AC11E22F003028B4238BF00224B +:104770000021FDF77DB953F8041B40F8041BECE75A +:1047800074520008343900203439002034390020B4 +:104790007047000000F07AB84FF080430022586361 +:1047A0001A610222DA6070474FF080430022DA601B +:1047B000704700004FF08043586370474FF08043CC +:1047C000586A7047264B2748DA6A42F0070210B44D +:1047D000DA62DA6A22F00702DA62DA6ADA6C42F046 +:1047E0000702DA64DA6E42F00702DA664FF09042AE +:1047F000DB6E4FF0AA31002353604FF4EE4491601A +:10480000D0604FF6FF7050611462174C5362146011 +:10481000164CC2F80434C2F80814C2F80C444FF61F +:10482000F774C2F814444FF0EE44C2F8204447F243 +:104830009974C2F824440E4CC2F80044C2F80438FB +:10484000C2F80818C2F80C38C2F81408C2F82038A8 +:10485000C2F82438C2F800385DF8044B00F09CB868 +:104860000010024050010024A00100281050500008 +:1048700050A0AA0008B500F035FAFFF7F1FBBDE83B +:104880000840FFF743BF0000704700000F4B9A6DD0 +:1048900042F001029A659A6F42F001029A670C4A4F +:1048A0009B6F936843F0010393604FF080434F2266 +:1048B0009A624FF0FF32DA6200229A615A63DA603C +:1048C0005A6001225A611A60704700BF001002400E +:1048D000002004E04FF0804208B51169D3680B4016 +:1048E000D9B2C9439B07116107D5202383F31188EF +:1048F000FFF7E2FB002383F3118808BD08B5244BC2 +:104900004FF0FF319A6A99629A6A00229A62986A15 +:10491000D86A60F00700D862D86A00F00700D86251 +:10492000D86A186B1963186B1A63186B986B60F070 +:1049300080509863986B00F080509863986BD86BA8 +:10494000D963D86BDA63D86B186C1964196C1A6464 +:10495000196C996D41F080519965996F41F08051C2 +:104960009967996FD3F8901011F4407F1EBF4FF4F0 +:104970008031C3F89010C3F89020D3F89020C3F88A +:10498000902000F07FF9034B00225A6008BD00BF61 +:104990000010024000700040394B3A4A9A650122EB +:1049A0001A601A689007FCD500229A609A6812F083 +:1049B0000C0FFBD1344A4FF4007111605169490565 +:1049C000FCD41A6842F480321A601A689203FCD54B +:1049D0002D490A6842F480720A602C4A4FF4E06163 +:1049E00011601A6842F060021A601A6842F0080208 +:1049F0001A601A689007FCD59A6812F00C0FFBD168 +:104A0000D3F8942042F4C062C3F89420204ADA60BC +:104A10001A6842F080721A601A689101FCD51D4A2A +:104A20001A611A6842F080621A601A681201FCD595 +:104A300000229A601549184AC3F888200A6822F0B3 +:104A4000070242F004020A600A6802F00702042A20 +:104A5000FAD19A6842F003029A609A6802F00C0256 +:104A60000C2AFAD11A6E42F001021A66D3F880209D +:104A700042F00102C3F88020D3F88030704700BFB5 +:104A800000100240000400100070004000200240AE +:104A900003140001000C100055550134074A08B5F5 +:104AA000536903F00103536123B1054A13680BB145 +:104AB00050689847BDE80840FEF79ABE00040140E0 +:104AC000A0380020074A08B5536903F00203536178 +:104AD00023B1054A93680BB1D0689847BDE80840F8 +:104AE000FEF786BE00040140A0380020074A08B542 +:104AF000536903F00403536123B1054A13690BB1F1 +:104B000050699847BDE80840FEF772BE00040140B6 +:104B1000A0380020074A08B5536903F00803536121 +:104B200023B1054A93690BB1D0699847BDE80840A5 +:104B3000FEF75EBE00040140A0380020074A08B519 +:104B4000536903F01003536123B1054A136A0BB193 +:104B5000506A9847BDE80840FEF74ABE000401408D +:104B6000A0380020164B10B55C6904F478725A61C5 +:104B7000A30604D5134A936A0BB1D06A984760061E +:104B800004D5104A136B0BB1506B9847210604D51E +:104B90000C4A936B0BB1D06B9847E20504D5094AD8 +:104BA000136C0BB1506C9847A30504D5054A936C60 +:104BB0000BB1D06C9847BDE81040FEF719BE00BF9E +:104BC00000040140A0380020194B10B55C6904F4C2 +:104BD0007C425A61620504D5164A136D0BB1506DC3 +:104BE0009847230504D5134A936D0BB1D06D9847B0 +:104BF000E00404D50F4A136E0BB1506E9847A10420 +:104C000004D50C4A936E0BB1D06E9847620404D55C +:104C1000084A136F0BB1506F9847230404D5054A17 +:104C2000936F0BB1D06F9847BDE81040FEF7E0BD21 +:104C300000040140A038002008B50348FEF7FAFE42 +:104C4000BDE80840FEF7D4BDFC34002008B5034899 +:104C5000FEF7F0FEBDE80840FEF7CABD683500204B +:104C600008B50348FEF7E6FEBDE80840FEF7C0BD04 +:104C7000D435002008B5FFF72DFEBDE80840FEF74B +:104C8000B7BD0000062108B50846FEF757FF06210C +:104C90000720FEF753FF06210820FEF74FFF0621ED +:104CA0000920FEF74BFF06210A20FEF747FF0621E9 +:104CB0001720FEF743FF06212820FEF73FFF0721BC +:104CC0001C20FEF73BFF0C212520FEF737FF0C21AF +:104CD0002620FEF733FFBDE808400C212720FEF711 +:104CE0002DBF000008B5FFF709FE00F009F8FFF737 +:104CF000C9F8FFF7C9FDBDE80840FFF74BBD00004C +:104D00000023054A19460133102BC2E9001102F1B4 +:104D10000802F8D1704700BFA03800200B460146BA +:104D2000184600F025B8000000F038B8012838BF58 +:104D3000012010B50446204600F028F830B900F0F4 +:104D400007F808B900F00CF88047F4E710BD000040 +:104D5000024B1868BFF35B8F704700BF20390020FB +:104D600008B5062000F032F90120FFF7E9FA00004B +:104D700010B5054C13462CB10A4601460220AFF38C +:104D8000008010BD2046FCE700000000024B0146F9 +:104D9000186800F089B800BF28110020024B0146B6 +:104DA000186800F035B800BF2811002010B501398F +:104DB0000244904201D1002005E0037811F8014F30 +:104DC000A34201D0181B10BD0130F2E72DE9F041DC +:104DD000A3B1C91A17780144044603F1FF3C8C4281 +:104DE000204601D9002009E00578BD4204F1010404 +:104DF000F5D10CEB0405D618A54201D1BDE8F08130 +:104E000015F8018D16F801EDF045F5D0E7E7000043 +:104E100037B5002944D051F8043C0190002BA1F192 +:104E20000404B8BFE41800F0F5F81E4A01981368AE +:104E300033B96360146003B0BDE8304000F0F0B8EF +:104E4000A34208D9256861198B4201BF19685B68C4 +:104E500049192160EDE71A465B680BB1A342FAD904 +:104E600011685518A5420BD1246821445418A34257 +:104E70001160E0D11C685B68536021441160DAE77F +:104E800002D90C230360D6E7256861198B4204BF61 +:104E900019685B68636004BF491921605460CAE700 +:104EA00003B030BD24390020F8B5CD1C25F0030532 +:104EB00008350C2D38BF0C25002D064601DBA94214 +:104EC00003D90C2333600020F8BD00F0A3F821497A +:104ED0000A6814469CB9204F3B6823B921463046E6 +:104EE00000F03CF838602946304600F037F8431CA3 +:104EF00023D10C233360304600F092F8E3E72368B7 +:104F00005B1B17D40B2B03D923601C44256004E0E2 +:104F10006368A2420CBF0B605360304600F080F81B +:104F200004F10B00231D20F00700C21ACCD01B1A7D +:104F3000A350C9E722466468CCE7C41C24F00304EC +:104F4000A042E3D0211A304600F008F80130DDD14C +:104F5000CFE700BF243900202839002038B5064D9E +:104F60000023044608462B60FFF7E2F9431C02D1F8 +:104F70002B6803B1236038BD2C3900201F2938B5B8 +:104F800004460D4604D9162303604FF0FF3038BDA8 +:104F9000426C12B152F821304BB9204600F030F883 +:104FA0002A4601462046BDE8384000F017B8012BDC +:104FB0000AD0591C03D1162303600120E7E700241F +:104FC00042F82540284698470020E0E7024B01467A +:104FD0001868FFF7D3BF00BF2811002038B5074D70 +:104FE00000230446084611462B60FFF7ADF9431C29 +:104FF00002D12B6803B1236038BD00BF2C390020DB +:10500000FFF79CB9034611F8012B03F8012B002A86 +:10501000F9D17047014800F009B800BF30390020CD +:10502000014800F005B800BF30390020704700008B +:10503000704700006F72672E6172647570696C6FE3 +:10504000742E4D6174656B4C3433312D416972732C +:1050500070656564000000004E6F20617070207301 +:1050600069670A00426164206677206C656E677428 +:10507000682025750A0042616420626F6172645F76 +:1050800069642025752073686F756C6420626520E3 +:1050900025750A0042616420667720646573637237 +:1050A0006970746F72206C656E6774682025750A6C +:1050B00000426164206170702043524320307825A3 +:1050C0003038783A307825303878203078253038C4 +:1050D000783A3078253038780A00476F6F64206658 +:1050E00069726D776172650A0040A2E4F1646891AB +:1050F0000600000053544D33324C343F3F00000053 +:10510000FC34002068350020D43500204261642042 +:1051100043414E496661636520696E6465782E007F +:1051200080000000008000000000800000000000FF +:105130000000000079230008D92200089D1B000808 +:10514000D51B0008D11D0008A11B0008B51B0008D5 +:10515000A51B0008A91B0008B11B0008AD1B000817 +:10516000F91C0008B91B0008E5250008C91B000848 +:10517000CD1C0008009600000000000000000000A8 +:10518000000000000000000000000000000000001F +:105190004D3E0008393E0008753E0008613E00089B +:1051A0006D3E0008593E0008453E0008313E0008AB +:1051B000813E00086D61696E0000000069646C65E5 +:1051C00000000000BC510008403700207838002063 +:1051D000010000005D4000080000000004B0FF7FF7 +:1051E0000100000000000000260400000000000094 +:1051F0000060030000000000FE2A0100D20400004D +:10520000FF00000000000000F45000083F00000014 +:105210002C11002000000000000000000000000031 +:10522000000000000000000000000000000000007E +:10523000000000000000000000000000000000006E +:10524000000000000000000000000000000000005E +:10525000000000000000000000000000000000004E +:10526000000000000000000000000000000000003E +:04527000000000003A +:00000001FF diff --git a/Tools/bootloaders/MatekL431-Periph_bl.bin b/Tools/bootloaders/MatekL431-Periph_bl.bin new file mode 100644 index 0000000000000000000000000000000000000000..1d210ebfe10b70effa3faacb6a8977f514607594 GIT binary patch literal 21100 zcmd74eRx#W^*_4L%gmfihGaqlh9tnr3}9da4hc#WzB-u(#NFefuf&hN8lfBKQzXtxBNdK;Tbnf@#-_85? z@cuF)ya)Fc+&DK8{tNEkVg5b7dnysy;Xa0=d29X;Wgb}ke_zjyfLQ`p0eAZUP{+Tw z`~T;-E|Uo4JAcdD%G$_u!B6sX{N;)^8_rIY``eC$1=Bs<{QnR|uYRKPU7vKbR=%ma zbY{0wD(vT_UrWTcB)HSLNAh;IM7Q zh^bd5M<0s_vGJFXUZRf~!uy=Y&YWJ3(}y*?Pnle^&*|vPsbO!fa^0ErLd(&Zst7T2 zp{s{;k(eR!VjP<1NmwiNDaM)?0d1&R?P7}@J$lDy*<{+p=5r2WeXpC8%$C}}P#)Rk zOzI>viSmt)OhYPTmlEkmqqSGatg_%PCwq3Alk^Sxwr5qKAB^~nL}qpc0K4>)5Zk8x zosNsla41jup6fL+7)dF{Fv3g|P0g`Lzaf^M=ICLHQ(~O5!zo}a48w)BBKFJ<)cT>X zL`uRq0I3*a<}_Ci<4TE4+%knwTMX3zY70qHyL6Qmbaau& zLyLPjlNdD&Fj0MfPRSs}}(htrq5B#|KXs^IlAdQI{A{O)`?3i2vT-`^eCGd*5=QN*P)e~rmeRj# z6uY35qVZ>TIWs!hn=4$Wv!os(6@_L=d6F)qmo9ytDXE{k7rb<)t(O&6_nK0eqJtqx z>IsR`o{&Qdh62)-5Ie8JRS>dENg+x*&QNtJ@v~h-A?iqI;~O_E^z{onZp1eU-=$ZC zTmPqzFoMU?N61HTORvniFOlN!FMq<{zu^g*bD~c{&W=&ot2yNgA^-f^Y+QO}{%9G! z=}R9@LKn6Wrx$tzCo!tE-by`yw6M3iDRZE@Q7(ss=2zyOCskA6l)PVR|I)k@tL+J5t?f_S zktZD}B_gN1GCUIEl&QnyA0r_`)K*@Zry3+)c}&$w2aZp6KkJ+wU<&DXqWfA!V;h{; z1=SX!*oS9H&OY<~M7drQn#q)D5>fJpg*Ls!OV>B2qZ}9Dl?pYUBl1d?n$k>^DwR=g zjS}S+jZyw0DVXv>RHB`^Iu?E_3LJ%G()5Q@N%et9;DdD#k}RcHt>GxtUXr4yTo+E+kge zc^{`#4=2VyX0cUHZ??+JJgcV)p(=!`Jc2uWb75PRY;9vjM%g)7V6F0)+limzxKmGr zSH=?Y1%z|mmNIi|jx<4LX3q=oh4s$CV5R)L^ZK(B-HkFACRN;Jv82H{^9-jnYn*aQ zBdG81>UcLRW>z!GiotR2hI@#mVa>h`4R`O`(0R{=HJuwS+}*ih!o68PIjj22r&ON^ z-@fsUa;3{E=AKf8CFi=e@q^6*s99En2Se z(0-XwiU(gROW%C3?a)Fd_mn5CI)R}CeyqbUls*3~e!7Md_#utYmT}GLo@W=4r-?PW zdbww>)F@4o8YBiIxvWlU{;fM(CN`qvY@=hds9YV_NNmgnqU;%=p|37d$iJvuWA421 zwo4t%^S7y1h4{}4L>PwKokWBi;a_!AjYNnf6Cn+56I>(QNjRE+cGO>BBEkcsNvsc?60xeV%<$cXCsrMo+ z6GWNM1I#VD>quW$%|E^{D4cPOIsFEmRaejF>JAYlpk34T!-ahAVCYVKuf)Iop>icB zX(6b%UrP$}NxHfO>|0t}I6t*7$cO|SxU?rZ`L5`$7`Tj)QQlXlH76^{T(S*2`4lbk z^vDlFi%{doAu{WCfke)8LE5KBMhp_kYYZ^r%}a@LLSsccD5pq8J;Ghl9fP#p_cUIr zYo?)qCWIM*YcY-x#hLC%f79*MCQFV*3HznEwFr&!1@>sl+r+EXjf*v z#x!yboXXgkxnG4S4Ije#`uDZQBVB+q(R{e~ft$f$ikn*o#=JIpW?IL4VUaCcU_3lk z;YgmpOsZYPJelbt%Ig}_;9IEW66K^;-t!Xo%SBB6>HbGpI_eXeK3>CG^g-8KtD7C7 zIRAK<*81tIL~&^}^vSE_+BT^g*gEda?3{dET8FWbDNG4mt8m=dm(yL#RdK$qexbx$ zbT2W=QjMQmBZWdmsJ~}KSJxN%Sj`xviKY<}BG%Qp{2{)cD9?i4whp}dDs8hq-sYiK z`MpHZQ0nvV!0n2|HN@ez#^LtF;l3K_iTC$`c-l_%cWC6F@s#!Pl$uv{FJ)kcA|re* zxOp893f?esqtq6n^R{@zFiE?z;nm}qv-PhQVa^P_#`v7AiMLc1uj8&)F-pS^U2iS8 zW#Hhe%P=M%YSY!x)v##980L-P_&U-e1#$RN^x`G-*A<{RN)8AX<4fR3N-{8cQy{^M zXg{@xWjJRcr#ui%Z;J0npt0$%UZt@quTm`O4(Wg;Q+x$%9mtCJN*k()&sh4aT7(%c z?%QGKl;u$!u;$d3qIcQae5vVQdTH_JbfszkIwo!Te6IB1=agnBZ5{X(Lz2qU!{6BA=erpMP(U77)SGj**b*KFZY`+*V8&3!0(DmKmkUyS;njrv{K zmuZ`hiO-4CYGx!~ZTp$E@8;I*l)j!;ym8|{rvCL!h3}Z~DpI&jixAD($5$0@*B{Q5mMr2GS)F9DubwX1 zrC+J@v2vf(#vX>mA_>IWZ6(4s?Pl!vr2 zz&BmXlrFB)!Rz4VbCwao+-h_i%d$M&W#968)g#{2_P2ZTn?;nH6%!>+DIaP7!tCG{ zGRm~$3h34j*|rEY%(yeFsmy-@^(d`{?wlSU+@~ zQvCc2`1#vi9HDaD?Ff6|Ho^%WEm8;n2;5)b;xyf)f6XVE(AtkMK9aX8XnlyaWd|?q zWNev1MoAjdOIqaD!yKgN_pAS*2A(_`Q~LCm$N!YY>3n>&w*RzB?3W%YY&+y8maOWh z=eE<+_Y((qe%->%JL=Sh8C`Pcyor-s-GuyxSOp8CsPWO=qamU`ik}JE;AToGk>OLJOFQ#y z(*ouW#x^OSvmFo8Q`q)J54%5**@3Rg^pX!fG==zwLw4IM!K#P0E@2ASZn@Y}@9dVi z;HMASwlp}`1U{CY3FS%-jMy-;FLZDxGq17bv~5qv{Yz$TnRZ+kHgTk0uoOa*K-=#e z*_6}Q(n%&s6tcmoyO2^a!$qE?qx>5CEwNCJNUVoF537H*zo&NCZCjGtE@4s3KreL0WWi!{%?1Rc{6kwIeG~d)|DW~$ zd7 zhMrRYihZXgdLARu)tRVt(BJ7_i_So<|E24qrL@6XOU({9$^Ye_9#Ay209^D z;8Xc+REu0QA~b7}9}U{%q_RdM_Td_e`;5@lz2iOb0rC~;;z?9@b*;RQG;=F@c)Q7M zksF+8!DM&kFSDwR3$4H8HW?S4wxnPjZ$-Pn;pxfpPf7t!q3X z;@sBEC7FvREM&3IuDg$(VX+_9k9Ut>MC!*cyav01?>0vH`OrA`gw1KqW3h4@hOp+N zJm~F{7}nROrdWR;8%yW-%j2uf(OSji&h&6fSQVO6t+pDj$=*)ax5JrAC{O$67$Urj zINg_zq!EGkg^q`=>zQA%yHfjBPw(s|k3PP*zUlF$zb>bOP_MTr<2= zCCko-8n7}AHq!Tok10foJ^9sVzq)r9v2HB?hdS__vHo%F7RmLSJiu`Bozml9kzLHx zG2w}<0rfqu<}7<$o!CT_Wh49596PZ&l;8Mh?c>39NZx{AWjW3^gY&@uZaK>+rv|qH zQw!smGF~44^Q+j(0vDR+Sy}#Dl{9@AHXD!Qe3s!alx&cSc1BYUJ+9{2wTLVBR#=~! z2Q8=Gm~xowO=fNb%})miA7%++UF*?MD`VN2a-T0ZV3%lRio>A&Kuictcwn{mde z9V{*@UXm`K$~t=a=UcBkYsQ-0GRR8Fac;18i1xfKmU+~+<;JDNmf`2Dtaa0UjB@v2 z%4?7aHr*(p$BNx!$0(kSWm*k(w*ERuVki3lOdNeH(m(Fzs!6?5a>wiYP0Y5%26|Sb zwk^cAbzstKkZiwXcTw(2Tl&G!!q5er#A+3R`vCqOlG&01Df*|_=_hf*%f|SOO^*HM z3y!27uElph<^pqVH#Vl!r@Zt@w-8^|(P8_j>;+v={O_s^M(OG*tx>w7>*rm#19K8I zPigT5gu{P)@nk?2Qh$dq;mJawRkx5RzlgPeAvm})ei79O)x1Uglx9`Hc+GV0QOhY# z{-Lh)!PV)dF%lc&B4oc$0~pVjXeSf0_7L_tLo2be5|cY7Y#@)T2GGI@ zO>Ew;8c0I7Lv`NJl@X&PwDP%Hq*?PR3^~!_a0>1A*9~r?G-0#eyE?I}&PL))O|fa+FO*)*KAG--+5N2cV-g*$4Yxnz|!!#0_BF@>*R7Rzm(zO&WP z{d|r79fD&O;SVHCJmj+IATlHqgr6id#ww3O?$o)nWSu*EM`Kp4lkGISSmkDLpXyRt zmex_!YFNTcd9sL{xx;yFG}Qh@5lTDTi_i{*qwuJA*@4SuM?kI9Ax89zcB@nDh0WNj z*47{-ShUDVKSLq?ub>R%B-$H45&5p_))e=Twx*W=sbY1yVwmEaEna;gA|6T3KqT|fL9I$z;p9S}WW~w4yCuoGGF>WD|5|1-} zwrshVQuA^Og*2R#to6|Nee|PuZ)XYzLsSmuBvuM!jU8mN$@FNjx|DTs=nWgdc|SV} zZ7%4lWb%>>=_z%JXpnYDw;lh7T3z?_QN-=GHhNxMC|y^nMJ{PbAt%BD`@A#HZ;}4e zQY<}5biF>vBd2Jd{e|pFt}89VtZQ6#uFqN?R;PF_2DL~P&Ygb^3SG9fT<2P#UEJ*@K9AiqeD6W1Gqbe%AZ zc!ZnU&Y|oOoP0eI&%q_R&8?aAteBe#uCYVYml@n+H}f6_QkL1VT(uBZd8&>0X*t>s zrNx9EuJR3FktFnPiEw;&=?ufsym&ssDKzb}Z&{uCJ~`3vNTk#F;OTf6`$@4$ZdWs_ zGq6uik~RdBw+OcN9a`iAZGrc&d?#Z%cUaD5gmbUR8H(@r9|KRPG?B`IHO!P`8Rrud zu^sbYB;`G(oY(y&3eVlCTcDck$#QCR`%t>#E6?=ndQFqyawhrzuKH$|C@l9s@RPTE zq;8+B>*dGolHXGwo9)JT$LznwAsFr>_@vi0uDDO;Cc3y@S32i{3^7e893$Zt%Cth| zdrRQfEEo)KpEGmw#A-liKo7@FeQ>Ihth1F^@6#gpX!_q4(%u#m<+ZojDZhj4q}iVh zrMChrw==uD?jg#J!;|xjoy@+jdqhYrhla0ejN_+6%(*~emuZn=?F68&4AD8&A`U;X zupV}k)3eP}T?%@(5bKG!Ovp=IjR~CS8{mo3wbBad8#S0D%z`~#;Zslb`xwEWCROxr zK$i$Fw5{G%#Fa=?qj;h zjDcVh&O(^`QmmsU127>&PrC!{{dr_JzI!!_m-?Z}dDA;7R=ki8o*5SNEJ5ywOVS;l zC2{}rjFaj%u9tA4=C1_p=tW_?7sPrRy=X@tUWUw|MP3;xla@=xQjsK3?L#S1I*)6S z=SHBTqwz=R`CBcgxMK>Zx_QZ=X3k4RAC_r+I1elKHuwt#{_?gJk|bPjm&6+&Cupxi zf6ia_t$^IM9P&rfd)?_bzNaqqG1jlw@RuK1iIeXS-cxU{dstm*e^~X{g&TyhAOO-x zr4^}LIQ;u?CTgFki8n}U-qX^8f#AdH>FR5m>HhZ9VcuO;L8)BDxq_ZA1$uw7nmEj@ zJS03`D$B{L)*{cxXLtZ>lj4$0Uly3|o>Bg0EPQ#N^D)2Cnr2-&sWsL=#Km%!zuRJK zX7j*JJ193%9x;Wr8{W9=+YmYPZtuG(DPfK)@41w8s{c>i3y_X+o;G3+eE@n}-0SI9 ztcvbXTVv|w*2@)JPx@1EQ%WJbvQD)8C}&x8`+zCN54;&07+kZ_pv{!{?a4atoie8|+)H>eTT8X!sY`S8(C&Ea`QQO?z;OA?f_12E+Mc@9nB} z+2fwH2a6S+NxP3Jx2biaRN@^kHOOZSyp(jlL1uFIdEV)Fu%O#D1iSOv zkt%P49MmNv4>NmTJQsQ`vW9_#u0>YG^UrXj`LpBsPMXig^UFp=i3L?vV|^a-<8Dfl zykz?NpgwFe6v;lr+JL^78CN8qSDP3!)*-*h11r*y%8o7t|4fO;GG+tu7e$g~}t9BFAuDce=6 zxhVHW4QHk}o9*$uH%uQ90?)gQ6EH>xHcN8u1Q$P5T zUje zR6pynJxF^1E&{oXfknV)!9-8m-iACt*Ga1uIWnY0x}l??T=K}<^p3E^tCRN1MCn@A z6z;k!Rhk_50@ClkA>HNL+s$*xG<|qGr|T_A&jz2;pJ#61X7OQ>@5g!jS%iFxHnd%F?8g%a3JZXlM zi`Y4h!TLQ`Lx>q|&mFo3_i|2!Y;0K5urBUbHLRn1i-o3DX_5S)J(#bHhRkzq&1`PE z2%dwJVI;=pX5hA~AO1M_-^3_X3D)2C`0Sd2yVSQaTZ#KQx`(gW(e#vh+RoT`aO&|A z>5KUuD*vHs6^(7Bs+If|67##N$v5r(w{S*E3KsVTAU_-*x>c${ua^w}NhNiO@;?qF zR@1W>*I#3m-gMcD9XZ`YL$n9!o+>Z7_O6Nq)JHKb@+CAkMA`290(kx@w1u^Iyd@=t zg(U2kxpbGB5n~TQ*E967nt5QYl+tHNTI46V zv##>K2~Ejqd;ReWZ%`!`r^2u&;U{92fnVHaQBuB>;vDc)()>#AJ8DJ&{qp!0U(xP+ zM>UP5aYsDWRZ!(+ET_~QK%P-affcjy4t$CuDm76I)xkpG*CD!wA(trAqsw|2j+NMe zb5cc^Fg#>?b8|BKxo|ixdn!3?qp+a7FGthHw&89Z@y|x%#-q>+^!bc?=%=GSfm4^9zt;(Wp0z6U4Fll{JG$Y7glomYZ;ocTBjF=9AmZsmT#OX+PyW|bd8 z(@bSIV)ggm`NSz`81IBl$emBr#zWPmG)!drg}jYDl*)UTH62F5$9c;)+5xA&(=Ocu z`s~)Gy7F*hqCDr4CT7v|C@tfV_co&h(IOovX&NngM%(VROMCqH$YiP(`A}o#=etCD z-XcYCXA>olYrIRAUymta5=(Q@{xISz@jl{&h+Uh?_ZiSTk3qTjP;;K2 zm(DdGlUAyxb(FU?YIawi$00q0emEtGbvChi?YzGW3 zwMrJ|Omy{!ri0tl^GBYz2j();3^YR_(ipFj4Sw^k|zX{pf#oA+EQ966iJFe|y1)VWB zb`($Pss5c(dC%F0u+lb<@3HEO{5M0pVv|S(43HU_7hW*E?z|?r-!LkQ29k>M2(K6nNH_j)!Oc>U{|C|^ zv0~G*e~opKBK=bkde5g6|0rEeInm~r9@R&3qsFk-@>mbUltzt_8yyIlIONW8UGF*{ z^7SyHU@h*;WtkQSA=7;=un{ag=3f@a&j2(Si8cFp=I@*&Sk#^YJqCC#1Q9}5xvV)| z<~*l%Fpn~YjV=3}r+h_zuEs1;ezKzpH-Dg98M=Xy$QIBF|CAQ_m&Qsfo7aH{2(N-q ze)PIC&*Sx2ai=8ct_00EUsXd61n%2D*ja_$?SpOR_IKqhImdI#Q-WKJl5J(4Z{m1T z%O^@7?o5?V^(Qg<={3@mkQ(i{&3(BA_jddk7$V5L^g)ehlf(2&W83>$ZVmh#c4W@0G-7kXoV zh_Qr1ClLB?N%Zj2$Y;^gem`SC2#<9;wQ) z-A}dZ`7TbG%vCA3aMq2a5gPeoFL>~D^ytnJX6nTd!EF?Sk&wWKt%VF7n9deaZ1HDR;#x|4;*`%DYd!Agd%9F<9C zsJlTwGx)gt`88 z=ggKz6()(!+OgoHA>4q4HYkj4dq`~^hco09jF*QDJrc2w=o_m%t(VDUL*t8{IsUat z-&IvZgX~4NlH4@7N3JvZ>8*U1`Vl&;ykQ zZ;}>CXd5!*yb%$4-i}jw#65l-uR#(TRrZ97w&=Nv6^?SO<~c!9r*T ztT2*cd&rd7qHJGUNv3qVMT_tw!u5`soYWhyU5liQa1!X`w*@#I&>gjFq;NA+c(G-& zZHJ#&3#&m9(CTl1rO_b87r8?BTw<(<{E`UhmDoKK+lWO(`MWkq34g#a}C`SYe(9`OsYO;`%eFD)w+29 zDPF!9?LQq$Kj!L$mo)SC>e5r*Ecu;^cjT;Z`ApoFyf~m{`X(&RkS3xva&1x@-5G+g zHGe*94QzvdXoxYbEdjhAw)9nz;EEwX1gS$-eu z=4u|z=>P?v3+<4SWNP>6+dc_KoBxDD-4 z&A7?>dEmO4OJPH1*T3n+YT)wyS6W~*S3h0qP+?wpxB$3p8+x%ebQT$n7^ z&$~$YdRik_Z_ni#)0^o@EkB4_i6b7?1;}rN!?#J^Ea>2didx8Z#{xew7z4P&>OIF) zI@45PKC2km^kN1fA%A|SIp5LO>J#yO(?`cdl&5`ybxzk?5}Ec+h;Qt&<@vj;`ub$o zVW+M>qjOa@x0n3p$pv%#w@GJOyjj9w-fnnOi&P958pgX`?q!YRpc$vV<{R}5x_ymV zd_C8gEpzq14-xd8j_)RLs#im_we$dEeI0mP9}x1#cI;P0d*vi6*2;OD;-P0PV59ti zwd8T6)4WG9gSUpvfNcL3GWb2nBS@!tF9C9XNX$>|5cX9~qBiFj47zV?G?fT@gvQf= znGtg2+XA@lT2nFU-Ec1ZLO7S3hBpd2^kFJWrIk!5`Oq--JY#tQR>1Ee9|?W%f9xPlT}38=tIZ_TLgQu~~)(;<8XGWfZWe*joM+9~FL-Ek^D>LUTxS{n}? z5AfE9LiSc>Mh2t_W{M;K;if#lV+xtd%z!o4ip-Ae&5uJ9T7@~}=^W}Ro#RI5u->6V zi0tMh3wC*WettHl!!58dLGbGM<+)r(|vSk()U97bO=adiS?6zH= ziQ>Nle#m%Gj{r(eUC;0K5_kO>ATDig|kB{>iDr5Ni-mA$} zPL$~C9mj{%-kB={Cvuk|-U+x%{EPi{5A3W_kMrsj7()KtZ$95rTs88&-)A{&s$`2-C*!)nT;qz@;(b3^v?6VSn&xo zWn>ySQ#D>zq~P5`n2V;2{2rXp8s~s14CR8STZ9{uo)o|VuZ=loVU7zz!DHKEzf4jl z>reG>=lZk$+Rf(f#h&xFpMXlTTW;%N$ou`goEqTvZp-5Ldhq(=xrKFft+P9e`LhPt zqOJ%qHtQ$RUzYbg%CQQyA^HWEUJ`pCp(B_dGc+D-i~SPyvrIu{|F5{%1ANy8C7J99 zz39CLqc)M+_z2&^2(N@DD2~+p0M~fj!#Bn}bZ;MXXe6>V$~7A6`NrgW+eA~nZOak$ z<^xo>djvLE`o_oA65RMYnhwg3tHqwjRi}^Qd2eaFRStSrNEnT5Y#R}mhGM@w-R~IN z(tCEYLpkT8qdtmxJ>xU%vu!Ew**BIDbOQEwcf&6;Q{vT_|}66j<5+r}WqZZ-myi z&6-HZ@xSYR1NEN0IAaTQ)2E@qohKocGV^rPFSIQ0$eT#4hH#dt0k^GYmFYF9Ocyho zDb!ED*fJ30_8E8c`;vF#&N7_En06QW`N9X~F}vsZSHNz7SX1g_J1N#6J#tyD1kR|= zVZKN)cNK42JTtZ&+sWKCeIjTiI&aspZM9A=cu-E@Qw05Lkp&|d2c*#L-@)v>U3;Aw z{xoPC_kX?x(&(E3y~uRs*sE}#O-{TG{-_3%vV@7cFmEEgT#|0@e(VHHFG=}BI6L(~ zM}za3PRM2RlHhc(fYsH1g&j7D{1lwWNH*a+hE(I5Lhivgm8|_dvCrYWSgTu9w$1{r zQ(2WDEN6e(_>br;(_nmV30IA9dgdvZj_3iR}NTr)DRV2AD zuCxARJ8fU-iNPcAYyGqAy0E=Sb9tJ|*KVL<$@$m&j=)jE8H^jNenTu&*sq zPC&bc){7X&_u?+{{rE!SnAju-^8Q;Pa z#TZSCLsP3A;-`1I^e)A51MDZ@Ruti`2xFo5ydK0)z^$Qa7!SSo^&$QM;$83w#$<=y z$-bCO&-BAq&z2gT<=*SO5#OwPodZ5+{B?&&LswsTSY+A9@Y~s}9cXxfIvv?(#SXezS?{c9UVPqZKAs-m24`AFG%xlLIYb z-(8aeK6SO--aZyBCkN8-{V141_1XhIyf|stKz&13QapteBvRnt-MHacXsfj=bc^-x znCoij|KqqYv+=Rhu|g&*{o_4zE<{mV zhucDceuRWQCEVqTk|gX$ux&}F_HmQB21iL!BjHB#Fu%E3SO@exwN2K0Uv?%-HQpls z!}3qFTAjR~1&rq1_wJOwGg?jTOS~CYt?U>F2 ziP=3IxmLsdxcpl7P10=CEynBa4$)Fs2*b{`)#xHqPF1jt+da4iCnvgfX(v5w1y^Zq z7g__|x`KLWV7AHI+P29|!8XsT9B0LZYP4U_;GE(=C6`nXyt+6Lr8hV9hMwx$s_`z1 z$H|iNlHbX{Yd$DHk2AqR&+?pu72`Hry!9w~Nj_L{$(s!8U}*kF->Dh8s0}Akyx3Jf zz3?zfSL3cbQcml-M%pU{)SNodiIL6~JPL|Orb`!T}QwndAKyv@5gtjH74+?IG4D|bKLw|0Q{(EW;!Q;QZ^ zG0a-6_b29#>hX(OPkHGqWVi9Tf=i3Mq)mrRLoJl?&L&TPL7Z4V;Dhe6$N`){^(hyPdH!tDw7cdd!}cqxk~brgX~vg0txYylqUt)<)w( zdx;me({Zj_QlUjI!fv!LgjTJT? zpMVouH@?BSS5^|(+XMw^k$%uR?&n7N zg@AnHXgQ&M5o9bUW*`&PlkO?;@W_q*fpW~mJaCv7eIQF_J2 zU{7`HEP$O&Y1=Rs>G5`}TXUA4z$ua5+~FNsv`|{EW?a_*t&F|$y>4d8439A%cVrWc z_CI#(R#58-yd!*T_S%xHOPNJ@D_Jieg#Ds00q-NDn_$1O$7uo`zv8iZ)_TTy8sr~) z#%4FfS3rM7b$hn_Le`gYIxK3qfi&Ng!^*}5&@;fMDbS2;~RTQ;bBu;j!8Mlz! zdgm(7Ne>4-ei3@`*jo*9jXZTNg(b=pQEE9-oW0Zeug=4n6SItjmDGxzuFNX$dDZ0m#sh2lAW?o0 zrMx%Kd!jTkNB;~}c$;zOlkUpS!tP#dwB`FxWEx>jUiuc*awYI;f{t@^X5zey(m0)q zt7-Jr0_0uqMQ-R^d@e-RA+#ppMe(LKEuv@!_bM-$;rHvy8a!-xM?r%Jt82hlndK4M zcNBpq0p7~oy+CM9Mwxn)`HPl>GERj!S5;)mklJp`N|cHFh<~r_*1QHk>S##nWHb7n+D#=P0W_dAkPRv=)&y6Y(qYNJwQ%| zrGGd8TOssmsWy43fZx4fsO47@lmA>;FzC_WpJ=dx%l{H%35NctX83f{ts*PM;nN&=Tnt_C)3LjlSUg=*X-__3D(4!aThO_mYm z_s6PRw>4>zXNFUj!j4eM8Z40OoOAuAvgAd#${F%3Kdf|$nc25`_z9c@i+;QTV|Ev! zcTWs6`=~t>?vDMBq0TzEL|vPQ&GEVh)I?n=Wn=!ME?N_ZyUIjeUepDvu#GqmNwLV8 zVOs0eI?b)DOApKXI^1%Z+g&-N1+QEQ{k?;(7HY4(ERQ(S-1gS9YMo;WpLCA;r{JjX z8XztjHe&ZE8SrhgdRUh*T9-Xu7qEM0i7GBj)J5%KNHuZLIt{Iig~5waE%FBq?*rp+ z`RUt1Ez*a(VO=1R%j}^x;naqK()Lwna4+avfmH!&^*EqEZ}2R>@{tp9H-nc>fU*6m-zQ0KW-1ro1~Wffg}7to7DoQ5c`@s+HsN~En^TU|QMeeJ>v zYn{*`n_s_Bd10*uA?jaPPtz`}zHo2c|4F%0=e&4tx$^RCVqNYjANbm}+Y|6mUE8vX zjLK^kFR#q-j9&~pmO1aJLI$i@T;*3H_rtoyRjz#PdPfyqrG5JlpW;dZ6hR7K1jtN4 z!p{%@0e=b{DZ+0s-~|5kF^a2Y44HC(A$jIy%V5<0GWJ2dwSi?n2oXuFHbkyUwHdmhG+DyABp*ald0-XI00#3vik1C#;^ZJ_~N&J;jRe)_wR* zhq#a5b-;R)n`O5Fin6Qwk#!b9I=gnf;H{A_o9xv6`BX9lQ z(u0VhCx`_Qqr2=#WWpanFQYh^Fxtj1ASF}+c6?d-Jt%q4-cl=KXc4ghVqH9cKD8|U zUgX`oxAZy0&>~_1#3~UJ%F;I=Zv)C9h87VEAeLyctNm(wLMyj$Bod8VgpNecwNo8Y zB25`h`=Fib$`WaxkEWeyr}`txJ7ZO`mHJMx>=FFls3Z( ziCSXAegrR-GG(GXdW`t<=sTRz`1PZ4Xa{a&U3FA?6?I+QW?1l)AB5FRX12MVvG>L8 z!VWf{&bnqZ_by69#H(kc>;&?KkMQZ=T?jVYC>>zab zQ{{>YyPS-Ff_$gEXhN>%t5BA7C)VIWXS6d-UV*lfgHz;O*X@}HooSw~U=HAxdvaZA zo(mzilS_<8@Sdct5S@cFzYPs*RljJgTi9=irw;bi(zfnMHonR5DX9F&AHQ zawreYp9md_a!}0$_sfu;ZWGJr!D5EqfWwAZE%Ho{hvup!Io2SBr|D0smqJ!(+pam6 zY~wnuo*dUy58qXV-|sQ`lW{JmFg}$JCll*L5qrTqSsw+mvkoph9kNKP<&Q$|=1fsI z{($pRAUBJ`3Mym&B=~2@Mzp_qa7C^9B?Q_FL9#(|`*g4*hYi|fuG#7-YR^ahj8E4^(^CZ@7Qfl8XD+tXn*%aQ4h)Yc_hTW&G;a2C<^LTHNHl z+en-htHq!EOkBU_-n;LY#idl<>YEGm_E`8_=wG<2^sC*to_>9M69iUM=s|V~s7peMw=# z;yJh6g7A0WZP9*~6fa%$lZvV}qW9j_Yc|g!KEQnELu2GYD-k*^MA)B=-%NxHP9(x} z@LS=22lohl_h7GZ=CPX<9r6 Twubj-6c_oRm(isE&%FNyb)jFf literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/MatekL431-Periph_bl.hex b/Tools/bootloaders/MatekL431-Periph_bl.hex new file mode 100644 index 00000000000..41bcb518062 --- /dev/null +++ b/Tools/bootloaders/MatekL431-Periph_bl.hex @@ -0,0 +1,1321 @@ +:020000040800F2 +:1000000000090020B5040008D5290008552900087A +:10001000AD2900085529000881290008B704000807 +:10002000B7040008B7040008B704000889370008BF +:10003000B7040008B7040008B7040008B7040008B4 +:10004000B7040008B7040008B7040008B7040008A4 +:10005000B7040008B70400089D4A0008C54A000814 +:10006000ED4A0008154B00083D4B0008B704000896 +:10007000B7040008B7040008B7040008B704000874 +:10008000B7040008B7040008B70400089125000869 +:10009000BD250008CD250008B7040008654B000801 +:1000A000B7040008B7040008B7040008B704000844 +:1000B000754C0008B7040008B7040008B70400082E +:1000C000B7040008B7040008B7040008B704000824 +:1000D000B7040008394C00084D4C0008614C00087A +:1000E000C94B0008B7040008B7040008B7040008AB +:1000F000B7040008B7040008B7040008B7040008F4 +:10010000B7040008B7040008B7040008B7040008E3 +:10011000B7040008B7040008B7040008B7040008D3 +:10012000B7040008B7040008B7040008B7040008C3 +:10013000B7040008B7040008B7040008B7040008B3 +:10014000B7040008B7040008B7040008B7040008A3 +:10015000B7040008B7040008B7040008B704000893 +:10016000B7040008B7040008B7040008B704000883 +:10017000B7040008B7040008B7040008B704000873 +:10018000B7040008B7040008B7040008B704000863 +:10019000B7040008B7040008B7040008B704000853 +:1001A000051800080000000000000000000000002A +:1001B00053B94AB9002908BF00281CBF4FF0FF31CE +:1001C0004FF0FF3000F074B9ADF1080C6DE904CECA +:1001D00000F006F8DDF804E0DDE9022304B0704722 +:1001E0002DE9F047089D04468E46002B4DD18A42EA +:1001F000944669D9B2FA82F252B101FA02F3C2F11D +:10020000200120FA01F10CFA02FC41EA030E9440AD +:100210004FEA1C48210CBEFBF8F61FFA8CF708FBCE +:1002200016E341EA034306FB07F199420AD91CEBA6 +:10023000030306F1FF3080F01F81994240F21C81D8 +:10024000023E63445B1AA4B2B3FBF8F008FB103320 +:1002500044EA034400FB07F7A7420AD91CEB040455 +:1002600000F1FF3380F00A81A74240F20781644425 +:10027000023840EA0640E41B00261DB1D4400023AA +:10028000C5E900433146BDE8F0878B4209D9002D0E +:1002900000F0EF800026C5E9000130463146BDE898 +:1002A000F087B3FA83F6002E4AD18B4202D3824202 +:1002B00000F2F980841A61EB030301209E46002DB1 +:1002C000E0D0C5E9004EDDE702B9FFDEB2FA82F206 +:1002D000002A40F09280A1EB0C014FEA1C471FFA64 +:1002E0008CFE0126200CB1FBF7F307FB131140EA4B +:1002F00001410EFB03F0884208D91CEB010103F118 +:10030000FF3802D2884200F2CB804346091AA4B2D9 +:10031000B1FBF7F007FB101144EA01440EFB00FEAD +:10032000A64508D91CEB040400F1FF3102D2A64512 +:1003300000F2BB800846A4EB0E0440EA03409CE7B1 +:10034000C6F12007B34022FA07FC4CEA030C20FA5E +:1003500007F401FA06F31C43F9404FEA1C4900FA7E +:1003600006F3B1FBF9F8200C1FFA8CFE09FB1811FB +:1003700040EA014108FB0EF0884202FA06F20BD96E +:100380001CEB010108F1FF3A80F08880884240F2BE +:100390008580A8F102086144091AA4B2B1FBF9F002 +:1003A00009FB101144EA014100FB0EFE8E4508D9FD +:1003B0001CEB010100F1FF346CD28E456AD9023882 +:1003C000614440EA0840A0FB0294A1EB0E01A14267 +:1003D000C846A64656D353D05DB1B3EB080261EBD5 +:1003E0000E0101FA07F722FA06F3F1401F43C5E9AF +:1003F000007100263146BDE8F087C2F12003D840E5 +:100400000CFA02FC21FA03F3914001434FEA1C4726 +:100410001FFA8CFEB3FBF7F007FB10360B0C43EA18 +:10042000064300FB0EF69E4204FA02F408D91CEBC8 +:10043000030300F1FF382FD29E422DD902386344C6 +:100440009B1B89B2B3FBF7F607FB163341EA034166 +:1004500006FB0EF38B4208D91CEB010106F1FF38B5 +:1004600016D28B4214D9023E6144C91A46EA0046AC +:1004700038E72E46284605E70646E3E61846F8E63E +:100480004B45A9D2B9EB020864EB0C0E0138A3E787 +:100490004646EAE7204694E74046D1E7D0467BE768 +:1004A000023B614432E7304609E76444023842E7E0 +:1004B000704700BF02E000F000F8FEE772B63A486D +:1004C00080F30888394880F3098839484EF6085186 +:1004D000CEF20001086040F20000CCF200004EF6BF +:1004E0003471CEF200010860BFF34F8FBFF36F8FFE +:1004F00040F20000C0F2F0004EF68851CEF200014A +:100500000860BFF34F8FBFF36F8F4FF00000E1EE35 +:10051000100A4EF63C71CEF200010860062080F30E +:100520001488BFF36F8F04F033F904F00FF904F06F +:1005300049F94FF055301F491B4A91423CBF41F8E1 +:10054000040BFAE71C49194A91423CBF41F8040BDD +:10055000FAE71A491A4A1B4B9A423EBF51F8040B5C +:1005600042F8040BF8E700201749184A91423CBFB3 +:1005700041F8040BFAE704F0EDF804F07BF9144CB1 +:10058000144DAC4203DA54F8041B8847F9E700F035 +:1005900041F8114C114DAC4203DA54F8041B884762 +:1005A000F9E704F0D5B80000000900200011002090 +:1005B000000000080001002000090020E0510008B0 +:1005C000001100208C1100209011002034390020EF +:1005D000A0010008A4010008A4010008A40100086B +:1005E0002DE9F04F2DED108AC1F80CD0C3689D465F +:1005F000BDEC108ABDE8F08F002383F311882846F4 +:10060000A047002003F0ACFDFEE703F025FD00DF6E +:10061000FEE70000F8B504F03BF8074604F08CF85C +:100620000546A0BB1F4B9F4231D001339F4231D0C2 +:100630001D4B27F0FF029A422FD1F8B200F030FD97 +:100640002E4642F2107400F035FF08B10024264611 +:1006500000F02CFD08B90646044635B1134B9F4205 +:1006600003D004F061F800242646002004F01AF8B4 +:100670000EB100F063F801F051FA00F03FFF01F015 +:100680003DF9204600F0A8F800F058F8F9E72E46AA +:100690000024D8E704460126D5E706464FF47A74CD +:1006A000D1E700BF010007B0000008B0263A09B04A +:1006B00008B501F0EFF8A0F120035842584108BDF9 +:1006C00007B541F21203022101A8ADF8043001F090 +:1006D000FFF803B05DF804FB10B5202383F3118805 +:1006E0001248C3680BB103F0B3FD114A0F48002351 +:1006F0004FF47A7103F070FD002383F311880D4CE1 +:10070000236813B12368013B2360636813B16368F6 +:10071000013B6360084B1B7833B9636823B902203F +:1007200001F08AF93223636010BD00BF90110020F0 +:10073000D9060008AC120020A4110020214B224A47 +:1007400010B51C46196801313BD004339342F9D1EE +:100750006268A24235D31D4B9B6803F1006303F529 +:1007600020439A422DD203F0B1FF03F0C3FF0020D3 +:1007700001F0DEF8164B0220187001F055F9154B08 +:100780009A6D00229A65996F9A67996FD96DDA65AB +:10079000D96FDA67D96F196E1A66D3F88010C3F86B +:1007A0008020D3F8803072B64FF0E0232021C3F8C8 +:1007B000084DD4E9003281F311889D4683F30888FF +:1007C000104710BD00A0000820A000080011002064 +:1007D000A4110020001002402DE9F04F93B0AC4B63 +:1007E00000902022FF210AA89D6801F041F9A94A42 +:1007F0001378A3B9A8480121C3601170202383F3A3 +:100800001188C3680BB103F023FDA44AA24800235A +:100810004FF47A7103F0E0FC002383F31188009B0E +:100820009F4A03B113609F49009C00230B705360E3 +:1008300098469B461E469A46012001F0F5F824B1E1 +:10084000974B1B68002B00F01C82002001F022F85F +:100850000390039B002B01DA00F094FE039B002B16 +:10086000EDDB012001F0D6F8039B213B162BE3D8EA +:1008700001A252F823F000BFD5080008FD080008C7 +:1008800091090008390800083908000839080008EB +:10089000250A0008F70B0008110B0008730B00086D +:1008A0009B0B0008C10B000839080008D30B000897 +:1008B00039080008450C00087509000839080008C7 +:1008C000890C0008E10800087509000839080008CB +:1008D000730B00080220FFF7EBFE002840F0FB81BD +:1008E000009B0221B8F1000F08BF1C4605A841F289 +:1008F0001233ADF8143000F0EBFF9DE74FF47A703F +:1009000000F0C8FF071EEBDB0220FFF7D1FE002836 +:10091000E6D0013F052F00F2E081DFE807F0030A8F +:100920000D10133605230593042105A800F0D0FF10 +:1009300017E057480421F9E75B480421F6E75B48D4 +:100940000421F3E74FF01C09484600F0F3FF09F1DA +:1009500004090590042105A800F0BAFFB9F12C0F95 +:10096000F2D1012000FA07F747EA0B0B5FFA8BFB85 +:100970004FF0000A01F0C8F826B10BF00B030B2B67 +:1009800008BF0024FFF79CFE56E749480421CDE745 +:10099000002EA5D00BF00B030B2BA1D10220FFF7EB +:1009A00087FE074600289BD001203E4E00F0C0FF86 +:1009B0000220307001F038F84FF000085FFA88F933 +:1009C000484600F0C5FF044690B1484600F0D0FF0D +:1009D00008F101080028F1D1B846044641F212138B +:1009E000022105A8ADF814303E4600F071FF23E760 +:1009F00001230220337001F00DF82546244B9B683B +:100A0000AB4207D9284600F095FF013040F06881DD +:100A10000435F3E7234B00251D70214BB8465D607C +:100A20003E46A7E7002E3FF45BAF0BF00B030B2B0A +:100A30007FF456AF1B4B0220187000F0F5FF3220F8 +:100A400000F028FFB0F10009FFF64AAF19F00307E4 +:100A50007FF446AF0E4A926809EB050393423FF6D6 +:100A60003FAFB9F5807F3FF73BAF124B0193B945DC +:100A700022DD4FF47A7000F00DFF0390039A002AF4 +:100A8000FFF62EAF019B039A03F8012B0137EDE728 +:100A900000110020A812002090110020D9060008A3 +:100AA000AC120020A4110020041100200811002025 +:100AB0000C110020A8110020C820FFF7F9FD0746FF +:100AC00000283FF40DAF1F2D11D8C5F120024A4573 +:100AD0000AAB25F0030028BF4A4683490192184417 +:100AE00000F0B4FF019A8048FF2100F0C1FF4FEAF7 +:100AF000A9037D490193C9F38702284600F0C0FF8E +:100B0000064600283FF46AAF019B05EB830531E7F9 +:100B10000220FFF7CDFD00283FF4E2AE00F044FFD5 +:100B200000283FF4DDAE0027B946704B9B68BB42FE +:100B300018D91F2F11D80A9B01330ED027F00303B9 +:100B400012AA134453F8203C05934846042205A9F1 +:100B500001F04EFF04378146E7E7384600F0EAFE31 +:100B60000590F2E7CDF81490042105A800F0B0FE3E +:100B700000E70023642104A8049300F09FFE0028EE +:100B80007FF4AEAE0220FFF793FD00283FF4A8AE3D +:100B9000049800F0FFFE0590E6E70023642104A816 +:100BA000049300F08BFE00287FF49AAE0220FFF73A +:100BB0007FFD00283FF494AE049800F0EDFEEAE7D4 +:100BC0000220FFF775FD00283FF48AAE00F0FCFE1E +:100BD000E1E70220FFF76CFD00283FF481AE05A994 +:100BE000142000F0F7FE04210746049004A800F04A +:100BF0006FFE3946B9E7322000F04CFE071EFFF6C3 +:100C00006FAEBB077FF46CAE384A926807EB0A03FD +:100C100093423FF665AE0220FFF74AFD00283FF4FD +:100C20005FAE27F003075744BA453FF4A3AE5046E2 +:100C300000F080FE0421059005A800F049FE0AF1AD +:100C4000040AF1E74FF47A70FFF732FD00283FF411 +:100C500047AE00F0A9FE002844D00A9B01330BD018 +:100C600008220AA9002000F00BFF00283AD0202219 +:100C7000FF210AA800F0FCFEFFF722FD1C4803F04C +:100C800075FA13B0BDE8F08F002E3FF429AE0BF0DB +:100C90000B030B2B7FF424AE0023642105A80593DE +:100CA00000F00CFE074600287FF41AAE0220FFF782 +:100CB000FFFC814600283FF413AEFFF701FD41F22F +:100CC000883003F053FA059800F042FF4E4600F0DA +:100CD0001BFF3C46B0E506464CE64FF0000AFFE538 +:100CE000B8467BE6374679E6A811002000110020BF +:100CF000A0860100094A136849F2690099B21B0CE9 +:100D000000FB01331360064B186844F2506182B255 +:100D1000000C01FB0200186080B270471411002023 +:100D20001011002010B500211022044600F0A0FE92 +:100D3000034B03CB206061601868A06010BD00BF4A +:100D40009075FF1F2DE9F043224DBBB001F04AFE24 +:100D5000AB6840F2ED22C31A934232D906AFA860C5 +:100D60002B4628220021384602F0D6FA05F10E0063 +:100D700000F076FE002604465FFA80F905F10E08C1 +:100D8000F3B2F100994501F1280107D908EB0603F8 +:100D90000822384602F0C0FA0136F1E708230122A2 +:100DA000CDE9023205340C4B0193A4B230230093F9 +:100DB000CDE9047405A3D3E90023297B074802F099 +:100DC000C3F83BB0BDE8F083AFF3008078F6339F03 +:100DD00093CACD8DC0330020CD330020CC2200201B +:100DE00070B50D4614461E4602F044F850B9022E66 +:100DF00010D1012C0ED112A3D3E90023C5E90023A1 +:100E0000012007E0282C10D005D8012C09D0052C92 +:100E10000FD0002070BD302CFBD10BA3D3E90023F1 +:100E2000ECE70BA3D3E90023E8E70BA3D3E9002306 +:100E3000E4E70BA3D3E90023E0E700BFAFF30080B2 +:100E4000401DA12026812A0B78F6339F93CACD8DB1 +:100E50009E6AC421818A46EE26417272DF25D7B789 +:100E6000F017304A39059E5613B504462346084606 +:100E700020220021019002F04FFA22790198032AE2 +:100E8000234628BF032203F8042F2021022202F068 +:100E900043FA62790198072A234628BF072203F8FC +:100EA000052F2221032202F037FAA2790198072A9E +:100EB000234628BF072203F8062F2521032202F02C +:100EC0002BFA019804F108031022282102F024FAD9 +:100ED000382002B010BD00002DE9F04FADF5017DC6 +:100EE00021AD0EAE40F2751280460F4622A80021B9 +:100EF000296000F0BDFD48220021304600F0B8FD19 +:100F000001F070FD564B4FF47A72B0FBF2F01860AE +:100F100093E80700012386E807000DF15A003382A9 +:100F2000FFF700FF42F20463338406AB18464D49D5 +:100F300004F068F81E2230642946304686F83C20CA +:100F4000FFF792FF12AB044601460822284602F042 +:100F5000E3F90822A1180DF14903284602F0DCF953 +:100F60000DF14A03082204F11001284602F0D4F9D9 +:100F700013AB202204F11801284602F0CDF914AB7E +:100F8000402204F13801284602F0C6F916AB0822C7 +:100F900004F17801284602F0BFF90DF15903082247 +:100FA00004F18001284602F0B7F904F1880A0DF136 +:100FB0005A0904F5847B4B465146082228460AF11B +:100FC000080A02F0A9F9D34509F10109F3D11BABD5 +:100FD00008225946284602F09FF904F588744FF01C +:100FE000000996F834304B450AD9B36B21464B447F +:100FF0000822284602F090F9083409F10109F0E7C7 +:101000004FF0000996F83C304B4504EBC90108D974 +:10101000336C08224B44284602F07EF909F101099D +:10102000F0E700230393BB7E0293073107F1190316 +:101030000193C1F3CF010123CDE904510093F97E5F +:1010400005A3D3E90023404601F07EFF0DF5017DA5 +:10105000BDE8F08FAFF300809E6AC421818A46EE1E +:10106000B41200203450000810B50A4B0A4A1A6026 +:1010700003F5805393F860203AB9DC6D2CB120461B +:1010800001F02EFB204603F04FFEBDE81040034860 +:1010900001F026BBF0220020305100083833002038 +:1010A000014B1870704700BFC0120020F0B5334BE1 +:1010B0001C7B85B034B1324B0E221A8100242046AD +:1010C00005B0F0BD2F4A1068516802AB03C3082376 +:1010D0002D492E480DEB030203F078FE054630B98A +:1010E000274B2B480A221A8101F078FAE6E70169BA +:1010F000B1F5583F06D9224B26480B221A8101F040 +:101100006DFADCE7438B40F22642934207D01C493C +:101110000C2008811946204801F060FACFE71F4AE9 +:10112000024402F11003994204D2154B1C481022CC +:101130001A81E4E710398E1A2046144901F058FC50 +:101140003246074605F11801204601F051FCAB6814 +:101150009F4202D1EB6898420AD0094B0D221A81B6 +:101160000090D5E902123B460E4801F037FAA5E798 +:101170000D4801F033FA0124A1E700BFC03300207D +:10118000B4120020E5500008DC5F030000A0000856 +:101190005450000860500008725000080860FFF7C3 +:1011A00090500008AD500008D65000082DE9F04FCF +:1011B000ADB006AF80460C4601F05CFE0546002847 +:1011C0005AD1237E022B1BD1E38A012B18D101F0C7 +:1011D00009FC0646FFF78EFD03464FF4C870DFF8A2 +:1011E000D092B3FBF0F206F5167602FB103316FA36 +:1011F00083F3C9F80030E37E33B9A84B00221A709C +:101200009C37BD46BDE8F08FA38AEEB2013BB342E6 +:1012100005F101050BD93B1D1E44E9000096002392 +:10122000082201F0F801204601F03AFFECE707F14F +:101230001400FFF777FD324607F11401381D03F063 +:10124000B5FD0028D9D10F2E08D8944B1E70D9F8BF +:101250000030A3F51673C9F80030D1E7FB1CF87015 +:101260000146009307220346204601F019FFF97852 +:10127000404601F0F7FDC3E7E38A282B26D010D8BB +:10128000012B1ED0052BBBD1BFF34F8F8449854B5B +:10129000CA6802F4E0621343CB60BFF34F8F00BF14 +:1012A000FDE7302BACD1637E7F4D01336A7BDBB22F +:1012B0009342E94603D1E27E2B7B9A4265D0CD462C +:1012C0009EE721464046FFF707FE99E7A38A013BC8 +:1012D0009BB2C92B94D8744D2E7B26BB05F10C0311 +:1012E0000093082233463146204601F0D9FE731C94 +:1012F000F2B2D9001E46A38A013B9A4205DA0E32A9 +:101300002A44009200230822EEE700230022C5E9C8 +:1013100000230023AB6085F8D730C5F8D8302B7B8D +:101320000BB9E37E2B73002507F114093B1D08223E +:1013300029464846C7E90155FD6001F0EDFF3B7ABB +:1013400005F1010AAB424FEACA0608D9FB68082238 +:101350002B443146484601F0DFFF5546EFE7C6F320 +:10136000CF06E17ECDE9049600230393A37E02938A +:10137000193428230093019446A3D3E9002340465F +:1013800001F0E2FDFFF7DEFC3AE74FF0000807F15D +:101390001403A7F81480102200934146012320462D +:1013A00001F07EFEA68A023EB6B2F31C9B109B00A3 +:1013B0000733DB08A9EBC3039D460DF1180A1FFA9A +:1013C00088F34FEAC801B34201F110010AD20AEBD7 +:1013D0000803009308220023204601F061FE08F173 +:1013E0000108ECE795F8D70000F0B2FAD5F8D8304C +:1013F00004461BB995F8D70000F0BAFAD5F8D830F2 +:1014000033449C4204D295F8D700013000F0B0FA82 +:101410004FEA960B4FF000081FFA88F18B45D5E98B +:10142000003209D90AEB880103EB8800012200F0A1 +:1014300027FB08F10108EFE7F31842F10002C5E9C4 +:101440000032D5F8D83095F8D70006EB0308C5F878 +:10145000D88000F07DFA804509D395F8D730D5F8CB +:10146000D8000133001B85F8D730C5F8D800FF2E0F +:1014700008D800232B7300F097FAFFF717FE08B186 +:10148000FFF75CF92B68094A9B0A0133138100239B +:10149000AB6014E726417272DF25D7B7C522002062 +:1014A00000ED00E00400FA05C0330020B412002073 +:1014B000C822002010B54FF000540C4B22689A420D +:1014C00011D10B4B627D1A700A48237D03730A49C0 +:1014D000C9220E3000F0BAFAE0220021204600F0C6 +:1014E000C7FA012010BD0020FCE700BF9AAD44C53B +:1014F000C0120020C03300201600002037B5184D60 +:1015000018491948022301226B7100F035FF0023AE +:101510000193164B164900931648174B4FF480520F +:1015200001F07AFC154B197811B1124801F09AFCC0 +:1015300001F058FA0446FFF7DDFB4FF4C873B0FB27 +:10154000F3F202FB130304F5167010FA83F00C4B50 +:10155000186003F0F1F808B10F232B8103B030BD00 +:10156000B412002040420F00F0220020E10D0008DC +:10157000C4120020CC220020AD110008C0120020AF +:10158000C82200202DE9F04F2DED028B0FF26429C7 +:10159000D9E900898D4C93B08D4E8E4F204601F0D5 +:1015A00037FD034660B30025CDE90F550E958DF844 +:1015B0004450027B8DF8442099684068DFF83CA2D3 +:1015C0000FAA03C21B6843F000430E9301F00CFA0C +:1015D000821941F1000300950EA9384600F0FAFA8D +:1015E000A84205DD204601F017FD8AF80050D5E736 +:1015F0009AF80030072B00F2B68001338AF80030E9 +:101600000BAE9FED6E8B0023724FDFF8F4A10A93AF +:101610008DF834300B9373604FF0000B5B468DED0B +:10162000008B01250DF11D0207A938468DF81C50CD +:101630008DF81DB000F050FE9DF81C30002B40F0DE +:101640009680204601F016FC0646002845D1624FE0 +:1016500001F0C8F93B6898423FD301F0C3F98246D4 +:10166000FFF748FB4FF4C8730AF5167AB0FBF3F2A4 +:1016700002FB13031AFA83F33B60584F97F800B04C +:10168000CBF1100ABBF1000F14BF33462B465FFAB3 +:101690008AFA0EA88DF82830FFF744FBBAF1060F3E +:1016A00028BF4FF0060A0EAB03EB0B0152460DF1BB +:1016B000290000F0CBF90AAB0393182302930AF137 +:1016C0000102474BD2B2CDE90053049220463DA31C +:1016D000D3E9002301F0D0FB3E7001F083F9414AC9 +:1016E000414D1368C31AB3F57A7F2FD3106001F010 +:1016F0007BF902460B46204601F098FC204601F09B +:10170000B9FB18B32B7B394E002B14BF03230223E4 +:10171000737101F067F90EAF4FF47A733946B0FB7D +:10172000F3F030603046FFF79FFB18230730029339 +:101730002F4B0193C0F3CF0040F25513CDE9037056 +:10174000009342464B46204601F096FB2B7B2BB183 +:10175000FFF7F8FA2B7B002B7FF41EAF13B0BDEC24 +:10176000028BBDE8F08F204601F056FC48E7DAF81E +:10177000143083F00803CAF81430594610220EA81A +:1017800000F076F90DF11E0308AA0AA9384600F008 +:101790001FFB96E803000FAB83E803009DF834308D +:1017A0008DF844300A9B0E930EA9DDE908232046EC +:1017B00001F0FAFD30E700BFAFF300800000000049 +:1017C00000000000401DA12026812A0BCC22002011 +:1017D00040420F00F0220020C8220020C522002035 +:1017E000C4220020A0340020C0330020B412002006 +:1017F000A4340020F1C6A7C1D068080FA53400208A +:101800000004004808B5054800F07CFBBDE808402E +:10181000034A0449002003F081BA00BFF0220020EF +:10182000F8340020691000082DE9F84F4FF47A735E +:10183000DFF85880DFF8589006460D4602FB03F7A4 +:10184000002498F900305A1C5FFA84FA01D0A342B0 +:1018500010D159F8240003682A46D3F820B0314645 +:101860003B46D847854205D1074B012083F800A0AD +:10187000BDE8F88F0134032CE3D14FF4FA7002F085 +:1018800075FC0020F4E700BFEC34002018110020A4 +:10189000FC50000807B50023024601210DF10700A6 +:1018A0008DF80730FFF7C0FF20B19DF8070003B0A7 +:1018B0005DF804FB4FF0FF30F9E700000A4608B579 +:1018C0000421FFF7B1FF80F00100C0B2404208BD23 +:1018D00030B4074B0A461978064B53F82140236869 +:1018E000DD69054B0146AC46204630BC604700BF71 +:1018F000EC340020FC500008A086010070B502F016 +:1019000073FD094E094D30800024286833888342D6 +:1019100008D902F065FD2B6804440133B4F5204F6B +:101920002B60F2D370BD00BFEE340020A83400203D +:1019300002F008BE00F1006000F52040006870472A +:1019400000F10060920000F5204002F089BD000027 +:10195000054B1A68054B1B889B1A834202D9104419 +:1019600002F03EBD00207047A8340020EE34002075 +:1019700038B5074D04462868204402F037FD28B9E1 +:1019800028682044BDE8384002F042BD38BD00BFA1 +:10199000A83400200020704700F10050A0F510404E +:1019A000D0F8900570470000064991F8243033B113 +:1019B0000023086A81F824300822FFF7C1BF012004 +:1019C000704700BFAC340020014B1868704700BF5F +:1019D000002004E070B50E4B5C6893F90860421E6D +:1019E0000A44013C0B46934207D214F9015F581C8C +:1019F0002DB100F8015C0346F5E7184605E02C24FC +:101A000082421C7001D9981C5E70401A70BD00BFE4 +:101A10001C110020022802BF024B4FF400229A61E1 +:101A2000704700BF00040048022802BF014B082293 +:101A30009A61704700040048022801BF024A5369B6 +:101A400083F00803536170470004004810B5002379 +:101A5000934203D0CC5CC4540133F9E710BD0000BD +:101A600003460246D01A12F9011B0029FAD1704729 +:101A700002440346934202D003F8011BFAE7704781 +:101A80002DE9F8431F4D144695F824200746884653 +:101A900052BBDFF870909CB395F824302BB920220C +:101AA000FF2148462F62FFF7E3FF95F82400C0F1BD +:101AB0000802A24228BF2246D6B24146920005EB58 +:101AC0008000FFF7C3FF95F82430A41B1E44F6B234 +:101AD000082E17449044E4B285F82460DBD1FFF768 +:101AE00063FF0028D7D108E02B6A03EB820383420F +:101AF000CFD0FFF759FF0028CBD10020BDE8F883F5 +:101B00000120FBE7AC340020024B1A78024B1A701C +:101B1000704700BFEC3400201811002008B50849B8 +:101B200008484FF461430B6002F0BAF904490648D3 +:101B300002F0B6F9BDE808400149044802F0B0B926 +:101B4000D4340020FC34002068350020D435002037 +:101B5000094B10B51822044600211846FFF788FFEC +:101B6000064A074B127804600146BDE8104053F85E +:101B7000220002F095B900BFD4340020EC340020DC +:101B8000FC500008202383F3118862B670470000E0 +:101B9000002383F3118862B670470000012070476C +:101BA0000020704700207047704700007047000019 +:101BB000002070470E20704700F5805090F8C80054 +:101BC000C0F340007047000000F5805090F9C90054 +:101BD000704700002DE9F0410C68BDF8187014F052 +:101BE00000541E466FD10B7B082B6CD8FFF7CAFF41 +:101BF0004569AB685B010CD4AB681B0108D4AC68C9 +:101C000014F080545DD1FFF7C3FF2046BDE8F0819A +:101C100001240B6804F1180C002BB8BFDB004FEA5D +:101C20000C1CB4BF43F004035B0545F80C300B6893 +:101C30000FFA84FC13F0804F18BF05EB0C1E05EB68 +:101C40000C1C1EBFDEF8803143F00203CEF8803159 +:101C50000B7BCCF8843105EB04158B68C5F88C310F +:101C60004B68C5F88831DCF8803143F00103CCF8CB +:101C7000803100EB441541F268031D4403EB44132B +:101C80000344C5E9002608330D4601F10C0C55F854 +:101C900004EB43F804EB6545F9D184342D781D70CD +:101CA00000EB441407F00303257925F00B052B43C3 +:101CB0002371FFF76DFF06973346BDE8F04100F052 +:101CC000AFBC0224A5E74FF0FF309FE713B500F546 +:101CD00080540191E06D00F039FD1F280AD9019967 +:101CE000E06D202200F0A8FDA0F1200358425841E9 +:101CF00002B010BD0020FBE708B500F58050FFF7EB +:101D000041FFC06D00F0F6FCBDE80840FFF740BFA2 +:101D100000220260027342608260704710B50022A8 +:101D20000023C0E900230023044603810C30FFF7A1 +:101D3000EFFF204610BD0000F0B5054600F58050CD +:101D40000C4690F8C83013F0040FC3F3800108BFAD +:101D5000114661F3820304F1840680F8C83005EB74 +:101D6000461389B01B79D8072ED57AB319072DD41D +:101D70006846FFF7D3FF05EB441303F5835303F1E4 +:101D8000180703AA103318685968144603C40833A7 +:101D9000BB422246F7D1186820601B792371DDE928 +:101DA0000E23CDE900230123ADF808302B686946E6 +:101DB000DB6B2846984705EB46152B791A075CBF65 +:101DC00043F008032B7101E0002AF4D109B0F0BD03 +:101DD0002DE9F047074688B007F5805468469A46D3 +:101DE0008846FFF7CFFE9146FFF798FFE06D00F0C1 +:101DF00093FC1F2829D9E06D2022694600F09EFD42 +:101E0000202822D103AD444605AB2E4603CE9E4288 +:101E100020606160354604F10804F6D13068206026 +:101E200033792371DDE90023C9E90023BDF80830C7 +:101E3000AAF80030FFF7ACFE4A4653464146384602 +:101E400008B0BDE8F04700F0D9BBFFF7A1FE0020C5 +:101E500008B0BDE8F08700002DE9F84F0023C0E985 +:101E60000133254B044640F8183B0F46FFF750FF5F +:101E700004F12800FFF752FF04F1480804F58255E9 +:101E80004646083530462036FFF748FFAE42F9D1C6 +:101E900004F580554FF480534FF00009C5E913391C +:101EA000C5F848800123EE6504F5875804F584568B +:101EB000C5F8549085F8583085F86030083608F138 +:101EC00008084FF0000A4FF0000B46E908ABA6F1F6 +:101ED0001800FFF71DFF203646F8289C4645F4D130 +:101EE00085F8C97017B1054800F076FD044B6361B1 +:101EF0002046BDE8F88F00BF3051000808510008A7 +:101F00000064004010B5044B197804464A1C1A704E +:101F1000FFF7A2FF204610BDF43400202DE9F04762 +:101F2000002950D0294B2A4FB7FBF1F599428CBFBD +:101F30000A231123581EB5FBF3FC03FB1C53C4B248 +:101F40002BB102280346F5D80020BDE8F0870CF13C +:101F5000FF36B6F5806FF7D2C4EBC40E0EF1030363 +:101F60004FEAE309C3F3C703A4EB030809F1010A2D +:101F70004FF47A755FFA88F009FB05555AFA88F82C +:101F8000B5FBF8F5B5F5617FC1BF0EF1FF33C3F3C3 +:101F9000C703E01AC0B25C1C50FA84F40CFB04F4D2 +:101FA000B7FBF4F4A142CFD1013BDBB20F2BCBD86E +:101FB0000138C0B20728C7D800211071168091706F +:101FC000D3700120C1E70846BFE700BF3F420F00C2 +:101FD00000B4C40470B505460E464FF47A746B69BC +:101FE0005B6803F00103B34207D04FF47A7002F04C +:101FF000BDF8013CF3D1204670BD0120FCE7000094 +:1020000030B54269936913F0700F16D000230B4C62 +:10201000936103F1840200EB421211794D0709D557 +:10202000890707D5416954F823508D60117941F033 +:10203000040111710133032BEBD130BD1C51000899 +:1020400073B51D46436916469A68D207044609D5FA +:102050009A6801219960C2F34002CDE90065002130 +:10206000FFF76AFE63699A68D1050BD59A684FF449 +:1020700080719960C2F34022CDE9006501212046BC +:10208000FFF75AFE63699A68D2030BD59A684FF43A +:1020900080319960C2F34042CDE9006502212046BB +:1020A000FFF74AFE204602B0BDE87040FFF7A8BF28 +:1020B000F8B50446466900296CD106F10C0738686A +:1020C00080076AD006EB01153868D5F8B00110F02A +:1020D000040FD5F8B0011ABFC00840F00040400D11 +:1020E000A061D5F8B0C11CF0020F1CBF40F08040C9 +:1020F000A061D5F8B40106EB011100F00F0084F8DF +:102100002400D1F8B8012077D1F8B801000A60772F +:10211000D1F8B801000CA077D1F8B801000EE07733 +:10212000D1F8BC0184F82000D1F8BC01000A84F881 +:102130002100D1F8BC01000C84F82200D1F8BC11B8 +:10214000090E84F823103821396004F1340004F1B9 +:10215000180104F1240551F8046B40F8046BA942FE +:10216000F9D109780170C4E90A23214600232386A6 +:1021700051F8283B2046DB6B984704F580522046F7 +:1021800092F8C83043F0040382F8C830BDE8F84044 +:10219000FFF736BF06F1100791E7F8BD10B504460A +:1021A00000F022FC02460B4652EA030102D0013A3B +:1021B00063F100030449086820B12146BDE81040DE +:1021C000FFF776BF10BD00BFF0340020F8B500F572 +:1021D00083511E46FFF7D6FCDFF844C008310024C7 +:1021E00004F1840500EB45152B795F070ED4DB065F +:1021F0000CD5D1E900739742B34107D243695CF82B +:1022000024709F602B7943F004032B710134032C5D +:1022100001F12001E4D1BDE8F840FFF7B9BC00BFEF +:102220001C51000808B5FFF7ADFCFFF7E9FEBDE85B +:102230000840FFF7ADBC0000F8B543690546986853 +:1022400000F0E050B0F1E05F0F461FD0E8B1FFF7BB +:1022500099FC05F583541034002606F1840305EB40 +:1022600043131B791A0706D50136032E04F1200407 +:10227000F3D1012007E05B07F6D42146384600F091 +:1022800009FA0028F0D1FFF783FCF8BD0120FCE734 +:1022900000F5805008B5FFF775FCC06D00F03CFA02 +:1022A000FFF776FC43090CBF0120002008BD0000A9 +:1022B000F8B51D46002313700F4606461446FFF777 +:1022C000E7FF80F00100387025B129463046FFF75E +:1022D000B3FF2070F8BD00002DE9B8410C4615464B +:1022E0001F46804600F080FB0B462178024609B964 +:1022F000287850B14046FFF769FFFFF793FF3B4650 +:102300002A462146FFF7D4FF0120BDE8B88100002E +:1023100010B5FFF737FC174B9A6D42F000729A65C3 +:102320009A6B42F000729A639A6B00F5805422F027 +:1023300000729A63FFF72CFC94F8C830DB0718D4BE +:10234000B9B102211320FFF71DFC01F0F7FB0221B8 +:10235000142001F0F3FB0221152001F0EFFB94F8AB +:10236000C83043F0010384F8C830BDE81040FFF7DF +:102370000FBC10BD001002402DE9F04700F580555C +:1023800088B095F8C930012B04468846164600F2FD +:10239000FB807E4F57F823200AB947F82300D7F86F +:1023A00000A0C4F80C802674BAF1000F09D141F2E4 +:1023B000D00002F0BBFC51468146FFF74DFDC7F847 +:1023C000009095F8C930012B5DD001212046FFF720 +:1023D0009FFFFFF7D7FB6269136823F002031360C6 +:1023E0006269136843F001031360636900275F614A +:1023F00001212046FFF7CCFBFFF7ECFD002800F0A1 +:102400008480E86D00F076F904F58359BA4609F145 +:102410000809202200216846FFF72AFB02A8FFF7DF +:1024200077FCCDF818A06A4609EB07030DF1180EEA +:102430009446BCE80300F44518605960624603F115 +:102440000803F5D1DCF80000186020379CF8042060 +:102450001A71602FDDD195F8C8306FF38203002721 +:1024600085F8C8306A4641462046ADF80070ADF8A0 +:1024700002708DF80470FFF751FD6369C0B94FF425 +:1024800000421A6011E0386803681B6B98470146E8 +:1024900000289AD13868FFF73BFF38680368324656 +:1024A0001B684146984700288FD108B0BDE8F087E7 +:1024B00061221A609DF802309DF803201B06120469 +:1024C00002F4702203F040731343BDF80020C2F3FE +:1024D000090213439DF804201205022E02F4E002C3 +:1024E0000CBF4FF000410021134362690B43D361DD +:1024F000636913225A616269136823F00103136050 +:1025000039462046FFF766FD08B96369B7E795F8D5 +:10251000C93093BB6169D1F8002242F00102C1F8D1 +:1025200000226169D1F8002222F47C5222F00E02CE +:10253000C1F800226169D1F8002242F46062C1F85A +:1025400000226269C2F814326269C2F80432626918 +:1025500041F6FF71C2F80C126269C2F8403262693A +:10256000C2F8443263690122C3F81C226269D2F8BE +:10257000003223F00103C2F8003295F8C83043F06E +:10258000020385F8C83090E700208EE7F034002081 +:1025900008B500F029FA50EA0103024602D0421EB3 +:1025A00061F10001044B186810B10B46FFF748FDBC +:1025B000BDE8084001F01CB9F034002008B5002047 +:1025C000FFF7ECFDBDE8084001F012B908B50120A5 +:1025D000FFF7E4FDBDE8084001F00AB90FB40020A0 +:1025E00004B0704713B56C4684E80600031D94E8F8 +:1025F000030083E80500012002B010BD73B58568B3 +:10260000019155B11B885B0707D4D0E900369B6B5D +:102610009847019AC1B23046A847012002B070BD68 +:10262000F0B5866889B005460C465EB1BDF8383015 +:102630005B070AD4D0E900379B6B98472246C1B2AA +:102640003846B047012009B0F0BD00220023CDE993 +:1026500000230023ADF808300A4603AB01F1080659 +:10266000106851681C4603C40832B2422346F7D1B1 +:10267000106820601279227100F0B8F90423ADF8D7 +:1026800008302B68CDE90001DB6B69462846984786 +:10269000D8E7000030B503680968DD0FB5EBD17FDE +:1026A00023F0604421F060424FEAD1700BD0002B40 +:1026B000B8BFA40C0029B8BF920C944202D034BF1A +:1026C0000120002030BD944205D1C1F38070C3F3D6 +:1026D00080738342F6D194422CBF00200120F1E7A1 +:1026E00010B5037C044613B9006802F057FB20467E +:1026F00010BD00000023BFF35B8FC360BFF35B8F8F +:10270000BFF35B8F8360BFF35B8F7047BFF35B8F5B +:102710000068BFF35B8F704770B505460C30FFF75C +:10272000F5FF05F1080604463046FFF7EFFFA0422B +:1027300006D930466D68FFF7E9FF2544281A70BDB9 +:102740003046FFF7E3FF201AF9E7000070B50546B1 +:10275000406898B105F10800FFF7D8FF05F10C06B5 +:1027600004463046FFF7D2FF8442304694BF6D687E +:102770000025FFF7CBFF013C2C44201A70BD000060 +:1027800038B50C460546FFF7C7FFA04210D305F148 +:102790000800FFF7BBFF04446868B4FBF0F100FBDE +:1027A0001144BFF35B8F0120AC60BFF35B8F38BD7A +:1027B0000020FCE72DE9F041144607460D46FFF7DF +:1027C000C5FF844228BF0446D4B1B84658F80C6B04 +:1027D0004046FFF79BFF3044286040467E68FFF785 +:1027E00095FF331A9C4203D86C600120BDE8F0814C +:1027F0006B60A41B3B68AB602044E8600220F5E7F7 +:102800002046F3E738B50C460546FFF79FFFA04288 +:1028100010D305F10C00FFF779FF04446868B4FB9E +:10282000F0F100FB1144BFF35B8F0120EC60BFF3BC +:102830005B8F38BD0020FCE72DE9FF4188466946E3 +:102840000746FFF7B7FF6C4606B204EBC606002545 +:10285000B44209D06268206808EB0501FFF7F6F87A +:10286000636808341D44F3E729463846FFF7CAFF7A +:10287000284604B0BDE8F081F8B505460C300F4697 +:10288000FFF744FF05F1080604463046FFF73EFF18 +:10289000A042304688BF6C68FFF738FF201A3860C6 +:1028A00020B130462C68FFF731FF2044F8BD00000E +:1028B00073B5144606460D46FFF72EFF844228BF27 +:1028C00004460190DCB101A93046FFF7D5FF019B1A +:1028D00033B93268C5E90233C5E9002401200CE0B0 +:1028E0009C4238BF01942860019868608442F5D901 +:1028F0003368AB60241AEC60022002B070BD204641 +:10290000FBE700002DE9FF410F466946FFF7D0FFC6 +:102910006C4600B204EBC0050026AC4209D0D4F8E6 +:10292000048054F8081BB8194246FFF78FF8464454 +:10293000F3E7304604B0BDE8F081000038B5054645 +:10294000FFF7E0FF044601462846FFF719FF20463F +:1029500038BD000000B59BB0EFF3098168226846DE +:10296000FFF774F8EFF30583044B9A6BDA6A9A6AFF +:102970009A6A9A6A9A6A9A6A9B6AFEE700ED00E090 +:1029800000B59BB0EFF3098168226846FFF75EF857 +:10299000EFF30583044B9A6B9A6A9A6A9A6A9A6A69 +:1029A0009A6A9B6AFEE700BF00ED00E000B59BB0AD +:1029B000EFF3098168226846FFF748F8EFF30583D3 +:1029C000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE7FA +:1029D00000ED00E0FEE700000FB408B5029801F03A +:1029E0005FFBFEE701F0EEBD01F0C6BD01F0C4BD26 +:1029F00030B5094D0A4491420DD011F8013B5840C1 +:102A0000082340F30004013B2C4013F0FF0384EA49 +:102A10005000F6D1EFE730BD2083B8ED2DE9F0414D +:102A2000C56915B9C161BDE8F0814B6823F0604705 +:102A3000C3F38A464FEAD37EC3F3807816EA2306AF +:102A400038BF3E46AC462B465A68BEEBD27F22F0DA +:102A500060440AD0002A18DAA40CB44217D19D426F +:102A60000FD10D60DEE71346EEE7A74207D102F073 +:102A70008044C2F3807242450BD054B1EFE708D2D4 +:102A8000EDE7CCF800100B60CDE7B44201D0B442C2 +:102A9000E5D81A689C46002AE5D11960C3E7000012 +:102AA0002DE9F047089D01F007044FEAD5082244BC +:102AB00005F0070500EBD1004FF47F49944201D1A6 +:102AC000BDE8F08704F0070705F0070A57453E46C2 +:102AD00038BF5646C6F10806111B8E4228BF0E4667 +:102AE000E10808EBD50E415C13F80EC0B94029FA95 +:102AF00006F721FA0AF1FFB28CEA010147FA0AF758 +:102B000039408CEA010C03F80EC034443544D5E753 +:102B100080EA0120082341F2210201B2400000298D +:102B200080B203F1FF33B8BF504013F0FF03F4D17C +:102B30007047000038B50C468D18A54200D138BD4D +:102B400014F8011BFFF7E4FFF7E7000002684AB141 +:102B500013680360C388018901339BB29942C38023 +:102B600038BF03811046704770B588B020220446F4 +:102B70000D4668460021FEF77BFF20460495FFF7CF +:102B8000E5FF024658B16B46054608AE1C4603CC2D +:102B9000B44228606960234605F10805F6D1104665 +:102BA00008B070BD10B54B6823B9CA8A63F3090237 +:102BB000CA8210BDC4681A681C60C360438A013BA6 +:102BC00043824A60EFE700002DE9F84F1D46CB8AAB +:102BD0000F46C3F309010629814692460B4630D0C1 +:102BE0000020AAB207F119049EB2052E1FFA80F840 +:102BF0000FD8904503F1010306D3FB8A0A4462F320 +:102C00000903FB8201201AE01AF80060E654013043 +:102C1000EAE79045F1D2A1F1060B1C237C68BBFBCF +:102C2000F3F203FB12BB1FFA8BF6002C45D148468A +:102C3000FFF78CFF044638B978606FF00200BDE8FA +:102C4000F88F4FF00008E6E7002606607860ADB226 +:102C50004FF0000B454510D90AEB0803221D13F86D +:102C6000011B9155B1B208F101081B291FFA88F820 +:102C70002BD0454506F10106F1D8FB8AC3F30902C2 +:102C8000154465F30903BCE7013292B21C46236880 +:102C9000002BF9D1AB1F0B441C21B3FBF1F3013323 +:102CA0009BB29A42D3D2BBF1000FD0D14846FFF776 +:102CB0004DFF20B9C4F800B0BFE70122E7E7C0F834 +:102CC00000B05E4620600446C1E74545D5D9484678 +:102CD000FFF73CFF08B92060AFE7C0F800B000265E +:102CE00020600446B6E700002DE9F04F2DED028B81 +:102CF00083B0CDE90013BDF83C4080469246002ADF +:102D000000F087802CB10E9B002B00F08280072CF6 +:102D10002BD808F10C00FFF719FF054638B96FF002 +:102D20000205284603B0BDEC028BBDE8F08F1422EB +:102D30000021FEF79DFE22460E9905F10800FEF7E0 +:102D400085FE631C2B749AF800302C4403F01F039B +:102D500063F03F032372009B43F0004169604046EB +:102D60002946FFF75BFE0125DBE700F10C034FF07E +:102D7000000908EE103A4FF0800B4E464D4618EE13 +:102D8000100AFFF7E3FE07460028C8D014220021EE +:102D9000FEF76EFE002E3AD1019B3B8102220E9B74 +:102DA00002F1080103EB060C57FA81F11046B44218 +:102DB00002F10102D2B201D8024607E01CF8010B71 +:102DC00001F8010B0136062AB6B2EFD99AF80010C5 +:102DD00001F01F01B44208BF4FF0400BB81841EAA0 +:102DE00049114BEA01030372009B013243F0004397 +:102DF0007B603A7439464046FFF710FE0135B44215 +:102E00002DB289F001094FF0000BB8D189E70022FB +:102E1000C5E76FF0010584E7F8B515460E46242294 +:102E2000002104461F46FEF723FE069B6360B5F5AE +:102E3000001F079BA76034BF6A094FF6FF72236229 +:102E400004F10C0097B200239A4205D800230360D6 +:102E500027826382A382F8BD0660013330462036A4 +:102E6000F2E7000003781BB94BB2002BC8BF01701A +:102E700070470000007870472DE9F74FDDF83C906F +:102E8000BDF830500D9E9DF83840BDF8407080462A +:102E900092469B46B9F1000F01D1002F51D11F2C52 +:102EA0004FD898F80000B0B9072F47D835F0030382 +:102EB00047D13A4649464FF6FF70FFF73BFE20F0F8 +:102EC00001002D02400445EA0464400C44EA402419 +:102ED0004FF6FF7321E040EA0520072F40EA046423 +:102EE000F6D900254FF6FF73C5F12000A5F12002A9 +:102EF0002AFA05F10BFA00F001432BFA02F2114312 +:102F00001846C9B2FFF704FE0835402D0346EBD141 +:102F10003A464946FFF70EFE0346CDE90097324692 +:102F200021464046FFF7E0FE33780133DBB21F2B2A +:102F300088BF0023337003B0BDE8F08F6FF003004B +:102F4000F9E76FF00100F6E72DE9F04F85B0924602 +:102F5000DDF848800F9D9DF840209DF84490BDF815 +:102F60004C7006469B46B8F1000F01D1002F48D1A6 +:102F70001F2A46D83378002B46D00C0244EA02645C +:102F80009DF8381044EAC93444EA01441C43072F31 +:102F900044F0800432D900234FF6FF72C3F1200CB5 +:102FA000A3F120002AFA03F10BFA0CFC41EA0C0110 +:102FB0002BFA00F00143C9B210460393FFF7A8FDB6 +:102FC000039B0833402B0246E8D13A464146FFF7BF +:102FD000B1FD0346CDE900872A4621463046FFF77A +:102FE00083FEB9F1010F06D12B780133DBB21F2B21 +:102FF00088BF00232B7005B0BDE8F08F4FF6FF733C +:10300000E8E76FF00100F6E76FF00300F3E7000078 +:10301000C06900B104307047C3691A68C261C268F0 +:103020001A60C360438A013B438270472DE9F04137 +:10303000D0F81880194E14461D464146002709B99C +:10304000BDE8F081D1E90223A21A65EB03039642A1 +:1030500077EB03031ED283698B420DD1FFF7A2FDEC +:1030600083691B688361C3680B60438AC16081699F +:10307000013B43828846E2E7FFF794FD0B68C8F8FE +:103080000030C3680B60438AC160013B4382D8F8BB +:103090000010D4E788460968D1E700BF80841E008D +:1030A0002DE9F04F8BB00D46DDF8509014469B464D +:1030B0008046002800F01981B9F1000F00F0158159 +:1030C000531E3F2B00F21181012A03D1BBF1000FE7 +:1030D00040F00B810023CDE90833B8F81430B5EB8C +:1030E000C30F4FEAC30703D300200BB0BDE8F08F36 +:1030F0002B199F42D8F80C303ABF7F1BFFB22746EE +:103100001BB9D8F81030002B7AD02F2D4ED8C5F12E +:103110003006B7424FF000032CBFF6B23E46009394 +:103120002946D8F8080008AB3246FFF7B9FCA7EBF0 +:10313000060A35445FFA8AFAB8F8143003F10053EE +:10314000063BDB000493D8F80C3003933021039B3B +:1031500013B1BAF1000F2CD1D8F8100040B1BAF178 +:10316000000F05D0009608AB5246691AFFF798FC8D +:1031700038B2002FB8D066070AD00AAB03EBD401EF +:10318000624211F8083C02F00702134101F8083CC2 +:10319000082C3CD9102C40F2B580202C40F2B7808E +:1031A000BBF1000F00F09C80082334E0BA460026F3 +:1031B000C2E7049BE02B28BFE02306930B44AB42FD +:1031C000059314D95A1B03980096924534BF524672 +:1031D000D2B2691A08AB04300792FFF761FC079A74 +:1031E0001644AAEB020A1544F6B25FFA8AFA049B67 +:1031F000069A05999B1A0493039B1B680393A6E701 +:103200000093D8F8080008AB3A462946AEE7BBF170 +:10321000000F13D00123B4EBC30F6CD0082C12D8CD +:103220009DF82030621E23FA02F2D50706D54FF032 +:10323000FF3202FA04F423438DF820309DF8203049 +:1032400089F8003051E7102C12D8BDF82030621EEA +:1032500023FA02F2D10706D54FF0FF3202FA04F446 +:103260002343ADF82030BDF82030A9F800303CE70A +:10327000202C0FD80899631E21FA03F3DA0705D52D +:103280004FF0FF3202FA04F40C430894089BC9F88B +:1032900000302AE7402C2BD0DDE90865611EC4F11F +:1032A0002102A4F1210326FA01F105FA02F225FA1E +:1032B00003F311431943CB0712D50122A4F12003D4 +:1032C000C4F1200102FA03F322FA01F1A2405242B2 +:1032D00043EA010363EB430332432B43CDE9082365 +:1032E000DDE90823C9E90023FFE66FF00100FCE6F1 +:1032F0006FF00800F9E6082CA0D9102CB3D9202CC7 +:10330000EED8C3E7BBF1000FADD0022383E7BBF1DA +:10331000000FBBD004237EE730B5012A144638BF26 +:103320000124402C85B028BF40240025012ACDE986 +:10333000025518D81B788DF8083063070AD004AB03 +:1033400003EBD405624215F8083C02F007029340F3 +:1033500005F8083C009103462246002102A8FFF729 +:103360009FFB05B030BD082AE4D9102A03D81B887A +:10337000ADF80830E1E7202A8DBFD3E900231B68B0 +:103380000293CDE90223D8E710B5CB681BB98B6057 +:103390000B618B8210BDC4681A681C60C360438ACD +:1033A000013B4382CA60F0E72DE9F04FD1F800807D +:1033B00093B018F0800FCDE90323C8F3C01219BFF2 +:1033C000C8F3C03BC8F306264FF0020B1646B8F10F +:1033D000000F04460D4680F2C58118F0C0430593E6 +:1033E00040F0C0810B7B002B00F0BC81BBF1020FD1 +:1033F00003D00178B14240F0B88108F07F01069116 +:103400006AB3C8F3074A2B44069A93F803907606EA +:1034100046EA0B4646EA82465FEAD91346EA0A06BE +:10342000079300F0908000220023CDE90A23069B39 +:10343000009367685B4652460AA92046B8470028B1 +:103440007ED0A7699FB9314604F10C00FFF78CFBD1 +:103450000746E0B96FF0020013B0BDE8F08FC8F383 +:103460000F2A18F07F0F08BF0AF0030ACBE73B6969 +:103470009E420DD03F68002FF9D1314604F10C0077 +:10348000FFF772FB07460028E4D0A3693B60A76101 +:10349000DDE90A2300264FF6FF70C6F1200E22FA5E +:1034A00006F103FA0EFEA6F1200C23FA0CFC41EA09 +:1034B0000E0141EA0C01C9B2083609920893FFF7E0 +:1034C00027FB402EDDE90832E7D1B882FB7D09F009 +:1034D0001F06C3F38403F31AD7E9022198B2002B25 +:1034E000BCBF00F120031BB252EA0100C8F304681C +:1034F0000FD00398821A049860EB0101A148904212 +:103500004FF000028A4104D3079A002A55D0012BBC +:1035100023DDFA7D4FEA890302F0030203F07C0306 +:103520001343FB7539462046FFF73CFB079BA3B9C5 +:10353000FB7DC3F38402013262F38603FB7504E072 +:103540006FF00B0088E7A76917B96FF00C0083E7ED +:103550003B699E42BAD03F68F6E719F0400F2CD085 +:10356000039BBB60049BFB60142200210DA8FEF7A7 +:103570007FFA039B0A93049B0B932B1D0C932B7BCD +:10358000ADF83EA0013BDBB2ADF83C30069B8DF8B8 +:1035900043308DF840B0A3688DF841608DF84280CB +:1035A0000AA920469847FB7DC3F38403013303F047 +:1035B0001F039B02FB8200204EE7FB7DC9F34012F4 +:1035C000B2EBD31F40F0D480C3F38403B34240F086 +:1035D000D28007992B7B4FEA9912002934D0D20769 +:1035E00041D4032B40F2CA80039BBB60049BFB6069 +:1035F0002B7BAE1D033BDBB23246394604F10C0097 +:10360000FFF7E2FA00280DDA20463946FFF7CAFA3A +:10361000FB7DC3F38403013303F01F039B02FB8292 +:10362000032019E7AB883B832A7B033AB88AD2B2DE +:103630003146FFF77FFAFB7DB882DA43C2F3C0124E +:1036400062F3C713FB75B6E76AB92E1D013BDBB207 +:103650003246394604F10C00FFF7B6FA0028D3DBF6 +:103660002A7B013AE2E7F98AC1F30901013B052906 +:10367000DAB253D8281D002307F11A0C9A4208D950 +:1036800010F801EB0CF801E0013101330629DBB23F +:10369000F4D103990A9104990B91934207F11A010D +:1036A0000C9138BF043379680D9134BF55FA83F318 +:1036B00000230E93FB8AADF83EA0C3F309031A441E +:1036C000069B8DF843300023ADF83C208DF840B0C8 +:1036D0008DF841608DF842807B602A7BB88A013A80 +:1036E000291DFFF727FA3B8BB882834203D1A368D9 +:1036F0000AA92046984720460AA9FFF745FEFB7D08 +:10370000B88AC3F38403013303F01F039B02FB82D7 +:103710003B8B984214BF112000209DE67B68002B54 +:10372000B7D0062001E01C306346D3F800C0BCF1DE +:10373000000FF8D1091A081D05F1040C00EB03096C +:1037400005989DF8143001EB000EBEF11B0FA0D8B8 +:103750009A429ED91CF8013B09F8013B059B0133B5 +:103760000593EDE76FF0090076E66FF00A0073E667 +:103770006FF00D0070E66FF00E006DE66FF00F0059 +:103780006AE600BF80841E00EFF3098305494A6B97 +:1037900022F001024A63683383F30988002383F32C +:1037A0001188704700EF00E0202080F3118862B696 +:1037B0000C4B0D4AD96821F4E0610904090C0A4355 +:1037C000DA60D3F8FC20094942F08072C3F8FC208B +:1037D0000A6842F001020A601022DA7783F82200B8 +:1037E000704700BF00ED00E00003FA05001000E0A4 +:1037F00010B5202383F311880E4B5B6813F400632C +:1038000014D0F1EE103AEFF30984683C4FF0807366 +:10381000E361094BDB6B236684F3098800F0F2FB5C +:1038200010B1064BA36110BD054BFBE783F3118874 +:10383000F9E700BF00ED00E000EF00E00B06000834 +:103840000E060008026843681143016003B118477F +:1038500070470000024A136843F0C00313607047CA +:1038600000380140024A136843F0C00313607047F8 +:1038700000440040024A136843F0C00313607047DD +:103880000048004037B5244C244D204600F0FCFA97 +:10389000009404F1140022490023202200F0BEF914 +:1038A00020490094202204F138001F4B00F038FA20 +:1038B0001E4BC4E917351E4C204600F0E5FA009473 +:1038C00004F114001B490023202200F0A7F91A4933 +:1038D0000094202204F13800184B00F021FA184B14 +:1038E000C4E91735174C204600F0CEFA04F1140055 +:1038F000154900940023202200F090F9134B14493D +:103900000094202204F1380000F00AFA114BC4E9B7 +:10391000173503B030BD00BFFC34002000B4C40430 +:1039200040360020A03600205538000800380140FD +:103930006835002060360020C03600206538000859 +:1039400000440040D435002080360020753800083F +:10395000E03600200048004030B5037C304C0029A0 +:1039600018BF0C46012B0FD12E4B984239D12E4B4C +:103970001A6E42F480421A66D3F8802042F48042E4 +:10398000C3F88020D3F880302268036EC16D84664E +:1039900003EB5203B3FBF2F36268150442BF23F05A +:1039A000070503F0070343EA4503CB60A36843F030 +:1039B00040034B60E36843F001038B6042F496736D +:1039C00043F001030B604FF0FF330B62510505D547 +:1039D00012F010221FD0B2F1805F1ED080F8643048 +:1039E00030BD124B98420BD0114B9842CCD10E4BAC +:1039F0009A6D42F480229A659A6F42F4802207E021 +:103A0000094B9A6D42F400329A659A6F42F4003283 +:103A10009A679B6FB8E77F23E0E73F23DEE700BFAD +:103A200070510008FC34002000100240683500206E +:103A3000D43500202DE9F047C66D3768F46934624B +:103A40002107054618D014F0080118BF8021E207AD +:103A500048BF41F02001A30748BF41F04001600783 +:103A600048BF41F48071202383F31188281DFFF79C +:103A7000E9FE002383F31188E2050AD5202383F3AE +:103A800011884FF40071281DFFF7DCFE002383F33B +:103A900011884FF020094FF0000A14F0200838D1A7 +:103AA0003B0616D54FF0200905F1380A200610D53F +:103AB00089F31188504600F067F9002836DA0821AA +:103AC000281DFFF7BFFE27F080033360002383F338 +:103AD0001188790614D5620612D5202383F3118844 +:103AE000D5E913239A4208D12B6C33B11021281D3C +:103AF00027F04007FFF7A6FE3760002383F3118805 +:103B0000E30619D5AA6E1369B3B1BDE8F047506951 +:103B1000184789F31188B38C95F86410284619402A +:103B200000F0CCF98AF31188F469B6E780B23085E9 +:103B300088F31188F469B9E7BDE8F08700F16043C4 +:103B400003F561430901C9B283F80013012200F0B3 +:103B50001F039A4043099B0003F1604303F561434F +:103B6000C3F880211A607047F8B515468268066967 +:103B7000AA420B46816938BF8568761AB542044669 +:103B80000BD218462A46FDF761FFA3692B44A361B7 +:103B9000A3685B1BA3602846F8BD0CD918463246C3 +:103BA000FDF754FFAF1BE1683A463044FDF74EFF86 +:103BB000E3683B44EBE718462A46FDF747FFE36816 +:103BC000E5E7000083689342F7B51546044638BF21 +:103BD0008568D0E90460361AB5420BD22A46FDF753 +:103BE00035FF63692B446361A36828465B1BA360B0 +:103BF00003B0F0BD0DD932460191FDF727FF0199C1 +:103C0000E068AF1B3A463144FDF720FFE3683B44D0 +:103C1000E9E72A46FDF71AFFE368E4E710B50A442E +:103C20000024C361029B8460C0E90000C0E9051163 +:103C3000C1600261036210BD08B5D0E9053293424C +:103C400001D1826882B98268013282605A1C426165 +:103C50001970D0E904329A4224BFC368436100213D +:103C600000F0A0FA002008BD4FF0FF30FBE7000095 +:103C700070B5202304460E4683F31188A568A5B1CC +:103C8000A368A269013BA360531CA3611578226954 +:103C9000934224BFE368A361E3690BB120469847D0 +:103CA000002383F31188284607E03146204600F0C0 +:103CB00069FA0028E2DA85F3118870BD2DE9F74F23 +:103CC00004460E4617469846D0F81C904FF0200A3E +:103CD0008AF311884FF0000B154665B12A4631462C +:103CE0002046FFF741FF034660B94146204600F0F9 +:103CF00049FA0028F1D0002383F31188781B03B020 +:103D0000BDE8F08FB9F1000F03D001902046C847FD +:103D1000019B8BF31188ED1A1E448AF31188DCE7AE +:103D2000C0E90511C160C3611144009B8260C0E914 +:103D30000000016103627047F8B504460D4616465F +:103D4000202383F31188A768A7B1A368013BA36070 +:103D500063695A1C62611D70D4E904329A4224BF1F +:103D6000E3686361E3690BB120469847002080F364 +:103D7000118807E03146204600F004FA0028E2DA14 +:103D800087F31188F8BD0000D0E905239A4210B5E9 +:103D900001D182687AB98268013282605A1C8261DC +:103DA0001C7803699A4224BFC3688361002100F034 +:103DB000F9F9204610BD4FF0FF30FBE72DE9F74F32 +:103DC00004460E4617469846D0F81C904FF0200A3D +:103DD0008AF311884FF0000B154665B12A4631462B +:103DE0002046FFF7EFFE034660B94146204600F04B +:103DF000C9F90028F1D0002383F31188781B03B0A0 +:103E0000BDE8F08FB9F1000F03D001902046C847FC +:103E1000019B8BF31188ED1A1E448AF31188DCE7AD +:103E2000026843681143016003B1184770470000FE +:103E30001430FFF743BF00004FF0FF331430FFF79B +:103E40003DBF00003830FFF7B9BF00004FF0FF332F +:103E50003830FFF7B3BF00001430FFF709BF000090 +:103E60004FF0FF311430FFF703BF00003830FFF789 +:103E700063BF00004FF0FF323830FFF75DBF000036 +:103E800000207047FFF7FEBC044B03600023C0E92D +:103E90000233436001230374704700BF8851000858 +:103EA00010B52023044683F31188FFF755FD022344 +:103EB0002374002383F3118810BD000038B5C36953 +:103EC00004460D461BB904210844FFF7A9FF294603 +:103ED00004F11400FFF7B0FE002806DA201D4FF4AD +:103EE0008061BDE83840FFF79BBF38BD024B002220 +:103EF000C3E900339A607047003700200023037441 +:103F00008268054B1B6899689142FBD25A6803602E +:103F100042601060586070470037002008B52023C9 +:103F200083F31188037C032B05D0042B0DD02BB910 +:103F300083F3118808BD436900221A604FF0FF33F4 +:103F40004361FFF7DBFF0023F2E7D0E900321360A3 +:103F50005A60F3E7002303748268054B1B68996875 +:103F60009142FBD85A680360426010605860704705 +:103F700000370020054B19690874186802681A6038 +:103F80005360186101230374FCF72ABB003700203B +:103F900030B54B1C0B4D87B0044610D02B690A4A34 +:103FA00001A800F019F92046FFF7E4FF049B13B1C4 +:103FB00001A800F04DF92B69586907B030BDFFF733 +:103FC000D9FFF8E7003700201D3F000838B50C4D39 +:103FD00041612B6981689A689142044603D8BDE823 +:103FE0003840FFF78BBF1846FFF7B4FF01232C6161 +:103FF000014623742046BDE83840FCF7F1BA00BF03 +:1040000000370020044B1A681B6990689B6898422F +:1040100094BF0020012070470037002010B5084CE5 +:10402000236820691A6822605460012223611A748F +:10403000FFF790FF01462069BDE81040FCF7D0BAB9 +:104040000037002008B5FFF7DDFF18B1BDE80840D4 +:10405000FFF7E4BF08BD0000FFF7E0BFFEE7000088 +:1040600010B50C4CFFF742FF00F0A8F80A49802277 +:10407000204600F03DF8012344F8180C0374FFF7C4 +:1040800093FB002383F3118862B60448BDE8104017 +:1040900000F04EB828370020B0510008C051000889 +:1040A00008B572B6034B586200F060FA00F00CFBE2 +:1040B000FEE700BF0037002000F004B9EFF31180E5 +:1040C00020B9EFF30583202282F3118870470000A6 +:1040D00010B530B9EFF30584C4F3080414B180F3CC +:1040E000118810BDFFF7AEFF84F31188F9E70000D7 +:1040F00082600222028270478368A3F17C0243F847 +:104100000C2C026943F83C2C426943F8382C074ACE +:1041100043F81C2CC26843F8102C022203F8082C28 +:10412000002203F8072CA3F118007047F9050008D6 +:1041300010B5202383F31188FFF7DEFF002104462A +:10414000FFF744FF002383F31188204610BD0000D1 +:10415000024B1B6958610F20FFF70CBF003700208E +:10416000202383F31188FFF7F3BF000008B5014651 +:10417000202383F311880820FFF70AFF002383F32D +:10418000118808BD49B1064B42681B6918605A6026 +:10419000136043600420FFF7FBBE4FF0FF30704711 +:1041A000003700200368984206D01A680260506009 +:1041B00059611846FFF7A2BE70470000054B03F196 +:1041C0001402C3E905224FF0FF310022C3E90712B0 +:1041D000704700BF0037002070B51C4EC0E90323B4 +:1041E00005460C4600F0EAFA334653F8142F9A427B +:1041F0000DD13062C5E901242A600A2C2CBF0019B8 +:104200000A30C6E90555BDE8704000F0C5BA316A0C +:10421000431AE31838BF1C469368A34202D9081911 +:1042200000F0C8FA73699A6894420CD85A68AC6076 +:104230002B606A6015609A685D60121B9A604FF08F +:10424000FF33F36170BD1B68A41AECE70037002050 +:1042500038B51B4C636998420DD0D0E90032136029 +:104260005A6000228168C2609A680A449A604FF0DE +:10427000FF33E36138BD2246036842F8143F002152 +:1042800093425A60C16003D1BDE8384000F08CBA57 +:104290009A688168256A0A449A6000F08FFA636917 +:1042A0009A68411B8A42E5D9AB181D1A092D206A6C +:1042B00098BF01F10A02BDE83840104400F07ABA14 +:1042C000003700202DE9F041184C002704F11406B6 +:1042D000656900F073FA236AAA68C11A8A4215D880 +:1042E00013442362D5E9003213605A606369D5F83C +:1042F0000C80EF60B34201D100F056FA87F31188C9 +:104300002869C047202383F31188E1E76169B1423E +:1043100009D013441B1ABDE8F0410A2B2CBFC0186A +:104320000A3000F047BABDE8F08100BF0037002036 +:104330000C2303604FF0FF307047000000207047EF +:10434000FEE70000704700004FF0FF3070470000AC +:10435000BFF34F8F024A1369DB03FCD4704700BFE1 +:104360000020024008B5094B1B7873B9FFF7F0FF36 +:10437000074B5A69002ABFBF064A9A6002F1883289 +:104380009A601A6822F480621A6008BD883800209A +:10439000002002402301674508B50B4B1B7893B9F9 +:1043A000FFF7D6FF094B5A6942F000425A611A687A +:1043B00042F480521A601A6822F480521A601A6815 +:1043C00042F480621A6008BD883800200020024054 +:1043D0007F289ABF00F58030C0020020704700009F +:1043E0004FF4006070470000802070477F2808B5B8 +:1043F0000BD8FFF7EDFF00F500630268013204D12E +:1044000004308342F9D1012008BD0020FCE7000000 +:104410007F2810B504461FD8FFF79AFFFFF7A2FFC9 +:104420000E4BF3221A6102225A615A6942EAC00213 +:104430005A615A6942F480325A61FFF789FF4FF49A +:104440000061FFF7C5FF00F03BF9FFF7A5FF20462D +:10445000BDE81040FFF7CABF002010BD0020024099 +:104460002DE9F84340EA020313F00703144606D08F +:104470002E4B40F2F5221A600020BDE8F883851823 +:104480002B4A95420CD9294A40F2FA211160F3E7F0 +:10449000031D1B684A68934208D1083C0830083164 +:1044A000072C14D902680B689A42F1D0FFF75AFF23 +:1044B000FFF74EFF1F4E08314FF001084FF0000983 +:1044C000012CA1F1080706D8FFF766FF01E0002CD8 +:1044D000ECD10120D1E7C6F81480054651F8083C1C +:1044E00045F8043B51F8043C4360FFF731FFC6F840 +:1044F0001490026851F8083C9A420CD00B4B4FF4D0 +:1045000048721A600C4B18600C4B1C600C4B1F60FF +:10451000FFF742FFB0E72D6851F8043C9D4201F1DE +:104520000801EBD1083C0830CAE700BF84380020FE +:104530000000040800200240783800208038002065 +:104540007C380020084908B50B7828B11BB9FFF763 +:1045500009FF01230B7008BD002BFCD0BDE808400B +:104560000870FFF719BF00BF883800204FF4803172 +:104570004FF0005000F0A6B870B582B0FFF79EFD76 +:104580000E4E054600F01AF93268904237BF0C4AC9 +:104590000B49516814682EBFD1E9004101315160C7 +:1045A0000419034641F10001284601913360FFF7E9 +:1045B0008FFD0199204602B070BD00BF8C380020ED +:1045C0009038002070B582B0FFF778FD104E054698 +:1045D00000F0F4F83268904237BF0E4A0D49516836 +:1045E00014682EBFD1E9004101315160041941F135 +:1045F00000010346284601913360FFF769FD0199E8 +:104600004FF47A7200232046FBF7D2FD02B070BD52 +:104610008C3800209038002010B50244064BD2B2EE +:10462000904200D110BD441C00B253F8200041F864 +:10463000040BE0B2F4E700BF50280040114B30B546 +:10464000D3F89040240409D4D3F89040C3F89040A4 +:10465000D3F8904044F40044C3F890400A4C2368D7 +:1046600043F4807323600244084BD2B2904200D1DD +:1046700030BD441C00B251F8045B43F82050E0B256 +:10468000F4E700BF001002400070004050280040D6 +:1046900007B5012201A90020FFF7BEFF019803B072 +:1046A0005DF804FB13B50446FFF7F2FFA04205D006 +:1046B000012201A900200194FFF7C0FF02B010BD44 +:1046C000704700007047000070470000074B45F23C +:1046D00055521A6002225A6040F6FF729A604CF6F8 +:1046E000CC421A60024B01221A7070470030004021 +:1046F0009C380020034B1B781BB1034B4AF6AA22BF +:104700001A6070479C38002000300040054B1A6842 +:1047100032B902F1804202F50432D2F894201A60D4 +:10472000704700BF98380020024B4FF40002C3F8D6 +:10473000942070470010024008B5FFF7E7FF024BD6 +:104740001868C0F3407008BD98380020704700001A +:10475000FEE700000A4B0B480B4A90420BD30B4B71 +:10476000DA1C121AC11E22F003028B4238BF00224B +:104770000021FDF77DB953F8041B40F8041BECE75A +:104780006C520008343900203439002034390020BC +:104790007047000000F07AB84FF080430022586361 +:1047A0001A610222DA6070474FF080430022DA601B +:1047B000704700004FF08043586370474FF08043CC +:1047C000586A7047264B2748DA6A42F0070210B44D +:1047D000DA62DA6A22F00702DA62DA6ADA6C42F046 +:1047E0000702DA64DA6E42F00702DA664FF09042AE +:1047F000DB6E4FF0AA31002353604FF4EE4491601A +:10480000D0604FF6FF7050611462174C5362146011 +:10481000164CC2F80434C2F80814C2F80C444FF61F +:10482000F774C2F814444FF0EE44C2F8204447F243 +:104830009974C2F824440E4CC2F80044C2F80438FB +:10484000C2F80818C2F80C38C2F81408C2F82038A8 +:10485000C2F82438C2F800385DF8044B00F09CB868 +:104860000010024050010024A00100281050500008 +:1048700050A0AA0008B500F035FAFFF7F1FBBDE83B +:104880000840FFF743BF0000704700000F4B9A6DD0 +:1048900042F001029A659A6F42F001029A670C4A4F +:1048A0009B6F936843F0010393604FF080434F2266 +:1048B0009A624FF0FF32DA6200229A615A63DA603C +:1048C0005A6001225A611A60704700BF001002400E +:1048D000002004E04FF0804208B51169D3680B4016 +:1048E000D9B2C9439B07116107D5202383F31188EF +:1048F000FFF7E2FB002383F3118808BD08B5244BC2 +:104900004FF0FF319A6A99629A6A00229A62986A15 +:10491000D86A60F00700D862D86A00F00700D86251 +:10492000D86A186B1963186B1A63186B986B60F070 +:1049300080509863986B00F080509863986BD86BA8 +:10494000D963D86BDA63D86B186C1964196C1A6464 +:10495000196C996D41F080519965996F41F08051C2 +:104960009967996FD3F8901011F4407F1EBF4FF4F0 +:104970008031C3F89010C3F89020D3F89020C3F88A +:10498000902000F07FF9034B00225A6008BD00BF61 +:104990000010024000700040394B3A4A9A650122EB +:1049A0001A601A689007FCD500229A609A6812F083 +:1049B0000C0FFBD1344A4FF4007111605169490565 +:1049C000FCD41A6842F480321A601A689203FCD54B +:1049D0002D490A6842F480720A602C4A4FF4E06163 +:1049E00011601A6842F060021A601A6842F0080208 +:1049F0001A601A689007FCD59A6812F00C0FFBD168 +:104A0000D3F8942042F4C062C3F89420204ADA60BC +:104A10001A6842F080721A601A689101FCD51D4A2A +:104A20001A611A6842F080621A601A681201FCD595 +:104A300000229A601549184AC3F888200A6822F0B3 +:104A4000070242F004020A600A6802F00702042A20 +:104A5000FAD19A6842F003029A609A6802F00C0256 +:104A60000C2AFAD11A6E42F001021A66D3F880209D +:104A700042F00102C3F88020D3F88030704700BFB5 +:104A800000100240000400100070004000200240AE +:104A900003140001000C100055550134074A08B5F5 +:104AA000536903F00103536123B1054A13680BB145 +:104AB00050689847BDE80840FEF79ABE00040140E0 +:104AC000A0380020074A08B5536903F00203536178 +:104AD00023B1054A93680BB1D0689847BDE80840F8 +:104AE000FEF786BE00040140A0380020074A08B542 +:104AF000536903F00403536123B1054A13690BB1F1 +:104B000050699847BDE80840FEF772BE00040140B6 +:104B1000A0380020074A08B5536903F00803536121 +:104B200023B1054A93690BB1D0699847BDE80840A5 +:104B3000FEF75EBE00040140A0380020074A08B519 +:104B4000536903F01003536123B1054A136A0BB193 +:104B5000506A9847BDE80840FEF74ABE000401408D +:104B6000A0380020164B10B55C6904F478725A61C5 +:104B7000A30604D5134A936A0BB1D06A984760061E +:104B800004D5104A136B0BB1506B9847210604D51E +:104B90000C4A936B0BB1D06B9847E20504D5094AD8 +:104BA000136C0BB1506C9847A30504D5054A936C60 +:104BB0000BB1D06C9847BDE81040FEF719BE00BF9E +:104BC00000040140A0380020194B10B55C6904F4C2 +:104BD0007C425A61620504D5164A136D0BB1506DC3 +:104BE0009847230504D5134A936D0BB1D06D9847B0 +:104BF000E00404D50F4A136E0BB1506E9847A10420 +:104C000004D50C4A936E0BB1D06E9847620404D55C +:104C1000084A136F0BB1506F9847230404D5054A17 +:104C2000936F0BB1D06F9847BDE81040FEF7E0BD21 +:104C300000040140A038002008B50348FEF7FAFE42 +:104C4000BDE80840FEF7D4BDFC34002008B5034899 +:104C5000FEF7F0FEBDE80840FEF7CABD683500204B +:104C600008B50348FEF7E6FEBDE80840FEF7C0BD04 +:104C7000D435002008B5FFF72DFEBDE80840FEF74B +:104C8000B7BD0000062108B50846FEF757FF06210C +:104C90000720FEF753FF06210820FEF74FFF0621ED +:104CA0000920FEF74BFF06210A20FEF747FF0621E9 +:104CB0001720FEF743FF06212820FEF73FFF0721BC +:104CC0001C20FEF73BFF0C212520FEF737FF0C21AF +:104CD0002620FEF733FFBDE808400C212720FEF711 +:104CE0002DBF000008B5FFF709FE00F009F8FFF737 +:104CF000C9F8FFF7C9FDBDE80840FFF74BBD00004C +:104D00000023054A19460133102BC2E9001102F1B4 +:104D10000802F8D1704700BFA03800200B460146BA +:104D2000184600F025B8000000F038B8012838BF58 +:104D3000012010B50446204600F028F830B900F0F4 +:104D400007F808B900F00CF88047F4E710BD000040 +:104D5000024B1868BFF35B8F704700BF20390020FB +:104D600008B5062000F032F90120FFF7E9FA00004B +:104D700010B5054C13462CB10A4601460220AFF38C +:104D8000008010BD2046FCE700000000024B0146F9 +:104D9000186800F089B800BF28110020024B0146B6 +:104DA000186800F035B800BF2811002010B501398F +:104DB0000244904201D1002005E0037811F8014F30 +:104DC000A34201D0181B10BD0130F2E72DE9F041DC +:104DD000A3B1C91A17780144044603F1FF3C8C4281 +:104DE000204601D9002009E00578BD4204F1010404 +:104DF000F5D10CEB0405D618A54201D1BDE8F08130 +:104E000015F8018D16F801EDF045F5D0E7E7000043 +:104E100037B5002944D051F8043C0190002BA1F192 +:104E20000404B8BFE41800F0F5F81E4A01981368AE +:104E300033B96360146003B0BDE8304000F0F0B8EF +:104E4000A34208D9256861198B4201BF19685B68C4 +:104E500049192160EDE71A465B680BB1A342FAD904 +:104E600011685518A5420BD1246821445418A34257 +:104E70001160E0D11C685B68536021441160DAE77F +:104E800002D90C230360D6E7256861198B4204BF61 +:104E900019685B68636004BF491921605460CAE700 +:104EA00003B030BD24390020F8B5CD1C25F0030532 +:104EB00008350C2D38BF0C25002D064601DBA94214 +:104EC00003D90C2333600020F8BD00F0A3F821497A +:104ED0000A6814469CB9204F3B6823B921463046E6 +:104EE00000F03CF838602946304600F037F8431CA3 +:104EF00023D10C233360304600F092F8E3E72368B7 +:104F00005B1B17D40B2B03D923601C44256004E0E2 +:104F10006368A2420CBF0B605360304600F080F81B +:104F200004F10B00231D20F00700C21ACCD01B1A7D +:104F3000A350C9E722466468CCE7C41C24F00304EC +:104F4000A042E3D0211A304600F008F80130DDD14C +:104F5000CFE700BF243900202839002038B5064D9E +:104F60000023044608462B60FFF7E2F9431C02D1F8 +:104F70002B6803B1236038BD2C3900201F2938B5B8 +:104F800004460D4604D9162303604FF0FF3038BDA8 +:104F9000426C12B152F821304BB9204600F030F883 +:104FA0002A4601462046BDE8384000F017B8012BDC +:104FB0000AD0591C03D1162303600120E7E700241F +:104FC00042F82540284698470020E0E7024B01467A +:104FD0001868FFF7D3BF00BF2811002038B5074D70 +:104FE00000230446084611462B60FFF7ADF9431C29 +:104FF00002D12B6803B1236038BD00BF2C390020DB +:10500000FFF79CB9034611F8012B03F8012B002A86 +:10501000F9D17047014800F009B800BF30390020CD +:10502000014800F005B800BF30390020704700008B +:10503000704700006F72672E6172647570696C6FE3 +:10504000742E4D6174656B4C3433312D506572692B +:10505000706800004E6F20617070207369670A00ED +:10506000426164206677206C656E677468202575E0 +:105070000A0042616420626F6172645F6964202586 +:10508000752073686F756C642062652025750A0051 +:105090004261642066772064657363726970746F1F +:1050A00072206C656E6774682025750A0042616421 +:1050B0002061707020435243203078253038783A90 +:1050C000307825303878203078253038783A307884 +:1050D000253038780A00476F6F64206669726D77F3 +:1050E0006172650A0040A2E4F16468910600000064 +:1050F00053544D33324C343F3F000000FC34002009 +:1051000068350020D43500204261642043414E4977 +:105110006661636520696E6465782E00800000001A +:10512000008000000000800000000000000000007F +:1051300079230008D92200089D1B0008D51B000810 +:10514000D11D0008A11B0008B51B0008A51B000805 +:10515000A91B0008B11B0008AD1B0008F91C0008C2 +:10516000B91B0008E5250008C91B0008CD1C000874 +:105170000096000000000000000000000000000099 +:105180000000000000000000000000004D3E00088C +:10519000393E0008753E0008613E00086D3E00087B +:1051A000593E0008453E0008313E0008813E000897 +:1051B0006D61696E0000000069646C6500000000AC +:1051C000B851000840370020783800200100000066 +:1051D0005D4000080000000008B0FF7F01000000F3 +:1051E0002604000000000000006003000000000032 +:1051F000FE2A0100D2040000FF00000000000000B1 +:10520000F05000083F0000002C11002000000000BA +:10521000000000000000000000000000000000008E +:10522000000000000000000000000000000000007E +:10523000000000000000000000000000000000006E +:10524000000000000000000000000000000000005E +:10525000000000000000000000000000000000004E +:0C52600000000000000000000000000042 +:00000001FF From 54e4c6e1be95de6723506a45df433782edd855a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 17:47:49 +1100 Subject: [PATCH 0352/3101] hwdef: leave MatekL431 ROMFS uncompressed this allows for bl update when low on memory --- libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc index 82492ac3fca..2e5d1f2d8bf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431/hwdef.inc @@ -101,3 +101,7 @@ PB11 USART3_RX USART3 SPEED_HIGH NODMA # allow for reboot command for faster development define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 + +# keep ROMFS uncompressed as we don't have enough RAM +# to uncompress the bootloader at runtime +env ROMFS_UNCOMPRESSED True From 62db423758bfee5a98fc6a7547b41351d9e10937 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 18 Mar 2022 17:48:03 +1100 Subject: [PATCH 0353/3101] Tools: update MatekL431 bootloaders --- Tools/bootloaders/MatekL431-Airspeed_bl.bin | Bin 21108 -> 21284 bytes Tools/bootloaders/MatekL431-Airspeed_bl.hex | 2017 ++++++++++--------- Tools/bootloaders/MatekL431-Periph_bl.bin | Bin 21100 -> 21284 bytes Tools/bootloaders/MatekL431-Periph_bl.hex | 2016 +++++++++--------- 4 files changed, 2028 insertions(+), 2005 deletions(-) mode change 100644 => 100755 Tools/bootloaders/MatekL431-Airspeed_bl.bin mode change 100644 => 100755 Tools/bootloaders/MatekL431-Periph_bl.bin diff --git a/Tools/bootloaders/MatekL431-Airspeed_bl.bin b/Tools/bootloaders/MatekL431-Airspeed_bl.bin old mode 100644 new mode 100755 index defa6d416371edcd858eb4af21cd35b6383dbf0d..1621f6485cbc62b4ff95c54cd6f12587838820ed GIT binary patch delta 4077 zcmYk93sh5Ax`6jSCy|ptNCE^B1Cj@iKm-F34WN?nI0RCP=)A1U5o%o%r&?foJ7QU0 zgE$>V%c{6LElPW@7CYK;tU%0+)JAkhJ8H`cj{NE|I^yKHt>l}!A4tO27 z1bowTOD{i?iI`E~CcwKWdIl4z%(|X@on-1U#DoEP9b$I$4C)Wy*W8D37cJU1BjA1p z93ch5YGD=RCdf`e($_zDzLn-KlfZ&zM!<=J z0!|$y&%_*(wTF+^zVS~3OW3VsN@)Be_uAMzj7LdFTm?QsF2(%@uOamkHKm&+TP3+v z+$`yp7>v@H(gE*XuJhD=$tA7XSzloEdf&1=~q ziGK~v=mdTl=JoVYEvq;o@GIvvWhnWC73wVWnmnXD!CSV?Yhv(hn)h_g>r_bK?-EPM zpj4rFFf&ml@?+8c@ig#Q$MTWCBG;v9_&S-9>hTqlCR+lsPL@)9do^NY9R_UK+v~M|^06U$4^%NOKU{xv`N(=rCd!$R`0=0Mi%^R6^PW>;>|sxcZX=3r@M% zIEZ;1`h*NmjO+iCGOEYF*o%$Iam>ldxO|=Rh3MD57=IFq2DXQxAO0k4NuxPQgp?=b ziZ@4R+N=LQ^$%QYF!$b^!l7!D4<^8r+?m5%BQYzqikcZ_P^vd}&;uqfKRMQNDGHJ; zisjPIFwHi)QRe8B^oQ}kJxNX}($z_!-71?O(df7FZ&_HxQM1R(g*W-$Z6Z^OZ0Mal zIWzkwk=KK%CS=XWickTqhibB1DLS>FHl9501)O1H7S_;F-R8lm-+{@GT{H4{Zj5&j!- zq@?3F$kQpAIGgmOXel;APN!%d=Xcd*;1X~VIKCA0N4ww;koUiL?82Cnk)DJ+5+FO{ z5!w;AXV!a{vC_4I8l<7Q$#>8Q9|si?o${yh(K)1f0GsInxG3SM8X!H&jdETafGun@>+`>Wz)xO|*E?$oIS@Kyi)dkRLUoX^j;*TdMi|0mN$* zcgu^A(6P`dbf~-f7Iv0Xld?*Rd&LXKOf~Pz$FO)}>IAI8sezi8!iHf!=e=tI??YY- zv;%g8s0#3D_a#*st~t#&=~&nXv_ND3>^|dRY8ZR{gQkkuMv6-a32GMDDK0+5&j79w zK%RN$2KWk!pmE>^sZ(c5z8OX%eChnKNrzf3lbxoVKX5K&_3VbAz@e-ruc_DIKa;*{yioQl_v zPQfPDpA0f4qjL+Z-b#kj3h`gbmuaPW?s*fWzXB#+F!+5CnWBNCky>^wYhx`e!ynbv zh?_hc4ICtOi%tmnqvc%^UZlozM3sJxlFX3r(rfXUd!;4|E4r}j;*?mgWszS0LPO9{ zs^Qyf)2@~VVUR%&n(k~^#6|=EL#}D7iU!|BBV!Pc@k9e}&P4-LaE$Szc=KKUlB(Du zWP6>6`+W6)|MQJfHht{wARN}DLAPApJ{R<2&72QSyH@kw+vfOLSP_Z_o+16ZR=k8L z^xD#lxtD{NP1j?T9}f>w+C)t>pn$ivdXCa2Y2XxrQ~_z(yj2B<={;c0AEePh3~AL@ zP_be1NBtuFA^AkF#TQ9H52pfAXEdj*f4a9C!;5%m+~SM|N~0Dh?JQ*1lOq}TaFBFo z=45Ui<_FAp5H$LAczzFXPYyF44EpUy`V-`OrWsCrX;u!%;;ho-16^;p5O-!q@MyJQ zdW$PhOs24aCYKmx=Yl18i=6TA>{fDA|ilz;|+PYdt^+esOa z1SkOwU^2WoBGtr$jR+6}u>c=K0w5&wIGp<7t9v)y7g1Mt%ZS^!3;%$a7w^OhGPqca z1!Q7z)~?ktY!8ca5qH3qHyGcCjEL)Y#pD%!8sm>c(ZH>UfVB)abCdwCxdHFj)IQjc zFD2=aHWJtni9eXj!X=L)fvSiz_54sYAdf}^_amDBI0*X~TarR9j^@(oO%zwlw*m%|eGa+Ep*+}48nB?XkpaNlXF5j*_A7+ZIc{F=( zyo9ZR*jM2Og3a`5A&lM1w?(p-ysCdHO!qNntC3>wf}IV@w>`A`buQSMK;KSoE!l}* zAsbC<+(p_=gZMd;S73vMY%bV_)7&=;Ix${Pn#|j^;~h%we1(u$xeq#G+xdmO+;P@V zN$D}%Hdt=uE|MAZ53!xpFE!(Q(z$e{@{5o)|K=1^=o>$?dv>RYOJDGp`+Mjuzowbv$?h&hCE*Aq!m8a5GQkvXw&jP5(zDCnDQ<(z1134{8ltjOv203VtVehJCSjaXqV@i4}E&5ymvA+h)YF) zun(JsBJ}e+&72th45S1-1u`Brfs~@3x;vJ?ii`RC2c5nsurGsK91s@6AA<#3 z`8&x8`ei`DmsKL`4JhjZJrnf1q@nmjxdHw!<|D|cd5;_Jf|4KOWuj>R7Gq_9Q@?Y) zy1&uD2EA}lu!U?mFc&$czU3@oO_kxm_mMB0rIq)5N;Vg2{vILcN_Cc-kxk&b30w^< z?@SMHUdd!3i+jR>4C=gw2=61D)`FVDMA`FQ6)N8HL4dnn4fhWyv6Con^1R;7bf zmHilGYnh7v=pBZ1mn|1E%aLZ3Tq!dP55ngML$Ec4x3gx_ZD3)ut`(cW!U@A_gHYCF zsPtHmf!tuL0C~h_2Kk9?JIK27XF*;le;Q<7#YViJbXSz({q8Fjo3P?V3rs5#Fz|6; zbwR!h*h6ep*+s+!p6!_+{F_o1#}uj8Y{&nAKtR;S~`B(qu%vZmUo z${b!B6sWDh*S?Ikh*?n#zjHk9?rJlQkCO|v*&wIprQE^ii|1vNV-w^rIht1 zZI1*(^H*ym@UfR)y@zGoqq3`Be(fKY-FZ}Y-g|?zK6VV^sOl=P!M&lb8so*}&Bsp( z7egORWLelW z{FlJ3G{g)-dI|6V!@vc=3y7fZ3Z&EUvpfmuP14$s-Vj~|5rE7x_*Vwf01+?^Ww(F= z$Zu=f@(lbEqb*Nu-OQ&gu(ozsV}4aI=n@-jZg^IoitfVX%LYAlw1J4%{QUm`G@Sbd delta 3926 zcmYjU3s_Xwwch)j88|$K8QwDtgn95Dl*fR)G#myzyn>+A=Fz4OHGX%fH9{2wAyyEZ zVxqSpI}em=qcLf;>L)moLd+CXC7;^(1(Y`y=a=HOSa_@7Z>iwh?#G zW{#yJB1_MS4|?bJj-&6&a>;p0Z)bP1<+zF6!z0*8B#!bb0StGtylKd}Khnwo!!;BRnK9w2ItP z?A0{-3|)AT%`yrZ6Rd&t$Z~Xg#=y`@G7u4;UgUcdMuQhegJCo%CsohFsL6*1WiF0Y z88Y+?O(7pgm~k6nl{q_Xo<^z*$$ctQ$Q7RVEPqLK4~Q|xWC9jALk7(Hg_koNQY&1m zS&6&mRk+r$oI&PNVTr%$trKF8$z1A1?jvv9F&@T>7r9BV`k18IxybzWdlvb3 zy)svSsDg|rV`GnaudWPn;gGx2Nnr7~AbI{Zxu#6Qzan0x5pO3+svMvVs`!$ECd8>m zL+eB960Gd~o@U~)>PpA|vMGB+Ct$jCUMH}$G2pnV;@KKZ)6P)X!vZ)3#pHl$MS5pI zF654<*PCiW4GF!NcKW_@;?|M-kCg|_8tT((!WN~Q2#(ZXE73+8m5(jt3&DaMRw(&j zO&TJTHx&8ab}GQT*@sgOInjiauhUUTHwUr(WgUyqLBu@<{igui0Gw5HzXI(D;Lm_1 zbF}fqaL~yfAsl@E5Mn|ZPfQzs&X}~*U%r4%>S;_v$aLi6>QeExGuRb_ME5g(h{F|w zi<4M7>L-?{tWb^U?c~9M+fTMdZDVEwZ7IHoMkN7M(8dF~`Ehr8lUD2qzL0zk>Sc#> z9n$9T7A2yjHMU4uhm43$gfM$ z#gn<%bi9{D#-*k=1rB%;bCHRsuk}b{aSK7Pbt|WT+=%G~Z<%e-QhpWFbL7dmuJWn* zhubji^H$VVG(_9ZCD`vjyCdhS%8F^Xml47Q$$oG_qT};wyQ#Zm6*9$4utvtXr5)4% zBC+v>_%%`=pNzjxHpZvoWHJ!1XMBEgI$pO*I#m|{lYsMpg9S+Uf2ZIr=(|3A=YuI5 zr+gdwNQP>-QF#rb&&@k-ay(lnt3^7N_P&QEBtM=4Vo?7mvSR`1?!jTY4JK--jFAEL zx=4vbaC{Fwk5StwL$?sDiQcinZ?eg%u=(_TU8BkR&#P??G;s$sj{q3T{T{iH72VhI z$ktXZ^x{I zRKw7Bz0wZgS{d|*$7jHH0bnacx*5`-O;@}+jwU4O;<8DvR!fd+*WgP;)N1fKqDg#( z?M!MT!-+9?6PZk`kE;;RZbJ0i1<9-E<|mMWu`*joTGB4Hw#(RleJ;Xyc@EC3@q~xV zHQ8E)Ya7WQlJfD#EK~@^0w-1GYD8zxi0(t= z$%l@~rAiV`6v@x%`MFS|UO-0U^Q%1>WjblTetnaY z!kRpz=zibohXm36C*stvEF6CyP3XWMH!8YMEQszos9Mr0PP{MONtLZ~VbF%?jizDO zjde;PdFtkf=-x+K3`N?$1&;&k7M$pQPm^Tbu^{cja-ZnlM(l=mTtrfh`m*eWS3DPT zuLY}T#zz=^v`%yS@kb;$#fY|*hVO3wbg@lzmx;wT)|M}5$wu z{^3 z&(KgMMHQ`9UdV)${~&-iDJMKV+Z)y`rSbjVGcHESPT|fG3s1+$K*3tbzq7!Me@3*0E7a-p{L<@lT)uO9 zZ0G#;5Skox?(Q!3ZUSj5OvyP^^O6Pjf_b4rb~SiE%33>xO~&iK?TugWW*N#dEPalR zuAJ{EfjdNWFDK^;&C%IY?9dsjY-rvpB~WoB;NC$(mgz(HdidePw!NMTE8&+V=B#qg zZ@+k#k2-bhYBEn}dZbzH5aE@c;C-g<#;^B9q0EA*E5Q{rtcm~UCGz4jt@#Td%251S zCzQZsmwfZvB^zv(4%!eizmwHzoX7gh`m(y&(yK zgy54}t|QMCsWs`ovKPm8_F35J!?w_)I$^L{Cxjjq-7Cq7qNLyhUv=A%>33whXo>mY zXZtO`o`nNe76OnDVY8fk11BQ{ZTo674MkgkDo`8H2($^P68*%USNu9Ik!m_ReO|^d zgPReswisUF#h91sycPJffDMwY3h^@_>jAz4_!|V5ejK?KYT}{?6|-nDWBu%8fv;SFXeTWJOgO?zeYUt;eyu zieX!kjDri8?}UC2;5p)2nGR|ip;^8M_?Jk#P#bo|&kVOYzT+o_YxsZ2q3UR$qt(gq z{d`hw1nR9eX||6ydt_Q3aND_c4dMz);IS;hzM{s=;)A5UJ{{=3Mf$-aom-@lkE{oI z$0M6p9E3@^QMM-F_ATCD0r!kU%HDS}`apKcA?5x%*;fx_XB{&{-|!Cjx!6z%SEyoD zHO47qZR1IK3dE2>LYk6sHpy)21A3wvrV6d{Qu)&e6`8aWklSp z0dWVk@Lv@$orJhU(2fG$1{?*908RkBiHIAA_V4gt^dhvEfk&^A=`{&!8y-en37`Ye z0@wt29Iz6Q2j~TCYH58M{%4}rr+&IYYTt+C@{nXHEg1MGd2k0m*|4}n;t_dujgh(6 KMBZETlYaq5I>B)O diff --git a/Tools/bootloaders/MatekL431-Airspeed_bl.hex b/Tools/bootloaders/MatekL431-Airspeed_bl.hex index 2c48620e5ae..d5220448124 100644 --- a/Tools/bootloaders/MatekL431-Airspeed_bl.hex +++ b/Tools/bootloaders/MatekL431-Airspeed_bl.hex @@ -1,19 +1,19 @@ :020000040800F2 -:1000000000090020B5040008D5290008552900087A -:10001000AD2900085529000881290008B704000807 -:10002000B7040008B7040008B704000889370008BF +:1000000000090020B5040008E5290008652900085A +:10001000BD2900086529000891290008B7040008D7 +:10002000B7040008B7040008B7040008393800080E :10003000B7040008B7040008B7040008B7040008B4 :10004000B7040008B7040008B7040008B7040008A4 -:10005000B7040008B70400089D4A0008C54A000814 -:10006000ED4A0008154B00083D4B0008B704000896 +:10005000B7040008B70400084D4B0008754B0008B2 +:100060009D4B0008C54B0008ED4B0008B704000885 :10007000B7040008B7040008B7040008B704000874 -:10008000B7040008B7040008B70400089125000869 -:10009000BD250008CD250008B7040008654B000801 +:10008000B7040008B7040008B7040008A125000859 +:10009000CD250008DD250008B7040008154C000830 :1000A000B7040008B7040008B7040008B704000844 -:1000B000754C0008B7040008B7040008B70400082E +:1000B000254D0008B7040008B7040008B70400087D :1000C000B7040008B7040008B7040008B704000824 -:1000D000B7040008394C00084D4C0008614C00087A -:1000E000C94B0008B7040008B7040008B7040008AB +:1000D000B7040008E94C0008FD4C0008114D000869 +:1000E000794C0008B7040008B7040008B7040008FA :1000F000B7040008B7040008B7040008B7040008F4 :10010000B7040008B7040008B7040008B7040008E3 :10011000B7040008B7040008B7040008B7040008D3 @@ -81,27 +81,27 @@ :1004F00040F20000C0F2F0004EF68851CEF200014A :100500000860BFF34F8FBFF36F8F4FF00000E1EE35 :10051000100A4EF63C71CEF200010860062080F30E -:100520001488BFF36F8F04F033F904F00FF904F06F -:1005300049F94FF055301F491B4A91423CBF41F8E1 +:100520001488BFF36F8F04F08BF904F067F904F0BF +:10053000A1F94FF055301F491B4A91423CBF41F889 :10054000040BFAE71C49194A91423CBF41F8040BDD :10055000FAE71A491A4A1B4B9A423EBF51F8040B5C :1005600042F8040BF8E700201749184A91423CBFB3 -:1005700041F8040BFAE704F0EDF804F07BF9144CB1 +:1005700041F8040BFAE704F045F904F0D3F9144C00 :10058000144DAC4203DA54F8041B8847F9E700F035 :1005900041F8114C114DAC4203DA54F8041B884762 -:1005A000F9E704F0D5B80000000900200011002090 -:1005B000000000080001002000090020E8510008A8 -:1005C000001100208C1100209011002034390020EF +:1005A000F9E704F02DB90000000900200011002037 +:1005B00000000008000100200009002098520008F7 +:1005C000001100208C110020901100203C390020E7 :1005D000A0010008A4010008A4010008A40100086B :1005E0002DE9F04F2DED108AC1F80CD0C3689D465F :1005F000BDEC108ABDE8F08F002383F311882846F4 -:10060000A047002003F0ACFDFEE703F025FD00DF6E -:10061000FEE70000F8B504F03BF8074604F08CF85C +:10060000A047002003F004FEFEE703F07DFD00DFBD +:10061000FEE70000F8B504F093F8074604F0E4F8AC :100620000546A0BB1F4B9F4231D001339F4231D0C2 :100630001D4B27F0FF029A422FD1F8B200F030FD97 :100640002E4642F2107400F035FF08B10024264611 :1006500000F02CFD08B90646044635B1134B9F4205 -:1006600003D004F061F800242646002004F01AF8B4 +:1006600003D004F0B9F800242646002004F072F804 :100670000EB100F063F801F051FA00F03FFF01F015 :100680003DF9204600F0A8F800F058F8F9E72E46AA :100690000024D8E704460126D5E706464FF47A74CD @@ -109,15 +109,15 @@ :1006B00008B501F0EFF8A0F120035842584108BDF9 :1006C00007B541F21203022101A8ADF8043001F090 :1006D000FFF803B05DF804FB10B5202383F3118805 -:1006E0001248C3680BB103F0B3FD114A0F48002351 -:1006F0004FF47A7103F070FD002383F311880D4CE1 +:1006E0001248C3680BB103F00BFE114A0F480023F8 +:1006F0004FF47A7103F0C8FD002383F311880D4C89 :10070000236813B12368013B2360636813B16368F6 :10071000013B6360084B1B7833B9636823B902203F :1007200001F08AF93223636010BD00BF90110020F0 :10073000D9060008AC120020A4110020214B224A47 :1007400010B51C46196801313BD004339342F9D1EE :100750006268A24235D31D4B9B6803F1006303F529 -:1007600020439A422DD203F0B1FF03F0C3FF0020D3 +:1007600020439A422DD204F009F804F01BF800202F :1007700001F0DEF8164B0220187001F055F9154B08 :100780009A6D00229A65996F9A67996FD96DDA65AB :10079000D96FDA67D96F196E1A66D3F88010C3F86B @@ -127,8 +127,8 @@ :1007D000A4110020001002402DE9F04F93B0AC4B63 :1007E00000902022FF210AA89D6801F041F9A94A42 :1007F0001378A3B9A8480121C3601170202383F3A3 -:100800001188C3680BB103F023FDA44AA24800235A -:100810004FF47A7103F0E0FC002383F31188009B0E +:100800001188C3680BB103F07BFDA44AA248002302 +:100810004FF47A7103F038FD002383F31188009BB5 :100820009F4A03B113609F49009C00230B705360E3 :1008300098469B461E469A46012001F0F5F824B1E1 :10084000974B1B68002B00F01C82002001F022F85F @@ -180,7 +180,7 @@ :100B200000283FF4DDAE0027B946704B9B68BB42FE :100B300018D91F2F11D80A9B01330ED027F00303B9 :100B400012AA134453F8203C05934846042205A9F1 -:100B500001F04EFF04378146E7E7384600F0EAFE31 +:100B500001F056FF04378146E7E7384600F0EAFE29 :100B60000590F2E7CDF81490042105A800F0B0FE3E :100B700000E70023642104A8049300F09FFE0028EE :100B80007FF4AEAE0220FFF793FD00283FF4A8AE3D @@ -199,11 +199,11 @@ :100C500047AE00F0A9FE002844D00A9B01330BD018 :100C600008220AA9002000F00BFF00283AD0202219 :100C7000FF210AA800F0FCFEFFF722FD1C4803F04C -:100C800075FA13B0BDE8F08F002E3FF429AE0BF0DB +:100C8000CDFA13B0BDE8F08F002E3FF429AE0BF083 :100C90000B030B2B7FF424AE0023642105A80593DE :100CA00000F00CFE074600287FF41AAE0220FFF782 :100CB000FFFC814600283FF413AEFFF701FD41F22F -:100CC000883003F053FA059800F042FF4E4600F0DA +:100CC000883003F0ABFA059800F042FF4E4600F082 :100CD0001BFF3C46B0E506464CE64FF0000AFFE538 :100CE000B8467BE6374679E6A811002000110020BF :100CF000A0860100094A136849F2690099B21B0CE9 @@ -211,17 +211,17 @@ :100D1000000C01FB0200186080B270471411002023 :100D20001011002010B500211022044600F0A0FE92 :100D3000034B03CB206061601868A06010BD00BF4A -:100D40009075FF1F2DE9F043224DBBB001F04AFE24 +:100D40009075FF1F2DE9F043224DBBB001F052FE1C :100D5000AB6840F2ED22C31A934232D906AFA860C5 -:100D60002B4628220021384602F0D6FA05F10E0063 +:100D60002B4628220021384602F022FB05F10E0016 :100D700000F076FE002604465FFA80F905F10E08C1 :100D8000F3B2F100994501F1280107D908EB0603F8 -:100D90000822384602F0C0FA0136F1E708230122A2 +:100D90000822384602F00CFB0136F1E70823012255 :100DA000CDE9023205340C4B0193A4B230230093F9 :100DB000CDE9047405A3D3E90023297B074802F099 -:100DC000C3F83BB0BDE8F083AFF3008078F6339F03 -:100DD00093CACD8DC0330020CD330020CC2200201B -:100DE00070B50D4614461E4602F044F850B9022E66 +:100DC0000FF93BB0BDE8F083AFF3008078F6339FB6 +:100DD00093CACD8DC8330020D5330020CC2200200B +:100DE00070B50D4614461E4602F090F850B9022E1A :100DF00010D1012C0ED112A3D3E90023C5E90023A1 :100E0000012007E0282C10D005D8012C09D0052C92 :100E10000FD0002070BD302CFBD10BA3D3E90023F1 @@ -230,93 +230,93 @@ :100E4000401DA12026812A0B78F6339F93CACD8DB1 :100E50009E6AC421818A46EE26417272DF25D7B789 :100E6000F017304A39059E5613B504462346084606 -:100E700020220021019002F04FFA22790198032AE2 +:100E700020220021019002F09BFA22790198032A96 :100E8000234628BF032203F8042F2021022202F068 -:100E900043FA62790198072A234628BF072203F8FC -:100EA000052F2221032202F037FAA2790198072A9E +:100E90008FFA62790198072A234628BF072203F8B0 +:100EA000052F2221032202F083FAA2790198072A52 :100EB000234628BF072203F8062F2521032202F02C -:100EC0002BFA019804F108031022282102F024FAD9 +:100EC00077FA019804F108031022282102F070FA41 :100ED000382002B010BD00002DE9F04FADF5037DC4 :100EE00023AD10AE40F2751280460F4624A80021B3 :100EF000296000F0BDFD48220021304600F0B8FD19 -:100F000001F070FD564B4FF47A72B0FBF2F01860AE +:100F000001F078FD564B4FF47A72B0FBF2F01860A6 :100F100093E80700012386E807000DF162003382A1 :100F2000FFF700FF42F20463338407AB18464D49D4 -:100F300004F068F84FF0200930642946304686F8FE +:100F300004F0C0F84FF0200930642946304686F8A6 :100F40003C90FFF791FF14AB044601460822284667 -:100F500002F0E2F90822A1180DF15103284602F02F -:100F6000DBF90DF15203082204F11001284602F0CA -:100F7000D3F94A4615AB04F11801284602F0CCF922 -:100F800016AB402204F13801284602F0C5F918AB2F -:100F9000082204F17801284602F0BEF90DF1610340 -:100FA000082204F18001284602F0B6F904F1880A0B +:100F500002F02EFA0822A1180DF15103284602F0E2 +:100F600027FA0DF15203082204F11001284602F07D +:100F70001FFA4A4615AB04F11801284602F018FA88 +:100F800016AB402204F13801284602F011FA18ABE2 +:100F9000082204F17801284602F00AFA0DF16103F3 +:100FA000082204F18001284602F002FA04F1880ABE :100FB0000DF1620904F5847B4B4651460822284610 -:100FC0000AF1080A02F0A8F9D34509F10109F3D1A1 -:100FD0001DAB08225946284602F09EF904F5887494 +:100FC0000AF1080A02F0F4F9D34509F10109F3D155 +:100FD0001DAB08225946284602F0EAF904F5887448 :100FE0004FF0000996F834304B450AD9B36B2146CF -:100FF0004B440822284602F08FF9083409F1010910 +:100FF0004B440822284602F0DBF9083409F10109C4 :10100000F0E74FF0000996F83C304B4504EBC9017E -:1010100008D9336C08224B44284602F07DF909F1C7 +:1010100008D9336C08224B44284602F0C9F909F17B :101020000109F0E7CB1DC3F3CF03CDE9045300233F :101030000393BB7E029307F11903019301230093ED -:10104000F97E05A3D3E90023404601F07DFF0DF5AD +:10104000F97E05A3D3E90023404601F0C9FF0DF561 :10105000037DBDE8F08F00BF9E6AC421818A46EE01 -:10106000B41200203450000810B50A4B0A4A1A6026 +:10106000B4120020E450000810B50A4B0A4A1A6076 :1010700003F5805393F860203AB9DC6D2CB120461B -:1010800001F02EFB204603F04FFEBDE81040034860 -:1010900001F026BBF0220020345100083833002034 +:1010800001F036FB204603F0A7FEBDE81040034800 +:1010900001F02EBBF8220020E4510008403300206C :1010A000014B1870704700BFC0120020F0B5334BE1 :1010B0001C7B85B034B1324B0E221A8100242046AD :1010C00005B0F0BD2F4A1068516802AB03C3082376 -:1010D0002D492E480DEB030203F078FE054630B98A -:1010E000274B2B480A221A8101F078FAE6E70169BA +:1010D0002D492E480DEB030203F0D0FE054630B932 +:1010E000274B2B480A221A8101F080FAE6E70169B2 :1010F000B1F5583F06D9224B26480B221A8101F040 -:101100006DFADCE7438B40F22642934207D01C493C -:101110000C2008811946204801F060FACFE71F4AE9 +:1011000075FADCE7438B40F22642934207D01C4934 +:101110000C2008811946204801F068FACFE71F4AE1 :10112000024402F11003994204D2154B1C481022CC -:101130001A81E4E710398E1A2046144901F058FC50 -:101140003246074605F11801204601F051FCAB6814 +:101130001A81E4E710398E1A2046144901F060FC48 +:101140003246074605F11801204601F059FCAB680C :101150009F4202D1EB6898420AD0094B0D221A81B6 -:101160000090D5E902123B460E4801F037FAA5E798 -:101170000D4801F033FA0124A1E700BFC03300207D -:10118000B4120020E9500008DC5F030000A0000852 -:101190005850000864500008765000080860FFF7B7 -:1011A00094500008B1500008DA5000082DE9F04FC3 -:1011B000ADB006AF80460C4601F05CFE0546002847 +:101160000090D5E902123B460E4801F03FFAA5E790 +:101170000D4801F03BFA0124A1E700BFC83300206D +:10118000B412002099510008DC5F030000A00008A1 +:101190000851000814510008265100080860FFF7A4 +:1011A00044510008615100088A5100082DE9F04FB0 +:1011B000ADB006AF80460C4601F0A8FE05460028FB :1011C0005AD1237E022B1BD1E38A012B18D101F0C7 -:1011D00009FC0646FFF78EFD03464FF4C870DFF8A2 +:1011D00011FC0646FFF78EFD03464FF4C870DFF89A :1011E000D092B3FBF0F206F5167602FB103316FA36 :1011F00083F3C9F80030E37E33B9A84B00221A709C :101200009C37BD46BDE8F08FA38AEEB2013BB342E6 :1012100005F101050BD93B1D1E44E9000096002392 -:10122000082201F0F801204601F03AFFECE707F14F +:10122000082201F0F801204601F086FFECE707F103 :101230001400FFF777FD324607F11401381D03F063 -:10124000B5FD0028D9D10F2E08D8944B1E70D9F8BF +:101240000DFE0028D9D10F2E08D8944B1E70D9F866 :101250000030A3F51673C9F80030D1E7FB1CF87015 -:101260000146009307220346204601F019FFF97852 -:10127000404601F0F7FDC3E7E38A282B26D010D8BB +:101260000146009307220346204601F065FFF97806 +:10127000404601F043FEC3E7E38A282B26D010D86E :10128000012B1ED0052BBBD1BFF34F8F8449854B5B :10129000CA6802F4E0621343CB60BFF34F8F00BF14 :1012A000FDE7302BACD1637E7F4D01336A7BDBB22F :1012B0009342E94603D1E27E2B7B9A4265D0CD462C :1012C0009EE721464046FFF707FE99E7A38A013BC8 :1012D0009BB2C92B94D8744D2E7B26BB05F10C0311 -:1012E0000093082233463146204601F0D9FE731C94 +:1012E0000093082233463146204601F025FF731C47 :1012F000F2B2D9001E46A38A013B9A4205DA0E32A9 :101300002A44009200230822EEE700230022C5E9C8 :1013100000230023AB6085F8D730C5F8D8302B7B8D :101320000BB9E37E2B73002507F114093B1D08223E -:1013300029464846C7E90155FD6001F0EDFF3B7ABB +:1013300029464846C7E90155FD6002F039F83B7A75 :1013400005F1010AAB424FEACA0608D9FB68082238 -:101350002B443146484601F0DFFF5546EFE7C6F320 +:101350002B443146484602F02BF85546EFE7C6F3DA :10136000CF06E17ECDE9049600230393A37E02938A :10137000193428230093019446A3D3E9002340465F -:1013800001F0E2FDFFF7DEFC3AE74FF0000807F15D +:1013800001F02EFEFFF7DEFC3AE74FF0000807F110 :101390001403A7F81480102200934146012320462D -:1013A00001F07EFEA68A023EB6B2F31C9B109B00A3 +:1013A00001F0CAFEA68A023EB6B2F31C9B109B0057 :1013B0000733DB08A9EBC3039D460DF1180A1FFA9A :1013C00088F34FEAC801B34201F110010AD20AEBD7 -:1013D0000803009308220023204601F061FE08F173 +:1013D0000803009308220023204601F0ADFE08F127 :1013E0000108ECE795F8D70000F0B2FAD5F8D8304C :1013F00004461BB995F8D70000F0BAFAD5F8D830F2 :1014000033449C4204D295F8D700013000F0B0FA82 @@ -329,34 +329,34 @@ :1014700008D800232B7300F097FAFFF717FE08B186 :10148000FFF75CF92B68094A9B0A0133138100239B :10149000AB6014E726417272DF25D7B7C522002062 -:1014A00000ED00E00400FA05C0330020B412002073 +:1014A00000ED00E00400FA05C8330020B41200206B :1014B000C822002010B54FF000540C4B22689A420D :1014C00011D10B4B627D1A700A48237D03730A49C0 :1014D000C9220E3000F0BAFAE0220021204600F0C6 :1014E000C7FA012010BD0020FCE700BF9AAD44C53B -:1014F000C0120020C03300201600002037B5184D60 -:1015000018491948022301226B7100F035FF0023AE +:1014F000C0120020C83300201600002037B5184D58 +:1015000018491948022301226B7100F03DFF0023A6 :101510000193164B164900931648174B4FF480520F -:1015200001F07AFC154B197811B1124801F09AFCC0 -:1015300001F058FA0446FFF7DDFB4FF4C873B0FB27 +:1015200001F0C6FC154B197811B1124801F0E6FC28 +:1015300001F060FA0446FFF7DDFB4FF4C873B0FB1F :10154000F3F202FB130304F5167010FA83F00C4B50 -:10155000186003F0F1F808B10F232B8103B030BD00 -:10156000B412002040420F00F0220020E10D0008DC +:10155000186003F049F908B10F232B8103B030BDA7 +:10156000B412002040420F00F8220020E10D0008D4 :10157000C4120020CC220020AD110008C0120020AF :10158000C82200202DE9F04F2DED028B0FF26429C7 :10159000D9E900898D4C93B08D4E8E4F204601F0D5 -:1015A00037FD034660B30025CDE90F550E958DF844 +:1015A00083FD034660B30025CDE90F550E95ADF8D8 :1015B0004450027B8DF8442099684068DFF83CA2D3 -:1015C0000FAA03C21B6843F000430E9301F00CFA0C -:1015D000821941F1000300950EA9384600F0FAFA8D -:1015E000A84205DD204601F017FD8AF80050D5E736 +:1015C0000FAA03C21B6843F000430E9301F014FA04 +:1015D000821941F1000300950EA9384600F002FB84 +:1015E000A84205DD204601F063FD8AF80050D5E7EA :1015F0009AF80030072B00F2B68001338AF80030E9 :101600000BAE9FED6E8B0023724FDFF8F4A10A93AF -:101610008DF834300B9373604FF0000B5B468DED0B +:10161000ADF834300B9373604FF0000B5B468DEDEB :10162000008B01250DF11D0207A938468DF81C50CD -:101630008DF81DB000F050FE9DF81C30002B40F0DE -:101640009680204601F016FC0646002845D1624FE0 -:1016500001F0C8F93B6898423FD301F0C3F98246D4 +:101630008DF81DB000F058FE9DF81C30002B40F0D6 +:101640009680204601F062FC0646002845D1624F94 +:1016500001F0D0F93B6898423FD301F0CBF98246C4 :10166000FFF748FB4FF4C8730AF5167AB0FBF3F2A4 :1016700002FB13031AFA83F33B60584F97F800B04C :10168000CBF1100ABBF1000F14BF33462B465FFAB3 @@ -364,54 +364,54 @@ :1016A00028BF4FF0060A0EAB03EB0B0152460DF1BB :1016B000290000F0CBF90AAB0393182302930AF137 :1016C0000102474BD2B2CDE90053049220463DA31C -:1016D000D3E9002301F0D0FB3E7001F083F9414AC9 +:1016D000D3E9002301F01CFC3E7001F08BF9414A74 :1016E000414D1368C31AB3F57A7F2FD3106001F010 -:1016F0007BF902460B46204601F098FC204601F09B -:10170000B9FB18B32B7B394E002B14BF03230223E4 -:10171000737101F067F90EAF4FF47A733946B0FB7D +:1016F00083F902460B46204601F0E4FC204601F047 +:1017000005FC18B32B7B394E002B14BF0323022397 +:10171000737101F06FF90EAF4FF47A733946B0FB75 :10172000F3F030603046FFF79FFB18230730029339 :101730002F4B0193C0F3CF0040F25513CDE9037056 -:10174000009342464B46204601F096FB2B7B2BB183 +:10174000009342464B46204601F0E2FB2B7B2BB137 :10175000FFF7F8FA2B7B002B7FF41EAF13B0BDEC24 -:10176000028BBDE8F08F204601F056FC48E7DAF81E +:10176000028BBDE8F08F204601F0A2FC48E7DAF8D2 :10177000143083F00803CAF81430594610220EA81A :1017800000F076F90DF11E0308AA0AA9384600F008 -:101790001FFB96E803000FAB83E803009DF834308D +:1017900027FB96E803000FAB83E803009DF8343085 :1017A0008DF844300A9B0E930EA9DDE908232046EC -:1017B00001F0FAFD30E700BFAFF300800000000049 +:1017B00001F046FE30E700BFAFF3008000000000FC :1017C00000000000401DA12026812A0BCC22002011 -:1017D00040420F00F0220020C8220020C522002035 -:1017E000C4220020A0340020C0330020B412002006 -:1017F000A4340020F1C6A7C1D068080FA53400208A -:101800000004004808B5054800F07CFBBDE808402E -:10181000034A0449002003F081BA00BFF0220020EF -:10182000F8340020691000082DE9F84F4FF47A735E +:1017D00040420F00F8220020C8220020C52200202D +:1017E000C4220020A8340020C8330020B4120020F6 +:1017F000AC340020F1C6A7C1D068080FAD3400207A +:101800000004004808B5054800F084FBBDE8084026 +:10181000034A0449002003F0D9BA00BFF82200208F +:1018200000350020691000082DE9F84F4FF47A7355 :10183000DFF85880DFF8589006460D4602FB03F7A4 :10184000002498F900305A1C5FFA84FA01D0A342B0 :1018500010D159F8240003682A46D3F820B0314645 :101860003B46D847854205D1074B012083F800A0AD :10187000BDE8F88F0134032CE3D14FF4FA7002F085 -:1018800075FC0020F4E700BFEC34002018110020A4 -:101890000051000807B50023024601210DF10700A1 +:10188000CDFC0020F4E700BFF43400201811002044 +:10189000B051000807B50023024601210DF10700F1 :1018A0008DF80730FFF7C0FF20B19DF8070003B0A7 :1018B0005DF804FB4FF0FF30F9E700000A4608B579 :1018C0000421FFF7B1FF80F00100C0B2404208BD23 :1018D00030B4074B0A461978064B53F82140236869 :1018E000DD69054B0146AC46204630BC604700BF71 -:1018F000EC34002000510008A086010070B502F011 -:1019000073FD094E094D30800024286833888342D6 -:1019100008D902F065FD2B6804440133B4F5204F6B -:101920002B60F2D370BD00BFEE340020A83400203D -:1019300002F008BE00F1006000F52040006870472A -:1019400000F10060920000F5204002F089BD000027 +:1018F000F4340020B0510008A086010070B502F059 +:10190000CBFD094E094D308000242868338883427E +:1019100008D902F0BDFD2B6804440133B4F5204F13 +:101920002B60F2D370BD00BFF6340020B03400202D +:1019300002F060BE00F1006000F5204000687047D2 +:1019400000F10060920000F5204002F0E1BD0000CF :10195000054B1A68054B1B889B1A834202D9104419 -:1019600002F03EBD00207047A8340020EE34002075 -:1019700038B5074D04462868204402F037FD28B9E1 -:1019800028682044BDE8384002F042BD38BD00BFA1 -:10199000A83400200020704700F10050A0F510404E +:1019600002F096BD00207047B0340020F63400200D +:1019700038B5074D04462868204402F08FFD28B989 +:1019800028682044BDE8384002F09ABD38BD00BF49 +:10199000B03400200020704700F10050A0F5104046 :1019A000D0F8900570470000064991F8243033B113 :1019B0000023086A81F824300822FFF7C1BF012004 -:1019C000704700BFAC340020014B1868704700BF5F +:1019C000704700BFB4340020014B1868704700BF57 :1019D000002004E070B50E4B5C6893F90860421E6D :1019E0000A44013C0B46934207D214F9015F581C8C :1019F0002DB100F8015C0346F5E7184605E02C24FC @@ -431,892 +431,903 @@ :101AD000082E17449044E4B285F82460DBD1FFF768 :101AE00063FF0028D7D108E02B6A03EB820383420F :101AF000CFD0FFF759FF0028CBD10020BDE8F883F5 -:101B00000120FBE7AC340020024B1A78024B1A701C -:101B1000704700BFEC3400201811002008B50849B8 -:101B200008484FF461430B6002F0BAF904490648D3 -:101B300002F0B6F9BDE808400149044802F0B0B926 -:101B4000D4340020FC34002068350020D435002037 +:101B00000120FBE7B4340020024B1A78024B1A7014 +:101B1000704700BFF43400201811002008B50849B0 +:101B200008484FF461430B6002F012FA044906487A +:101B300002F00EFABDE808400149044802F008BA74 +:101B4000DC3400200435002070350020DC35002016 :101B5000094B10B51822044600211846FFF788FFEC :101B6000064A074B127804600146BDE8104053F85E -:101B7000220002F095B900BFD4340020EC340020DC -:101B800000510008202383F3118862B670470000DB -:101B9000002383F3118862B670470000012070476C -:101BA0000020704700207047704700007047000019 -:101BB000002070470E20704700F5805090F8C80054 -:101BC000C0F340007047000000F5805090F9C90054 -:101BD000704700002DE9F0410C68BDF8187014F052 -:101BE00000541E466FD10B7B082B6CD8FFF7CAFF41 -:101BF0004569AB685B010CD4AB681B0108D4AC68C9 -:101C000014F080545DD1FFF7C3FF2046BDE8F0819A -:101C100001240B6804F1180C002BB8BFDB004FEA5D -:101C20000C1CB4BF43F004035B0545F80C300B6893 -:101C30000FFA84FC13F0804F18BF05EB0C1E05EB68 -:101C40000C1C1EBFDEF8803143F00203CEF8803159 -:101C50000B7BCCF8843105EB04158B68C5F88C310F -:101C60004B68C5F88831DCF8803143F00103CCF8CB -:101C7000803100EB441541F268031D4403EB44132B -:101C80000344C5E9002608330D4601F10C0C55F854 -:101C900004EB43F804EB6545F9D184342D781D70CD -:101CA00000EB441407F00303257925F00B052B43C3 -:101CB0002371FFF76DFF06973346BDE8F04100F052 -:101CC000AFBC0224A5E74FF0FF309FE713B500F546 -:101CD00080540191E06D00F039FD1F280AD9019967 -:101CE000E06D202200F0A8FDA0F1200358425841E9 -:101CF00002B010BD0020FBE708B500F58050FFF7EB -:101D000041FFC06D00F0F6FCBDE80840FFF740BFA2 -:101D100000220260027342608260704710B50022A8 -:101D20000023C0E900230023044603810C30FFF7A1 -:101D3000EFFF204610BD0000F0B5054600F58050CD -:101D40000C4690F8C83013F0040FC3F3800108BFAD -:101D5000114661F3820304F1840680F8C83005EB74 -:101D6000461389B01B79D8072ED57AB319072DD41D -:101D70006846FFF7D3FF05EB441303F5835303F1E4 -:101D8000180703AA103318685968144603C40833A7 -:101D9000BB422246F7D1186820601B792371DDE928 -:101DA0000E23CDE900230123ADF808302B686946E6 -:101DB000DB6B2846984705EB46152B791A075CBF65 -:101DC00043F008032B7101E0002AF4D109B0F0BD03 -:101DD0002DE9F047074688B007F5805468469A46D3 -:101DE0008846FFF7CFFE9146FFF798FFE06D00F0C1 -:101DF00093FC1F2829D9E06D2022694600F09EFD42 -:101E0000202822D103AD444605AB2E4603CE9E4288 -:101E100020606160354604F10804F6D13068206026 -:101E200033792371DDE90023C9E90023BDF80830C7 -:101E3000AAF80030FFF7ACFE4A4653464146384602 -:101E400008B0BDE8F04700F0D9BBFFF7A1FE0020C5 -:101E500008B0BDE8F08700002DE9F84F0023C0E985 -:101E60000133254B044640F8183B0F46FFF750FF5F -:101E700004F12800FFF752FF04F1480804F58255E9 -:101E80004646083530462036FFF748FFAE42F9D1C6 -:101E900004F580554FF480534FF00009C5E913391C -:101EA000C5F848800123EE6504F5875804F584568B -:101EB000C5F8549085F8583085F86030083608F138 -:101EC00008084FF0000A4FF0000B46E908ABA6F1F6 -:101ED0001800FFF71DFF203646F8289C4645F4D130 -:101EE00085F8C97017B1054800F076FD044B6361B1 -:101EF0002046BDE8F88F00BF345100080C5100089F -:101F00000064004010B5044B197804464A1C1A704E -:101F1000FFF7A2FF204610BDF43400202DE9F04762 -:101F2000002950D0294B2A4FB7FBF1F599428CBFBD -:101F30000A231123581EB5FBF3FC03FB1C53C4B248 -:101F40002BB102280346F5D80020BDE8F0870CF13C -:101F5000FF36B6F5806FF7D2C4EBC40E0EF1030363 -:101F60004FEAE309C3F3C703A4EB030809F1010A2D -:101F70004FF47A755FFA88F009FB05555AFA88F82C -:101F8000B5FBF8F5B5F5617FC1BF0EF1FF33C3F3C3 -:101F9000C703E01AC0B25C1C50FA84F40CFB04F4D2 -:101FA000B7FBF4F4A142CFD1013BDBB20F2BCBD86E -:101FB0000138C0B20728C7D800211071168091706F -:101FC000D3700120C1E70846BFE700BF3F420F00C2 -:101FD00000B4C40470B505460E464FF47A746B69BC -:101FE0005B6803F00103B34207D04FF47A7002F04C -:101FF000BDF8013CF3D1204670BD0120FCE7000094 -:1020000030B54269936913F0700F16D000230B4C62 -:10201000936103F1840200EB421211794D0709D557 -:10202000890707D5416954F823508D60117941F033 -:10203000040111710133032BEBD130BD2051000895 -:1020400073B51D46436916469A68D207044609D5FA -:102050009A6801219960C2F34002CDE90065002130 -:10206000FFF76AFE63699A68D1050BD59A684FF449 -:1020700080719960C2F34022CDE9006501212046BC -:10208000FFF75AFE63699A68D2030BD59A684FF43A -:1020900080319960C2F34042CDE9006502212046BB -:1020A000FFF74AFE204602B0BDE87040FFF7A8BF28 -:1020B000F8B50446466900296CD106F10C0738686A -:1020C00080076AD006EB01153868D5F8B00110F02A -:1020D000040FD5F8B0011ABFC00840F00040400D11 -:1020E000A061D5F8B0C11CF0020F1CBF40F08040C9 -:1020F000A061D5F8B40106EB011100F00F0084F8DF -:102100002400D1F8B8012077D1F8B801000A60772F -:10211000D1F8B801000CA077D1F8B801000EE07733 -:10212000D1F8BC0184F82000D1F8BC01000A84F881 -:102130002100D1F8BC01000C84F82200D1F8BC11B8 -:10214000090E84F823103821396004F1340004F1B9 -:10215000180104F1240551F8046B40F8046BA942FE -:10216000F9D109780170C4E90A23214600232386A6 -:1021700051F8283B2046DB6B984704F580522046F7 -:1021800092F8C83043F0040382F8C830BDE8F84044 -:10219000FFF736BF06F1100791E7F8BD10B504460A -:1021A00000F022FC02460B4652EA030102D0013A3B -:1021B00063F100030449086820B12146BDE81040DE -:1021C000FFF776BF10BD00BFF0340020F8B500F572 -:1021D00083511E46FFF7D6FCDFF844C008310024C7 -:1021E00004F1840500EB45152B795F070ED4DB065F -:1021F0000CD5D1E900739742B34107D243695CF82B -:1022000024709F602B7943F004032B710134032C5D -:1022100001F12001E4D1BDE8F840FFF7B9BC00BFEF -:102220002051000808B5FFF7ADFCFFF7E9FEBDE857 -:102230000840FFF7ADBC0000F8B543690546986853 -:1022400000F0E050B0F1E05F0F461FD0E8B1FFF7BB -:1022500099FC05F583541034002606F1840305EB40 -:1022600043131B791A0706D50136032E04F1200407 -:10227000F3D1012007E05B07F6D42146384600F091 -:1022800009FA0028F0D1FFF783FCF8BD0120FCE734 -:1022900000F5805008B5FFF775FCC06D00F03CFA02 -:1022A000FFF776FC43090CBF0120002008BD0000A9 -:1022B000F8B51D46002313700F4606461446FFF777 -:1022C000E7FF80F00100387025B129463046FFF75E -:1022D000B3FF2070F8BD00002DE9B8410C4615464B -:1022E0001F46804600F080FB0B462178024609B964 -:1022F000287850B14046FFF769FFFFF793FF3B4650 -:102300002A462146FFF7D4FF0120BDE8B88100002E -:1023100010B5FFF737FC174B9A6D42F000729A65C3 -:102320009A6B42F000729A639A6B00F5805422F027 -:1023300000729A63FFF72CFC94F8C830DB0718D4BE -:10234000B9B102211320FFF71DFC01F0F7FB0221B8 -:10235000142001F0F3FB0221152001F0EFFB94F8AB -:10236000C83043F0010384F8C830BDE81040FFF7DF -:102370000FBC10BD001002402DE9F04700F580555C -:1023800088B095F8C930012B04468846164600F2FD -:10239000FB807E4F57F823200AB947F82300D7F86F -:1023A00000A0C4F80C802674BAF1000F09D141F2E4 -:1023B000D00002F0BBFC51468146FFF74DFDC7F847 -:1023C000009095F8C930012B5DD001212046FFF720 -:1023D0009FFFFFF7D7FB6269136823F002031360C6 -:1023E0006269136843F001031360636900275F614A -:1023F00001212046FFF7CCFBFFF7ECFD002800F0A1 -:102400008480E86D00F076F904F58359BA4609F145 -:102410000809202200216846FFF72AFB02A8FFF7DF -:1024200077FCCDF818A06A4609EB07030DF1180EEA -:102430009446BCE80300F44518605960624603F115 -:102440000803F5D1DCF80000186020379CF8042060 -:102450001A71602FDDD195F8C8306FF38203002721 -:1024600085F8C8306A4641462046ADF80070ADF8A0 -:1024700002708DF80470FFF751FD6369C0B94FF425 -:1024800000421A6011E0386803681B6B98470146E8 -:1024900000289AD13868FFF73BFF38680368324656 -:1024A0001B684146984700288FD108B0BDE8F087E7 -:1024B00061221A609DF802309DF803201B06120469 -:1024C00002F4702203F040731343BDF80020C2F3FE -:1024D000090213439DF804201205022E02F4E002C3 -:1024E0000CBF4FF000410021134362690B43D361DD -:1024F000636913225A616269136823F00103136050 -:1025000039462046FFF766FD08B96369B7E795F8D5 -:10251000C93093BB6169D1F8002242F00102C1F8D1 -:1025200000226169D1F8002222F47C5222F00E02CE -:10253000C1F800226169D1F8002242F46062C1F85A -:1025400000226269C2F814326269C2F80432626918 -:1025500041F6FF71C2F80C126269C2F8403262693A -:10256000C2F8443263690122C3F81C226269D2F8BE -:10257000003223F00103C2F8003295F8C83043F06E -:10258000020385F8C83090E700208EE7F034002081 -:1025900008B500F029FA50EA0103024602D0421EB3 -:1025A00061F10001044B186810B10B46FFF748FDBC -:1025B000BDE8084001F01CB9F034002008B5002047 -:1025C000FFF7ECFDBDE8084001F012B908B50120A5 -:1025D000FFF7E4FDBDE8084001F00AB90FB40020A0 -:1025E00004B0704713B56C4684E80600031D94E8F8 -:1025F000030083E80500012002B010BD73B58568B3 -:10260000019155B11B885B0707D4D0E900369B6B5D -:102610009847019AC1B23046A847012002B070BD68 -:10262000F0B5866889B005460C465EB1BDF8383015 -:102630005B070AD4D0E900379B6B98472246C1B2AA -:102640003846B047012009B0F0BD00220023CDE993 -:1026500000230023ADF808300A4603AB01F1080659 -:10266000106851681C4603C40832B2422346F7D1B1 -:10267000106820601279227100F0B8F90423ADF8D7 -:1026800008302B68CDE90001DB6B69462846984786 -:10269000D8E7000030B503680968DD0FB5EBD17FDE -:1026A00023F0604421F060424FEAD1700BD0002B40 -:1026B000B8BFA40C0029B8BF920C944202D034BF1A -:1026C0000120002030BD944205D1C1F38070C3F3D6 -:1026D00080738342F6D194422CBF00200120F1E7A1 -:1026E00010B5037C044613B9006802F057FB20467E -:1026F00010BD00000023BFF35B8FC360BFF35B8F8F -:10270000BFF35B8F8360BFF35B8F7047BFF35B8F5B -:102710000068BFF35B8F704770B505460C30FFF75C -:10272000F5FF05F1080604463046FFF7EFFFA0422B -:1027300006D930466D68FFF7E9FF2544281A70BDB9 -:102740003046FFF7E3FF201AF9E7000070B50546B1 -:10275000406898B105F10800FFF7D8FF05F10C06B5 -:1027600004463046FFF7D2FF8442304694BF6D687E -:102770000025FFF7CBFF013C2C44201A70BD000060 -:1027800038B50C460546FFF7C7FFA04210D305F148 -:102790000800FFF7BBFF04446868B4FBF0F100FBDE -:1027A0001144BFF35B8F0120AC60BFF35B8F38BD7A -:1027B0000020FCE72DE9F041144607460D46FFF7DF -:1027C000C5FF844228BF0446D4B1B84658F80C6B04 -:1027D0004046FFF79BFF3044286040467E68FFF785 -:1027E00095FF331A9C4203D86C600120BDE8F0814C -:1027F0006B60A41B3B68AB602044E8600220F5E7F7 -:102800002046F3E738B50C460546FFF79FFFA04288 -:1028100010D305F10C00FFF779FF04446868B4FB9E -:10282000F0F100FB1144BFF35B8F0120EC60BFF3BC -:102830005B8F38BD0020FCE72DE9FF4188466946E3 -:102840000746FFF7B7FF6C4606B204EBC606002545 -:10285000B44209D06268206808EB0501FFF7F6F87A -:10286000636808341D44F3E729463846FFF7CAFF7A -:10287000284604B0BDE8F081F8B505460C300F4697 -:10288000FFF744FF05F1080604463046FFF73EFF18 -:10289000A042304688BF6C68FFF738FF201A3860C6 -:1028A00020B130462C68FFF731FF2044F8BD00000E -:1028B00073B5144606460D46FFF72EFF844228BF27 -:1028C00004460190DCB101A93046FFF7D5FF019B1A -:1028D00033B93268C5E90233C5E9002401200CE0B0 -:1028E0009C4238BF01942860019868608442F5D901 -:1028F0003368AB60241AEC60022002B070BD204641 -:10290000FBE700002DE9FF410F466946FFF7D0FFC6 -:102910006C4600B204EBC0050026AC4209D0D4F8E6 -:10292000048054F8081BB8194246FFF78FF8464454 -:10293000F3E7304604B0BDE8F081000038B5054645 -:10294000FFF7E0FF044601462846FFF719FF20463F -:1029500038BD000000B59BB0EFF3098168226846DE -:10296000FFF774F8EFF30583044B9A6BDA6A9A6AFF -:102970009A6A9A6A9A6A9A6A9B6AFEE700ED00E090 -:1029800000B59BB0EFF3098168226846FFF75EF857 -:10299000EFF30583044B9A6B9A6A9A6A9A6A9A6A69 -:1029A0009A6A9B6AFEE700BF00ED00E000B59BB0AD -:1029B000EFF3098168226846FFF748F8EFF30583D3 -:1029C000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE7FA -:1029D00000ED00E0FEE700000FB408B5029801F03A -:1029E0005FFBFEE701F0EEBD01F0C6BD01F0C4BD26 -:1029F00030B5094D0A4491420DD011F8013B5840C1 -:102A0000082340F30004013B2C4013F0FF0384EA49 -:102A10005000F6D1EFE730BD2083B8ED2DE9F0414D -:102A2000C56915B9C161BDE8F0814B6823F0604705 -:102A3000C3F38A464FEAD37EC3F3807816EA2306AF -:102A400038BF3E46AC462B465A68BEEBD27F22F0DA -:102A500060440AD0002A18DAA40CB44217D19D426F -:102A60000FD10D60DEE71346EEE7A74207D102F073 -:102A70008044C2F3807242450BD054B1EFE708D2D4 -:102A8000EDE7CCF800100B60CDE7B44201D0B442C2 -:102A9000E5D81A689C46002AE5D11960C3E7000012 -:102AA0002DE9F047089D01F007044FEAD5082244BC -:102AB00005F0070500EBD1004FF47F49944201D1A6 -:102AC000BDE8F08704F0070705F0070A57453E46C2 -:102AD00038BF5646C6F10806111B8E4228BF0E4667 -:102AE000E10808EBD50E415C13F80EC0B94029FA95 -:102AF00006F721FA0AF1FFB28CEA010147FA0AF758 -:102B000039408CEA010C03F80EC034443544D5E753 -:102B100080EA0120082341F2210201B2400000298D -:102B200080B203F1FF33B8BF504013F0FF03F4D17C -:102B30007047000038B50C468D18A54200D138BD4D -:102B400014F8011BFFF7E4FFF7E7000002684AB141 -:102B500013680360C388018901339BB29942C38023 -:102B600038BF03811046704770B588B020220446F4 -:102B70000D4668460021FEF77BFF20460495FFF7CF -:102B8000E5FF024658B16B46054608AE1C4603CC2D -:102B9000B44228606960234605F10805F6D1104665 -:102BA00008B070BD10B54B6823B9CA8A63F3090237 -:102BB000CA8210BDC4681A681C60C360438A013BA6 -:102BC00043824A60EFE700002DE9F84F1D46CB8AAB -:102BD0000F46C3F309010629814692460B4630D0C1 -:102BE0000020AAB207F119049EB2052E1FFA80F840 -:102BF0000FD8904503F1010306D3FB8A0A4462F320 -:102C00000903FB8201201AE01AF80060E654013043 -:102C1000EAE79045F1D2A1F1060B1C237C68BBFBCF -:102C2000F3F203FB12BB1FFA8BF6002C45D148468A -:102C3000FFF78CFF044638B978606FF00200BDE8FA -:102C4000F88F4FF00008E6E7002606607860ADB226 -:102C50004FF0000B454510D90AEB0803221D13F86D -:102C6000011B9155B1B208F101081B291FFA88F820 -:102C70002BD0454506F10106F1D8FB8AC3F30902C2 -:102C8000154465F30903BCE7013292B21C46236880 -:102C9000002BF9D1AB1F0B441C21B3FBF1F3013323 -:102CA0009BB29A42D3D2BBF1000FD0D14846FFF776 -:102CB0004DFF20B9C4F800B0BFE70122E7E7C0F834 -:102CC00000B05E4620600446C1E74545D5D9484678 -:102CD000FFF73CFF08B92060AFE7C0F800B000265E -:102CE00020600446B6E700002DE9F04F2DED028B81 -:102CF00083B0CDE90013BDF83C4080469246002ADF -:102D000000F087802CB10E9B002B00F08280072CF6 -:102D10002BD808F10C00FFF719FF054638B96FF002 -:102D20000205284603B0BDEC028BBDE8F08F1422EB -:102D30000021FEF79DFE22460E9905F10800FEF7E0 -:102D400085FE631C2B749AF800302C4403F01F039B -:102D500063F03F032372009B43F0004169604046EB -:102D60002946FFF75BFE0125DBE700F10C034FF07E -:102D7000000908EE103A4FF0800B4E464D4618EE13 -:102D8000100AFFF7E3FE07460028C8D014220021EE -:102D9000FEF76EFE002E3AD1019B3B8102220E9B74 -:102DA00002F1080103EB060C57FA81F11046B44218 -:102DB00002F10102D2B201D8024607E01CF8010B71 -:102DC00001F8010B0136062AB6B2EFD99AF80010C5 -:102DD00001F01F01B44208BF4FF0400BB81841EAA0 -:102DE00049114BEA01030372009B013243F0004397 -:102DF0007B603A7439464046FFF710FE0135B44215 -:102E00002DB289F001094FF0000BB8D189E70022FB -:102E1000C5E76FF0010584E7F8B515460E46242294 -:102E2000002104461F46FEF723FE069B6360B5F5AE -:102E3000001F079BA76034BF6A094FF6FF72236229 -:102E400004F10C0097B200239A4205D800230360D6 -:102E500027826382A382F8BD0660013330462036A4 -:102E6000F2E7000003781BB94BB2002BC8BF01701A -:102E700070470000007870472DE9F74FDDF83C906F -:102E8000BDF830500D9E9DF83840BDF8407080462A -:102E900092469B46B9F1000F01D1002F51D11F2C52 -:102EA0004FD898F80000B0B9072F47D835F0030382 -:102EB00047D13A4649464FF6FF70FFF73BFE20F0F8 -:102EC00001002D02400445EA0464400C44EA402419 -:102ED0004FF6FF7321E040EA0520072F40EA046423 -:102EE000F6D900254FF6FF73C5F12000A5F12002A9 -:102EF0002AFA05F10BFA00F001432BFA02F2114312 -:102F00001846C9B2FFF704FE0835402D0346EBD141 -:102F10003A464946FFF70EFE0346CDE90097324692 -:102F200021464046FFF7E0FE33780133DBB21F2B2A -:102F300088BF0023337003B0BDE8F08F6FF003004B -:102F4000F9E76FF00100F6E72DE9F04F85B0924602 -:102F5000DDF848800F9D9DF840209DF84490BDF815 -:102F60004C7006469B46B8F1000F01D1002F48D1A6 -:102F70001F2A46D83378002B46D00C0244EA02645C -:102F80009DF8381044EAC93444EA01441C43072F31 -:102F900044F0800432D900234FF6FF72C3F1200CB5 -:102FA000A3F120002AFA03F10BFA0CFC41EA0C0110 -:102FB0002BFA00F00143C9B210460393FFF7A8FDB6 -:102FC000039B0833402B0246E8D13A464146FFF7BF -:102FD000B1FD0346CDE900872A4621463046FFF77A -:102FE00083FEB9F1010F06D12B780133DBB21F2B21 -:102FF00088BF00232B7005B0BDE8F08F4FF6FF733C -:10300000E8E76FF00100F6E76FF00300F3E7000078 -:10301000C06900B104307047C3691A68C261C268F0 -:103020001A60C360438A013B438270472DE9F04137 -:10303000D0F81880194E14461D464146002709B99C -:10304000BDE8F081D1E90223A21A65EB03039642A1 -:1030500077EB03031ED283698B420DD1FFF7A2FDEC -:1030600083691B688361C3680B60438AC16081699F -:10307000013B43828846E2E7FFF794FD0B68C8F8FE -:103080000030C3680B60438AC160013B4382D8F8BB -:103090000010D4E788460968D1E700BF80841E008D -:1030A0002DE9F04F8BB00D46DDF8509014469B464D -:1030B0008046002800F01981B9F1000F00F0158159 -:1030C000531E3F2B00F21181012A03D1BBF1000FE7 -:1030D00040F00B810023CDE90833B8F81430B5EB8C -:1030E000C30F4FEAC30703D300200BB0BDE8F08F36 -:1030F0002B199F42D8F80C303ABF7F1BFFB22746EE -:103100001BB9D8F81030002B7AD02F2D4ED8C5F12E -:103110003006B7424FF000032CBFF6B23E46009394 -:103120002946D8F8080008AB3246FFF7B9FCA7EBF0 -:10313000060A35445FFA8AFAB8F8143003F10053EE -:10314000063BDB000493D8F80C3003933021039B3B -:1031500013B1BAF1000F2CD1D8F8100040B1BAF178 -:10316000000F05D0009608AB5246691AFFF798FC8D -:1031700038B2002FB8D066070AD00AAB03EBD401EF -:10318000624211F8083C02F00702134101F8083CC2 -:10319000082C3CD9102C40F2B580202C40F2B7808E -:1031A000BBF1000F00F09C80082334E0BA460026F3 -:1031B000C2E7049BE02B28BFE02306930B44AB42FD -:1031C000059314D95A1B03980096924534BF524672 -:1031D000D2B2691A08AB04300792FFF761FC079A74 -:1031E0001644AAEB020A1544F6B25FFA8AFA049B67 -:1031F000069A05999B1A0493039B1B680393A6E701 -:103200000093D8F8080008AB3A462946AEE7BBF170 -:10321000000F13D00123B4EBC30F6CD0082C12D8CD -:103220009DF82030621E23FA02F2D50706D54FF032 -:10323000FF3202FA04F423438DF820309DF8203049 -:1032400089F8003051E7102C12D8BDF82030621EEA -:1032500023FA02F2D10706D54FF0FF3202FA04F446 -:103260002343ADF82030BDF82030A9F800303CE70A -:10327000202C0FD80899631E21FA03F3DA0705D52D -:103280004FF0FF3202FA04F40C430894089BC9F88B -:1032900000302AE7402C2BD0DDE90865611EC4F11F -:1032A0002102A4F1210326FA01F105FA02F225FA1E -:1032B00003F311431943CB0712D50122A4F12003D4 -:1032C000C4F1200102FA03F322FA01F1A2405242B2 -:1032D00043EA010363EB430332432B43CDE9082365 -:1032E000DDE90823C9E90023FFE66FF00100FCE6F1 -:1032F0006FF00800F9E6082CA0D9102CB3D9202CC7 -:10330000EED8C3E7BBF1000FADD0022383E7BBF1DA -:10331000000FBBD004237EE730B5012A144638BF26 -:103320000124402C85B028BF40240025012ACDE986 -:10333000025518D81B788DF8083063070AD004AB03 -:1033400003EBD405624215F8083C02F007029340F3 -:1033500005F8083C009103462246002102A8FFF729 -:103360009FFB05B030BD082AE4D9102A03D81B887A -:10337000ADF80830E1E7202A8DBFD3E900231B68B0 -:103380000293CDE90223D8E710B5CB681BB98B6057 -:103390000B618B8210BDC4681A681C60C360438ACD -:1033A000013B4382CA60F0E72DE9F04FD1F800807D -:1033B00093B018F0800FCDE90323C8F3C01219BFF2 -:1033C000C8F3C03BC8F306264FF0020B1646B8F10F -:1033D000000F04460D4680F2C58118F0C0430593E6 -:1033E00040F0C0810B7B002B00F0BC81BBF1020FD1 -:1033F00003D00178B14240F0B88108F07F01069116 -:103400006AB3C8F3074A2B44069A93F803907606EA -:1034100046EA0B4646EA82465FEAD91346EA0A06BE -:10342000079300F0908000220023CDE90A23069B39 -:10343000009367685B4652460AA92046B8470028B1 -:103440007ED0A7699FB9314604F10C00FFF78CFBD1 -:103450000746E0B96FF0020013B0BDE8F08FC8F383 -:103460000F2A18F07F0F08BF0AF0030ACBE73B6969 -:103470009E420DD03F68002FF9D1314604F10C0077 -:10348000FFF772FB07460028E4D0A3693B60A76101 -:10349000DDE90A2300264FF6FF70C6F1200E22FA5E -:1034A00006F103FA0EFEA6F1200C23FA0CFC41EA09 -:1034B0000E0141EA0C01C9B2083609920893FFF7E0 -:1034C00027FB402EDDE90832E7D1B882FB7D09F009 -:1034D0001F06C3F38403F31AD7E9022198B2002B25 -:1034E000BCBF00F120031BB252EA0100C8F304681C -:1034F0000FD00398821A049860EB0101A148904212 -:103500004FF000028A4104D3079A002A55D0012BBC -:1035100023DDFA7D4FEA890302F0030203F07C0306 -:103520001343FB7539462046FFF73CFB079BA3B9C5 -:10353000FB7DC3F38402013262F38603FB7504E072 -:103540006FF00B0088E7A76917B96FF00C0083E7ED -:103550003B699E42BAD03F68F6E719F0400F2CD085 -:10356000039BBB60049BFB60142200210DA8FEF7A7 -:103570007FFA039B0A93049B0B932B1D0C932B7BCD -:10358000ADF83EA0013BDBB2ADF83C30069B8DF8B8 -:1035900043308DF840B0A3688DF841608DF84280CB -:1035A0000AA920469847FB7DC3F38403013303F047 -:1035B0001F039B02FB8200204EE7FB7DC9F34012F4 -:1035C000B2EBD31F40F0D480C3F38403B34240F086 -:1035D000D28007992B7B4FEA9912002934D0D20769 -:1035E00041D4032B40F2CA80039BBB60049BFB6069 -:1035F0002B7BAE1D033BDBB23246394604F10C0097 -:10360000FFF7E2FA00280DDA20463946FFF7CAFA3A -:10361000FB7DC3F38403013303F01F039B02FB8292 -:10362000032019E7AB883B832A7B033AB88AD2B2DE -:103630003146FFF77FFAFB7DB882DA43C2F3C0124E -:1036400062F3C713FB75B6E76AB92E1D013BDBB207 -:103650003246394604F10C00FFF7B6FA0028D3DBF6 -:103660002A7B013AE2E7F98AC1F30901013B052906 -:10367000DAB253D8281D002307F11A0C9A4208D950 -:1036800010F801EB0CF801E0013101330629DBB23F -:10369000F4D103990A9104990B91934207F11A010D -:1036A0000C9138BF043379680D9134BF55FA83F318 -:1036B00000230E93FB8AADF83EA0C3F309031A441E -:1036C000069B8DF843300023ADF83C208DF840B0C8 -:1036D0008DF841608DF842807B602A7BB88A013A80 -:1036E000291DFFF727FA3B8BB882834203D1A368D9 -:1036F0000AA92046984720460AA9FFF745FEFB7D08 -:10370000B88AC3F38403013303F01F039B02FB82D7 -:103710003B8B984214BF112000209DE67B68002B54 -:10372000B7D0062001E01C306346D3F800C0BCF1DE -:10373000000FF8D1091A081D05F1040C00EB03096C -:1037400005989DF8143001EB000EBEF11B0FA0D8B8 -:103750009A429ED91CF8013B09F8013B059B0133B5 -:103760000593EDE76FF0090076E66FF00A0073E667 -:103770006FF00D0070E66FF00E006DE66FF00F0059 -:103780006AE600BF80841E00EFF3098305494A6B97 -:1037900022F001024A63683383F30988002383F32C -:1037A0001188704700EF00E0202080F3118862B696 -:1037B0000C4B0D4AD96821F4E0610904090C0A4355 -:1037C000DA60D3F8FC20094942F08072C3F8FC208B -:1037D0000A6842F001020A601022DA7783F82200B8 -:1037E000704700BF00ED00E00003FA05001000E0A4 -:1037F00010B5202383F311880E4B5B6813F400632C -:1038000014D0F1EE103AEFF30984683C4FF0807366 -:10381000E361094BDB6B236684F3098800F0F2FB5C -:1038200010B1064BA36110BD054BFBE783F3118874 -:10383000F9E700BF00ED00E000EF00E00B06000834 -:103840000E060008026843681143016003B118477F -:1038500070470000024A136843F0C00313607047CA -:1038600000380140024A136843F0C00313607047F8 -:1038700000440040024A136843F0C00313607047DD -:103880000048004037B5244C244D204600F0FCFA97 -:10389000009404F1140022490023202200F0BEF914 -:1038A00020490094202204F138001F4B00F038FA20 -:1038B0001E4BC4E917351E4C204600F0E5FA009473 -:1038C00004F114001B490023202200F0A7F91A4933 -:1038D0000094202204F13800184B00F021FA184B14 -:1038E000C4E91735174C204600F0CEFA04F1140055 -:1038F000154900940023202200F090F9134B14493D -:103900000094202204F1380000F00AFA114BC4E9B7 -:10391000173503B030BD00BFFC34002000B4C40430 -:1039200040360020A03600205538000800380140FD -:103930006835002060360020C03600206538000859 -:1039400000440040D435002080360020753800083F -:10395000E03600200048004030B5037C304C0029A0 -:1039600018BF0C46012B0FD12E4B984239D12E4B4C -:103970001A6E42F480421A66D3F8802042F48042E4 -:10398000C3F88020D3F880302268036EC16D84664E -:1039900003EB5203B3FBF2F36268150442BF23F05A -:1039A000070503F0070343EA4503CB60A36843F030 -:1039B00040034B60E36843F001038B6042F496736D -:1039C00043F001030B604FF0FF330B62510505D547 -:1039D00012F010221FD0B2F1805F1ED080F8643048 -:1039E00030BD124B98420BD0114B9842CCD10E4BAC -:1039F0009A6D42F480229A659A6F42F4802207E021 -:103A0000094B9A6D42F400329A659A6F42F4003283 -:103A10009A679B6FB8E77F23E0E73F23DEE700BFAD -:103A200074510008FC34002000100240683500206A -:103A3000D43500202DE9F047C66D3768F46934624B -:103A40002107054618D014F0080118BF8021E207AD -:103A500048BF41F02001A30748BF41F04001600783 -:103A600048BF41F48071202383F31188281DFFF79C -:103A7000E9FE002383F31188E2050AD5202383F3AE -:103A800011884FF40071281DFFF7DCFE002383F33B -:103A900011884FF020094FF0000A14F0200838D1A7 -:103AA0003B0616D54FF0200905F1380A200610D53F -:103AB00089F31188504600F067F9002836DA0821AA -:103AC000281DFFF7BFFE27F080033360002383F338 -:103AD0001188790614D5620612D5202383F3118844 -:103AE000D5E913239A4208D12B6C33B11021281D3C -:103AF00027F04007FFF7A6FE3760002383F3118805 -:103B0000E30619D5AA6E1369B3B1BDE8F047506951 -:103B1000184789F31188B38C95F86410284619402A -:103B200000F0CCF98AF31188F469B6E780B23085E9 -:103B300088F31188F469B9E7BDE8F08700F16043C4 -:103B400003F561430901C9B283F80013012200F0B3 -:103B50001F039A4043099B0003F1604303F561434F -:103B6000C3F880211A607047F8B515468268066967 -:103B7000AA420B46816938BF8568761AB542044669 -:103B80000BD218462A46FDF761FFA3692B44A361B7 -:103B9000A3685B1BA3602846F8BD0CD918463246C3 -:103BA000FDF754FFAF1BE1683A463044FDF74EFF86 -:103BB000E3683B44EBE718462A46FDF747FFE36816 -:103BC000E5E7000083689342F7B51546044638BF21 -:103BD0008568D0E90460361AB5420BD22A46FDF753 -:103BE00035FF63692B446361A36828465B1BA360B0 -:103BF00003B0F0BD0DD932460191FDF727FF0199C1 -:103C0000E068AF1B3A463144FDF720FFE3683B44D0 -:103C1000E9E72A46FDF71AFFE368E4E710B50A442E -:103C20000024C361029B8460C0E90000C0E9051163 -:103C3000C1600261036210BD08B5D0E9053293424C -:103C400001D1826882B98268013282605A1C426165 -:103C50001970D0E904329A4224BFC368436100213D -:103C600000F0A0FA002008BD4FF0FF30FBE7000095 -:103C700070B5202304460E4683F31188A568A5B1CC -:103C8000A368A269013BA360531CA3611578226954 -:103C9000934224BFE368A361E3690BB120469847D0 -:103CA000002383F31188284607E03146204600F0C0 -:103CB00069FA0028E2DA85F3118870BD2DE9F74F23 -:103CC00004460E4617469846D0F81C904FF0200A3E -:103CD0008AF311884FF0000B154665B12A4631462C -:103CE0002046FFF741FF034660B94146204600F0F9 -:103CF00049FA0028F1D0002383F31188781B03B020 -:103D0000BDE8F08FB9F1000F03D001902046C847FD -:103D1000019B8BF31188ED1A1E448AF31188DCE7AE -:103D2000C0E90511C160C3611144009B8260C0E914 -:103D30000000016103627047F8B504460D4616465F -:103D4000202383F31188A768A7B1A368013BA36070 -:103D500063695A1C62611D70D4E904329A4224BF1F -:103D6000E3686361E3690BB120469847002080F364 -:103D7000118807E03146204600F004FA0028E2DA14 -:103D800087F31188F8BD0000D0E905239A4210B5E9 -:103D900001D182687AB98268013282605A1C8261DC -:103DA0001C7803699A4224BFC3688361002100F034 -:103DB000F9F9204610BD4FF0FF30FBE72DE9F74F32 -:103DC00004460E4617469846D0F81C904FF0200A3D -:103DD0008AF311884FF0000B154665B12A4631462B -:103DE0002046FFF7EFFE034660B94146204600F04B -:103DF000C9F90028F1D0002383F31188781B03B0A0 -:103E0000BDE8F08FB9F1000F03D001902046C847FC -:103E1000019B8BF31188ED1A1E448AF31188DCE7AD -:103E2000026843681143016003B1184770470000FE -:103E30001430FFF743BF00004FF0FF331430FFF79B -:103E40003DBF00003830FFF7B9BF00004FF0FF332F -:103E50003830FFF7B3BF00001430FFF709BF000090 -:103E60004FF0FF311430FFF703BF00003830FFF789 -:103E700063BF00004FF0FF323830FFF75DBF000036 -:103E800000207047FFF7FEBC044B03600023C0E92D -:103E90000233436001230374704700BF8C51000854 -:103EA00010B52023044683F31188FFF755FD022344 -:103EB0002374002383F3118810BD000038B5C36953 -:103EC00004460D461BB904210844FFF7A9FF294603 -:103ED00004F11400FFF7B0FE002806DA201D4FF4AD -:103EE0008061BDE83840FFF79BBF38BD024B002220 -:103EF000C3E900339A607047003700200023037441 -:103F00008268054B1B6899689142FBD25A6803602E -:103F100042601060586070470037002008B52023C9 -:103F200083F31188037C032B05D0042B0DD02BB910 -:103F300083F3118808BD436900221A604FF0FF33F4 -:103F40004361FFF7DBFF0023F2E7D0E900321360A3 -:103F50005A60F3E7002303748268054B1B68996875 -:103F60009142FBD85A680360426010605860704705 -:103F700000370020054B19690874186802681A6038 -:103F80005360186101230374FCF72ABB003700203B -:103F900030B54B1C0B4D87B0044610D02B690A4A34 -:103FA00001A800F019F92046FFF7E4FF049B13B1C4 -:103FB00001A800F04DF92B69586907B030BDFFF733 -:103FC000D9FFF8E7003700201D3F000838B50C4D39 -:103FD00041612B6981689A689142044603D8BDE823 -:103FE0003840FFF78BBF1846FFF7B4FF01232C6161 -:103FF000014623742046BDE83840FCF7F1BA00BF03 -:1040000000370020044B1A681B6990689B6898422F -:1040100094BF0020012070470037002010B5084CE5 -:10402000236820691A6822605460012223611A748F -:10403000FFF790FF01462069BDE81040FCF7D0BAB9 -:104040000037002008B5FFF7DDFF18B1BDE80840D4 -:10405000FFF7E4BF08BD0000FFF7E0BFFEE7000088 -:1040600010B50C4CFFF742FF00F0A8F80A49802277 -:10407000204600F03DF8012344F8180C0374FFF7C4 -:1040800093FB002383F3118862B60448BDE8104017 -:1040900000F04EB828370020B4510008C451000881 -:1040A00008B572B6034B586200F060FA00F00CFBE2 -:1040B000FEE700BF0037002000F004B9EFF31180E5 -:1040C00020B9EFF30583202282F3118870470000A6 -:1040D00010B530B9EFF30584C4F3080414B180F3CC -:1040E000118810BDFFF7AEFF84F31188F9E70000D7 -:1040F00082600222028270478368A3F17C0243F847 -:104100000C2C026943F83C2C426943F8382C074ACE -:1041100043F81C2CC26843F8102C022203F8082C28 -:10412000002203F8072CA3F118007047F9050008D6 -:1041300010B5202383F31188FFF7DEFF002104462A -:10414000FFF744FF002383F31188204610BD0000D1 -:10415000024B1B6958610F20FFF70CBF003700208E -:10416000202383F31188FFF7F3BF000008B5014651 -:10417000202383F311880820FFF70AFF002383F32D -:10418000118808BD49B1064B42681B6918605A6026 -:10419000136043600420FFF7FBBE4FF0FF30704711 -:1041A000003700200368984206D01A680260506009 -:1041B00059611846FFF7A2BE70470000054B03F196 -:1041C0001402C3E905224FF0FF310022C3E90712B0 -:1041D000704700BF0037002070B51C4EC0E90323B4 -:1041E00005460C4600F0EAFA334653F8142F9A427B -:1041F0000DD13062C5E901242A600A2C2CBF0019B8 -:104200000A30C6E90555BDE8704000F0C5BA316A0C -:10421000431AE31838BF1C469368A34202D9081911 -:1042200000F0C8FA73699A6894420CD85A68AC6076 -:104230002B606A6015609A685D60121B9A604FF08F -:10424000FF33F36170BD1B68A41AECE70037002050 -:1042500038B51B4C636998420DD0D0E90032136029 -:104260005A6000228168C2609A680A449A604FF0DE -:10427000FF33E36138BD2246036842F8143F002152 -:1042800093425A60C16003D1BDE8384000F08CBA57 -:104290009A688168256A0A449A6000F08FFA636917 -:1042A0009A68411B8A42E5D9AB181D1A092D206A6C -:1042B00098BF01F10A02BDE83840104400F07ABA14 -:1042C000003700202DE9F041184C002704F11406B6 -:1042D000656900F073FA236AAA68C11A8A4215D880 -:1042E00013442362D5E9003213605A606369D5F83C -:1042F0000C80EF60B34201D100F056FA87F31188C9 -:104300002869C047202383F31188E1E76169B1423E -:1043100009D013441B1ABDE8F0410A2B2CBFC0186A -:104320000A3000F047BABDE8F08100BF0037002036 -:104330000C2303604FF0FF307047000000207047EF -:10434000FEE70000704700004FF0FF3070470000AC -:10435000BFF34F8F024A1369DB03FCD4704700BFE1 -:104360000020024008B5094B1B7873B9FFF7F0FF36 -:10437000074B5A69002ABFBF064A9A6002F1883289 -:104380009A601A6822F480621A6008BD883800209A -:10439000002002402301674508B50B4B1B7893B9F9 -:1043A000FFF7D6FF094B5A6942F000425A611A687A -:1043B00042F480521A601A6822F480521A601A6815 -:1043C00042F480621A6008BD883800200020024054 -:1043D0007F289ABF00F58030C0020020704700009F -:1043E0004FF4006070470000802070477F2808B5B8 -:1043F0000BD8FFF7EDFF00F500630268013204D12E -:1044000004308342F9D1012008BD0020FCE7000000 -:104410007F2810B504461FD8FFF79AFFFFF7A2FFC9 -:104420000E4BF3221A6102225A615A6942EAC00213 -:104430005A615A6942F480325A61FFF789FF4FF49A -:104440000061FFF7C5FF00F03BF9FFF7A5FF20462D -:10445000BDE81040FFF7CABF002010BD0020024099 -:104460002DE9F84340EA020313F00703144606D08F -:104470002E4B40F2F5221A600020BDE8F883851823 -:104480002B4A95420CD9294A40F2FA211160F3E7F0 -:10449000031D1B684A68934208D1083C0830083164 -:1044A000072C14D902680B689A42F1D0FFF75AFF23 -:1044B000FFF74EFF1F4E08314FF001084FF0000983 -:1044C000012CA1F1080706D8FFF766FF01E0002CD8 -:1044D000ECD10120D1E7C6F81480054651F8083C1C -:1044E00045F8043B51F8043C4360FFF731FFC6F840 -:1044F0001490026851F8083C9A420CD00B4B4FF4D0 -:1045000048721A600C4B18600C4B1C600C4B1F60FF -:10451000FFF742FFB0E72D6851F8043C9D4201F1DE -:104520000801EBD1083C0830CAE700BF84380020FE -:104530000000040800200240783800208038002065 -:104540007C380020084908B50B7828B11BB9FFF763 -:1045500009FF01230B7008BD002BFCD0BDE808400B -:104560000870FFF719BF00BF883800204FF4803172 -:104570004FF0005000F0A6B870B582B0FFF79EFD76 -:104580000E4E054600F01AF93268904237BF0C4AC9 -:104590000B49516814682EBFD1E9004101315160C7 -:1045A0000419034641F10001284601913360FFF7E9 -:1045B0008FFD0199204602B070BD00BF8C380020ED -:1045C0009038002070B582B0FFF778FD104E054698 -:1045D00000F0F4F83268904237BF0E4A0D49516836 -:1045E00014682EBFD1E9004101315160041941F135 -:1045F00000010346284601913360FFF769FD0199E8 -:104600004FF47A7200232046FBF7D2FD02B070BD52 -:104610008C3800209038002010B50244064BD2B2EE -:10462000904200D110BD441C00B253F8200041F864 -:10463000040BE0B2F4E700BF50280040114B30B546 -:10464000D3F89040240409D4D3F89040C3F89040A4 -:10465000D3F8904044F40044C3F890400A4C2368D7 -:1046600043F4807323600244084BD2B2904200D1DD -:1046700030BD441C00B251F8045B43F82050E0B256 -:10468000F4E700BF001002400070004050280040D6 -:1046900007B5012201A90020FFF7BEFF019803B072 -:1046A0005DF804FB13B50446FFF7F2FFA04205D006 -:1046B000012201A900200194FFF7C0FF02B010BD44 -:1046C000704700007047000070470000074B45F23C -:1046D00055521A6002225A6040F6FF729A604CF6F8 -:1046E000CC421A60024B01221A7070470030004021 -:1046F0009C380020034B1B781BB1034B4AF6AA22BF -:104700001A6070479C38002000300040054B1A6842 -:1047100032B902F1804202F50432D2F894201A60D4 -:10472000704700BF98380020024B4FF40002C3F8D6 -:10473000942070470010024008B5FFF7E7FF024BD6 -:104740001868C0F3407008BD98380020704700001A -:10475000FEE700000A4B0B480B4A90420BD30B4B71 -:10476000DA1C121AC11E22F003028B4238BF00224B -:104770000021FDF77DB953F8041B40F8041BECE75A -:1047800074520008343900203439002034390020B4 -:104790007047000000F07AB84FF080430022586361 -:1047A0001A610222DA6070474FF080430022DA601B -:1047B000704700004FF08043586370474FF08043CC -:1047C000586A7047264B2748DA6A42F0070210B44D -:1047D000DA62DA6A22F00702DA62DA6ADA6C42F046 -:1047E0000702DA64DA6E42F00702DA664FF09042AE -:1047F000DB6E4FF0AA31002353604FF4EE4491601A -:10480000D0604FF6FF7050611462174C5362146011 -:10481000164CC2F80434C2F80814C2F80C444FF61F -:10482000F774C2F814444FF0EE44C2F8204447F243 -:104830009974C2F824440E4CC2F80044C2F80438FB -:10484000C2F80818C2F80C38C2F81408C2F82038A8 -:10485000C2F82438C2F800385DF8044B00F09CB868 -:104860000010024050010024A00100281050500008 -:1048700050A0AA0008B500F035FAFFF7F1FBBDE83B -:104880000840FFF743BF0000704700000F4B9A6DD0 -:1048900042F001029A659A6F42F001029A670C4A4F -:1048A0009B6F936843F0010393604FF080434F2266 -:1048B0009A624FF0FF32DA6200229A615A63DA603C -:1048C0005A6001225A611A60704700BF001002400E -:1048D000002004E04FF0804208B51169D3680B4016 -:1048E000D9B2C9439B07116107D5202383F31188EF -:1048F000FFF7E2FB002383F3118808BD08B5244BC2 -:104900004FF0FF319A6A99629A6A00229A62986A15 -:10491000D86A60F00700D862D86A00F00700D86251 -:10492000D86A186B1963186B1A63186B986B60F070 -:1049300080509863986B00F080509863986BD86BA8 -:10494000D963D86BDA63D86B186C1964196C1A6464 -:10495000196C996D41F080519965996F41F08051C2 -:104960009967996FD3F8901011F4407F1EBF4FF4F0 -:104970008031C3F89010C3F89020D3F89020C3F88A -:10498000902000F07FF9034B00225A6008BD00BF61 -:104990000010024000700040394B3A4A9A650122EB -:1049A0001A601A689007FCD500229A609A6812F083 -:1049B0000C0FFBD1344A4FF4007111605169490565 -:1049C000FCD41A6842F480321A601A689203FCD54B -:1049D0002D490A6842F480720A602C4A4FF4E06163 -:1049E00011601A6842F060021A601A6842F0080208 -:1049F0001A601A689007FCD59A6812F00C0FFBD168 -:104A0000D3F8942042F4C062C3F89420204ADA60BC -:104A10001A6842F080721A601A689101FCD51D4A2A -:104A20001A611A6842F080621A601A681201FCD595 -:104A300000229A601549184AC3F888200A6822F0B3 -:104A4000070242F004020A600A6802F00702042A20 -:104A5000FAD19A6842F003029A609A6802F00C0256 -:104A60000C2AFAD11A6E42F001021A66D3F880209D -:104A700042F00102C3F88020D3F88030704700BFB5 -:104A800000100240000400100070004000200240AE -:104A900003140001000C100055550134074A08B5F5 -:104AA000536903F00103536123B1054A13680BB145 -:104AB00050689847BDE80840FEF79ABE00040140E0 -:104AC000A0380020074A08B5536903F00203536178 -:104AD00023B1054A93680BB1D0689847BDE80840F8 -:104AE000FEF786BE00040140A0380020074A08B542 -:104AF000536903F00403536123B1054A13690BB1F1 -:104B000050699847BDE80840FEF772BE00040140B6 -:104B1000A0380020074A08B5536903F00803536121 -:104B200023B1054A93690BB1D0699847BDE80840A5 -:104B3000FEF75EBE00040140A0380020074A08B519 -:104B4000536903F01003536123B1054A136A0BB193 -:104B5000506A9847BDE80840FEF74ABE000401408D -:104B6000A0380020164B10B55C6904F478725A61C5 -:104B7000A30604D5134A936A0BB1D06A984760061E -:104B800004D5104A136B0BB1506B9847210604D51E -:104B90000C4A936B0BB1D06B9847E20504D5094AD8 -:104BA000136C0BB1506C9847A30504D5054A936C60 -:104BB0000BB1D06C9847BDE81040FEF719BE00BF9E -:104BC00000040140A0380020194B10B55C6904F4C2 -:104BD0007C425A61620504D5164A136D0BB1506DC3 -:104BE0009847230504D5134A936D0BB1D06D9847B0 -:104BF000E00404D50F4A136E0BB1506E9847A10420 -:104C000004D50C4A936E0BB1D06E9847620404D55C -:104C1000084A136F0BB1506F9847230404D5054A17 -:104C2000936F0BB1D06F9847BDE81040FEF7E0BD21 -:104C300000040140A038002008B50348FEF7FAFE42 -:104C4000BDE80840FEF7D4BDFC34002008B5034899 -:104C5000FEF7F0FEBDE80840FEF7CABD683500204B -:104C600008B50348FEF7E6FEBDE80840FEF7C0BD04 -:104C7000D435002008B5FFF72DFEBDE80840FEF74B -:104C8000B7BD0000062108B50846FEF757FF06210C -:104C90000720FEF753FF06210820FEF74FFF0621ED -:104CA0000920FEF74BFF06210A20FEF747FF0621E9 -:104CB0001720FEF743FF06212820FEF73FFF0721BC -:104CC0001C20FEF73BFF0C212520FEF737FF0C21AF -:104CD0002620FEF733FFBDE808400C212720FEF711 -:104CE0002DBF000008B5FFF709FE00F009F8FFF737 -:104CF000C9F8FFF7C9FDBDE80840FFF74BBD00004C -:104D00000023054A19460133102BC2E9001102F1B4 -:104D10000802F8D1704700BFA03800200B460146BA -:104D2000184600F025B8000000F038B8012838BF58 -:104D3000012010B50446204600F028F830B900F0F4 -:104D400007F808B900F00CF88047F4E710BD000040 -:104D5000024B1868BFF35B8F704700BF20390020FB -:104D600008B5062000F032F90120FFF7E9FA00004B -:104D700010B5054C13462CB10A4601460220AFF38C -:104D8000008010BD2046FCE700000000024B0146F9 -:104D9000186800F089B800BF28110020024B0146B6 -:104DA000186800F035B800BF2811002010B501398F -:104DB0000244904201D1002005E0037811F8014F30 -:104DC000A34201D0181B10BD0130F2E72DE9F041DC -:104DD000A3B1C91A17780144044603F1FF3C8C4281 -:104DE000204601D9002009E00578BD4204F1010404 -:104DF000F5D10CEB0405D618A54201D1BDE8F08130 -:104E000015F8018D16F801EDF045F5D0E7E7000043 -:104E100037B5002944D051F8043C0190002BA1F192 -:104E20000404B8BFE41800F0F5F81E4A01981368AE -:104E300033B96360146003B0BDE8304000F0F0B8EF -:104E4000A34208D9256861198B4201BF19685B68C4 -:104E500049192160EDE71A465B680BB1A342FAD904 -:104E600011685518A5420BD1246821445418A34257 -:104E70001160E0D11C685B68536021441160DAE77F -:104E800002D90C230360D6E7256861198B4204BF61 -:104E900019685B68636004BF491921605460CAE700 -:104EA00003B030BD24390020F8B5CD1C25F0030532 -:104EB00008350C2D38BF0C25002D064601DBA94214 -:104EC00003D90C2333600020F8BD00F0A3F821497A -:104ED0000A6814469CB9204F3B6823B921463046E6 -:104EE00000F03CF838602946304600F037F8431CA3 -:104EF00023D10C233360304600F092F8E3E72368B7 -:104F00005B1B17D40B2B03D923601C44256004E0E2 -:104F10006368A2420CBF0B605360304600F080F81B -:104F200004F10B00231D20F00700C21ACCD01B1A7D -:104F3000A350C9E722466468CCE7C41C24F00304EC -:104F4000A042E3D0211A304600F008F80130DDD14C -:104F5000CFE700BF243900202839002038B5064D9E -:104F60000023044608462B60FFF7E2F9431C02D1F8 -:104F70002B6803B1236038BD2C3900201F2938B5B8 -:104F800004460D4604D9162303604FF0FF3038BDA8 -:104F9000426C12B152F821304BB9204600F030F883 -:104FA0002A4601462046BDE8384000F017B8012BDC -:104FB0000AD0591C03D1162303600120E7E700241F -:104FC00042F82540284698470020E0E7024B01467A -:104FD0001868FFF7D3BF00BF2811002038B5074D70 -:104FE00000230446084611462B60FFF7ADF9431C29 -:104FF00002D12B6803B1236038BD00BF2C390020DB -:10500000FFF79CB9034611F8012B03F8012B002A86 -:10501000F9D17047014800F009B800BF30390020CD -:10502000014800F005B800BF30390020704700008B -:10503000704700006F72672E6172647570696C6FE3 -:10504000742E4D6174656B4C3433312D416972732C -:1050500070656564000000004E6F20617070207301 -:1050600069670A00426164206677206C656E677428 -:10507000682025750A0042616420626F6172645F76 -:1050800069642025752073686F756C6420626520E3 -:1050900025750A0042616420667720646573637237 -:1050A0006970746F72206C656E6774682025750A6C -:1050B00000426164206170702043524320307825A3 -:1050C0003038783A307825303878203078253038C4 -:1050D000783A3078253038780A00476F6F64206658 -:1050E00069726D776172650A0040A2E4F1646891AB -:1050F0000600000053544D33324C343F3F00000053 -:10510000FC34002068350020D43500204261642042 -:1051100043414E496661636520696E6465782E007F -:1051200080000000008000000000800000000000FF -:105130000000000079230008D92200089D1B000808 -:10514000D51B0008D11D0008A11B0008B51B0008D5 -:10515000A51B0008A91B0008B11B0008AD1B000817 -:10516000F91C0008B91B0008E5250008C91B000848 -:10517000CD1C0008009600000000000000000000A8 -:10518000000000000000000000000000000000001F -:105190004D3E0008393E0008753E0008613E00089B -:1051A0006D3E0008593E0008453E0008313E0008AB -:1051B000813E00086D61696E0000000069646C65E5 -:1051C00000000000BC510008403700207838002063 -:1051D000010000005D4000080000000004B0FF7FF7 -:1051E0000100000000000000260400000000000094 -:1051F0000060030000000000FE2A0100D20400004D -:10520000FF00000000000000F45000083F00000014 -:105210002C11002000000000000000000000000031 -:10522000000000000000000000000000000000007E +:101B7000220002F0EDB900BFDC340020F434002074 +:101B8000B0510008202383F3118862B6704700002B +:101B9000002383F3118862B67047000010B4026816 +:101BA00054681A4623465DF8044B184701207047D5 +:101BB0000020704700207047704700007047000009 +:101BC000002070470E20704700F5805090F8C80044 +:101BD000C0F340007047000000F5805090F9C90044 +:101BE000704700002DE9F0410C68BDF8187014F042 +:101BF00000541E466FD10B7B082B6CD8FFF7C2FF39 +:101C00004569AB685B010CD4AB681B0108D4AC68B8 +:101C100014F080545DD1FFF7BBFF2046BDE8F08192 +:101C200001240B6804F1180C002BB8BFDB004FEA4D +:101C30000C1CB4BF43F004035B0545F80C300B6883 +:101C40000FFA84FC13F0804F18BF05EB0C1E05EB58 +:101C50000C1C1EBFDEF8803143F00203CEF8803149 +:101C60000B7BCCF8843105EB04158B68C5F88C31FF +:101C70004B68C5F88831DCF8803143F00103CCF8BB +:101C8000803100EB441541F268031D4403EB44131B +:101C90000344C5E9002608330D4601F10C0C55F844 +:101CA00004EB43F804EB6545F9D184342D881D809D +:101CB00000EB441407F00303257925F00B052B43B3 +:101CC0002371FFF765FF06973346BDE8F04100F04A +:101CD000AFBC0224A5E74FF0FF309FE713B500F536 +:101CE00080540191E06D00F039FD1F280AD9019957 +:101CF000E06D202200F0A8FDA0F1200358425841D9 +:101D000002B010BD0020FBE708B500F58050FFF7DA +:101D100039FFC06D00F0F6FCBDE80840FFF738BFA2 +:101D200000220260828142608260704710B500220A +:101D30000023C0E900230023044603810C30FFF791 +:101D4000EFFF204610BD0000F0B5054600F58050BD +:101D50000C4690F8C83013F0040FC3F3800108BF9D +:101D6000114661F3820304F1840680F8C83005EB64 +:101D7000461389B01B79D8072ED57AB319072DD40D +:101D80006846FFF7D3FF05EB441303F5835303F1D4 +:101D9000180703AA103318685968144603C4083397 +:101DA000BB422246F7D1186820609B88A380DDE9FA +:101DB0000E23CDE900230123ADF808302B686946D6 +:101DC0001B6C2846984705EB46152B791A075CBF14 +:101DD00043F008032B7101E0002AF4D109B0F0BDF3 +:101DE0002DE9F047074688B007F5805468469A46C3 +:101DF0008846FFF7C7FE9146FFF798FFE06D00F0B9 +:101E000093FC1F2829D9E06D2022694600F09EFD31 +:101E1000202822D103AD444605AB2E4603CE9E4278 +:101E200020606160354604F10804F6D13068206016 +:101E3000B388A380DDE90023C9E90023BDF8083099 +:101E4000AAF80030FFF7A4FE4A46534641463846FA +:101E500008B0BDE8F04700F0D9BBFFF799FE0020BD +:101E600008B0BDE8F08700002DE9F84F0023C0E975 +:101E70000133254B044640F8183B0F46FFF750FF4F +:101E800004F12800FFF752FF04F1480804F58255D9 +:101E90004646083530462036FFF748FFAE42F9D1B6 +:101EA00004F580554FF480534FF00009C5E913390C +:101EB000C5F848800123EE6504F5875804F584567B +:101EC000C5F8549085F8583085F86030083608F128 +:101ED00008084FF0000A4FF0000B46E908ABA6F1E6 +:101EE0001800FFF71DFF203646F8289C4645F4D120 +:101EF00085F8C97017B1054800F076FD044B6361A1 +:101F00002046BDE8F88F00BFE4510008BC5100082E +:101F10000064004010B5044B197804464A1C1A703E +:101F2000FFF7A2FF204610BDFC3400202DE9F0474A +:101F3000002950D0294B2A4FB7FBF1F599428CBFAD +:101F40000A231123581EB5FBF3FC03FB1C53C4B238 +:101F50002BB102280346F5D80020BDE8F0870CF12C +:101F6000FF36B6F5806FF7D2C4EBC40E0EF1030353 +:101F70004FEAE309C3F3C703A4EB030809F1010A1D +:101F80004FF47A755FFA88F009FB05555AFA88F81C +:101F9000B5FBF8F5B5F5617FC1BF0EF1FF33C3F3B3 +:101FA000C703E01AC0B25C1C50FA84F40CFB04F4C2 +:101FB000B7FBF4F4A142CFD1013BDBB20F2BCBD85E +:101FC0000138C0B20728C7D800211071168091705F +:101FD000D3700120C1E70846BFE700BF3F420F00B2 +:101FE00000B4C40470B505460E464FF47A746B69AC +:101FF0005B6803F00103B34207D04FF47A7002F03C +:102000000DF9013CF3D1204670BD0120FCE7000032 +:1020100030B54269936913F0700F16D000230B4C52 +:10202000936103F1840200EB421211794D0709D547 +:10203000890707D5416954F823508D60117941F023 +:10204000040111710133032BEBD130BDD0510008D5 +:1020500073B51D46436916469A68D207044609D5EA +:102060009A6801219960C2F34002CDE90065002120 +:10207000FFF76AFE63699A68D1050BD59A684FF439 +:1020800080719960C2F34022CDE9006501212046AC +:10209000FFF75AFE63699A68D2030BD59A684FF42A +:1020A00080319960C2F34042CDE9006502212046AB +:1020B000FFF74AFE204602B0BDE87040FFF7A8BF18 +:1020C000F8B50446466900296CD106F10C0738685A +:1020D00080076AD006EB01153868D5F8B00110F01A +:1020E000040FD5F8B0011ABFC00840F00040400D01 +:1020F000A061D5F8B0C11CF0020F1CBF40F08040B9 +:10210000A061D5F8B40106EB011100F00F0084F8CE +:102110002400D1F8B8012077D1F8B801000A60771F +:10212000D1F8B801000CA077D1F8B801000EE07723 +:10213000D1F8BC0184F82000D1F8BC01000A84F871 +:102140002100D1F8BC01000C84F82200D1F8BC11A8 +:10215000090E84F823103821396004F1340004F1A9 +:10216000180104F1240551F8046B40F8046BA942EE +:10217000F9D109880180C4E90A2321460023238676 +:1021800051F8283B20461B6C984704F580522046A6 +:1021900092F8C83043F0040382F8C830BDE8F84034 +:1021A000FFF736BF06F1100791E7F8BD10B50446FA +:1021B00000F022FC02460B4652EA030102D0013A2B +:1021C00063F100030449086820B12146BDE81040CE +:1021D000FFF776BF10BD00BFF8340020F8B500F55A +:1021E00083511E46FFF7CEFCDFF844C008310024BF +:1021F00004F1840500EB45152B795F070ED4DB064F +:102200000CD5D1E900739742B34107D243695CF81A +:1022100024709F602B7943F004032B710134032C4D +:1022200001F12001E4D1BDE8F840FFF7B1BC00BFE7 +:10223000D051000808B5FFF7A5FCFFF7E9FEBDE89F +:102240000840FFF7A5BC0000F8B54369054698684B +:1022500000F0E050B0F1E05F0F461FD0E8B1FFF7AB +:1022600091FC05F583541034002606F1840305EB38 +:1022700043131B791A0706D50136032E04F12004F7 +:10228000F3D1012007E05B07F6D42146384600F081 +:1022900009FA0028F0D1FFF77BFCF8BD0120FCE72C +:1022A00000F5805008B5FFF76DFCC06D00F03CFAFA +:1022B000FFF76EFC43090CBF0120002008BD0000A1 +:1022C000F8B51D46002313700F4606461446FFF767 +:1022D000E7FF80F00100387025B129463046FFF74E +:1022E000B3FF2070F8BD00002DE9B8410C4615463B +:1022F0001F46804600F080FB0B462178024609B954 +:10230000287850B14046FFF769FFFFF793FF3B463F +:102310002A462146FFF7D4FF0120BDE8B88100001E +:1023200010B5FFF72FFC174B9A6D42F000729A65BB +:102330009A6B42F000729A639A6B00F5805422F017 +:1023400000729A63FFF724FC94F8C830DB0718D4B6 +:10235000B9B102211320FFF715FC01F047FC02215F +:10236000142001F043FC0221152001F03FFC94F8F9 +:10237000C83043F0010384F8C830BDE81040FFF7CF +:1023800007BC10BD001002402DE9F04700F5805554 +:1023900088B095F8C930012B04468846164600F2ED +:1023A000FB807E4F57F823200AB947F82300D7F85F +:1023B00000A0C4F80C802674BAF1000F09D141F2D4 +:1023C000D00002F00BFD51468146FFF74DFDC7F8E6 +:1023D000009095F8C930012B5DD001212046FFF710 +:1023E0009FFFFFF7CFFB6269136823F002031360BE +:1023F0006269136843F001031360636900275F613A +:1024000001212046FFF7C4FBFFF7ECFD002800F098 +:102410008480E86D00F076F904F58359BA4609F135 +:102420000809202200216846FFF722FB02A8FFF7D7 +:1024300077FCCDF818A06A4609EB07030DF1180EDA +:102440009446BCE80300F44518605960624603F105 +:102450000803F5D1DCF80000186020379CF8042050 +:102460001A71602FDDD195F8C8306FF38203002711 +:1024700085F8C8306A4641462046ADF80070ADF890 +:1024800002708DF80470FFF751FD6369C0B94FF415 +:1024900000421A6011E0386803685B6B9847014698 +:1024A00000289AD13868FFF73BFF38680368324646 +:1024B0005B684146984700288FD108B0BDE8F08797 +:1024C00061221A609DF802309DF803201B06120459 +:1024D00002F4702203F040731343BDF80020C2F3EE +:1024E000090213439DF804201205022E02F4E002B3 +:1024F0000CBF4FF000410021134362690B43D361CD +:10250000636913225A616269136823F0010313603F +:1025100039462046FFF766FD08B96369B7E795F8C5 +:10252000C93093BB6169D1F8002242F00102C1F8C1 +:1025300000226169D1F8002222F47C5222F00E02BE +:10254000C1F800226169D1F8002242F46062C1F84A +:1025500000226269C2F814326269C2F80432626908 +:1025600041F6FF71C2F80C126269C2F8403262692A +:10257000C2F8443263690122C3F81C226269D2F8AE +:10258000003223F00103C2F8003295F8C83043F05E +:10259000020385F8C83090E700208EE7F834002069 +:1025A00008B500F029FA50EA0103024602D0421EA3 +:1025B00061F10001044B186810B10B46FFF748FDAC +:1025C000BDE8084001F06CB9F834002008B50020DF +:1025D000FFF7ECFDBDE8084001F062B908B5012045 +:1025E000FFF7E4FDBDE8084001F05AB90FB4002040 +:1025F00004B0704713B56C4684E80600031D94E8E8 +:10260000030083E80500012002B010BD73B58568A2 +:10261000019155B11B885B0707D4D0E90036DB6B0D +:102620009847019AC1B23046A847012002B070BD58 +:10263000F0B5866889B005460C465EB1BDF8383005 +:102640005B070AD4D0E90037DB6B98472246C1B25A +:102650003846B047012009B0F0BD00220023CDE983 +:1026600000230023ADF808300A4603AB01F1080649 +:10267000106851681C4603C40832B2422346F7D1A1 +:10268000106820609288A28000F0B8F90423ADF8A9 +:1026900008302B68CDE900011B6C69462846984735 +:1026A000D8E7000030B503680968DD0FB5EBD17FCE +:1026B00023F0604421F060424FEAD1700BD0002B30 +:1026C000B8BFA40C0029B8BF920C944202D034BF0A +:1026D0000120002030BD944205D1C1F38070C3F3C6 +:1026E00080738342F6D194422CBF00200120F1E791 +:1026F00010B5037C044613B9006802F0A7FB20461E +:1027000010BD00000023BFF35B8FC360BFF35B8F7E +:10271000BFF35B8F8360BFF35B8F7047BFF35B8F4B +:102720000068BFF35B8F704770B505460C30FFF74C +:10273000F5FF05F1080604463046FFF7EFFFA0421B +:1027400006D930466D68FFF7E9FF2544281A70BDA9 +:102750003046FFF7E3FF201AF9E7000070B50546A1 +:10276000406898B105F10800FFF7D8FF05F10C06A5 +:1027700004463046FFF7D2FF8442304694BF6D686E +:102780000025FFF7CBFF013C2C44201A70BD000050 +:1027900038B50C460546FFF7C7FFA04210D305F138 +:1027A0000800FFF7BBFF04446868B4FBF0F100FBCE +:1027B0001144BFF35B8F0120AC60BFF35B8F38BD6A +:1027C0000020FCE72DE9F041144607460D46FFF7CF +:1027D000C5FF844228BF0446D4B1B84658F80C6BF4 +:1027E0004046FFF79BFF3044286040467E68FFF775 +:1027F00095FF331A9C4203D86C600120BDE8F0813C +:102800006B60A41B3B68AB602044E8600220F5E7E6 +:102810002046F3E738B50C460546FFF79FFFA04278 +:1028200010D305F10C00FFF779FF04446868B4FB8E +:10283000F0F100FB1144BFF35B8F0120EC60BFF3AC +:102840005B8F38BD0020FCE72DE9FF4188466946D3 +:102850000746FFF7B7FF6C4606B204EBC606002535 +:10286000B44209D06268206808EB0501FFF7EEF872 +:10287000636808341D44F3E729463846FFF7CAFF6A +:10288000284604B0BDE8F081F8B505460C300F4687 +:10289000FFF744FF05F1080604463046FFF73EFF08 +:1028A000A042304688BF6C68FFF738FF201A3860B6 +:1028B00020B130462C68FFF731FF2044F8BD0000FE +:1028C00073B5144606460D46FFF72EFF844228BF17 +:1028D00004460190DCB101A93046FFF7D5FF019B0A +:1028E00033B93268C5E90233C5E9002401200CE0A0 +:1028F0009C4238BF01942860019868608442F5D9F1 +:102900003368AB60241AEC60022002B070BD204630 +:10291000FBE700002DE9FF410F466946FFF7D0FFB6 +:102920006C4600B204EBC0050026AC4209D0D4F8D6 +:10293000048054F8081BB8194246FFF787F846444C +:10294000F3E7304604B0BDE8F081000038B5054635 +:10295000FFF7E0FF044601462846FFF719FF20462F +:1029600038BD000000B59BB0EFF3098168226846CE +:10297000FFF76CF8EFF30583044B9A6BDA6A9A6AF7 +:102980009A6A9A6A9A6A9A6A9B6AFEE700ED00E080 +:1029900000B59BB0EFF3098168226846FFF756F84F +:1029A000EFF30583044B9A6B9A6A9A6A9A6A9A6A59 +:1029B0009A6A9B6AFEE700BF00ED00E000B59BB09D +:1029C000EFF3098168226846FFF740F8EFF30583CB +:1029D000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE7EA +:1029E00000ED00E0FEE700000FB408B5029801F02A +:1029F000AFFBFEE701F03EBE01F016BE01F014BED3 +:102A000030B5094D0A4491420DD011F8013B5840B0 +:102A1000082340F30004013B2C4013F0FF0384EA39 +:102A20005000F6D1EFE730BD2083B8ED2DE9F0413D +:102A3000C56915B9C161BDE8F0814B6823F06047F5 +:102A4000C3F38A464FEAD37EC3F3807816EA23069F +:102A500038BF3E46AC462B465A68BEEBD27F22F0CA +:102A600060440AD0002A18DAA40CB44217D19D425F +:102A70000FD10D60DEE71346EEE7A74207D102F063 +:102A80008044C2F3807242450BD054B1EFE708D2C4 +:102A9000EDE7CCF800100B60CDE7B44201D0B442B2 +:102AA000E5D81A689C46002AE5D11960C3E7000002 +:102AB0002DE9F047089D01F007044FEAD5082244AC +:102AC00005F0070500EBD1004FF47F49944201D196 +:102AD000BDE8F08704F0070705F0070A57453E46B2 +:102AE00038BF5646C6F10806111B8E4228BF0E4657 +:102AF000E10808EBD50E415C13F80EC0B94029FA85 +:102B000006F721FA0AF1FFB28CEA010147FA0AF747 +:102B100039408CEA010C03F80EC034443544D5E743 +:102B200080EA0120082341F2210201B2400000297D +:102B300080B203F1FF33B8BF504013F0FF03F4D16C +:102B40007047000038B50C468D18A54200D138BD3D +:102B500014F8011BFFF7E4FFF7E7000002684AB131 +:102B600013680360C388018901339BB29942C38013 +:102B700038BF03811046704770B588B020220446E4 +:102B80000D4668460021FEF773FF20460495FFF7C7 +:102B9000E5FF024658B16B46054608AE1C4603CC1D +:102BA000B44228606960234605F10805F6D1104655 +:102BB00008B070BD082817D909280CD00A280CD0F5 +:102BC0000B280CD00C280CD00D280CD00E2814BFCC +:102BD0004020302070470C20704710207047142090 +:102BE000704718207047202070470000082817D928 +:102BF0000C280CD910280CD914280CD918280CD959 +:102C000020280CD930288CBF0F200E2070470920B7 +:102C100070470A2070470B2070470C2070470D202A +:102C20007047000010B54B6823B9CA8A63F30902E4 +:102C3000CA8210BDC4681A681C60C360438A013B25 +:102C400043824A60EFE700002DE9F84F1D46CB8A2A +:102C50000F46C3F309010629814692460B4630D040 +:102C60000020AAB207F119049EB2052E1FFA80F8BF +:102C70000FD8904503F1010306D3FB8A0A4462F39F +:102C80000903FB8201201AE01AF80060E6540130C3 +:102C9000EAE79045F1D2A1F1060B1C237C68BBFB4F +:102CA000F3F203FB12BB1FFA8BF6002C45D148460A +:102CB000FFF754FF044638B978606FF00200BDE8B2 +:102CC000F88F4FF00008E6E7002606607860ADB2A6 +:102CD0004FF0000B454510D90AEB0803221D13F8ED +:102CE000011B9155B1B208F101081B291FFA88F8A0 +:102CF0002BD0454506F10106F1D8FB8AC3F3090242 +:102D0000154465F30903BCE7013292B21C462368FF +:102D1000002BF9D1AB1F0B441C21B3FBF1F30133A2 +:102D20009BB29A42D3D2BBF1000FD0D14846FFF7F5 +:102D300015FF20B9C4F800B0BFE70122E7E7C0F8EB +:102D400000B05E4620600446C1E74545D5D94846F7 +:102D5000FFF704FF08B92060AFE7C0F800B0002615 +:102D600020600446B6E700002DE9F04F2DED028B00 +:102D700083B0CDE90013BDF83C5007469146002AC8 +:102D800000F092802DB10E9B002B00F08D80072D5E +:102D900032D807F10C00FFF7E1FE044638B96FF0B6 +:102DA0000204204603B0BDEC028BBDE8F08F142274 +:102DB0000021FEF75DFE0E992A4604F10800FEF799 +:102DC00045FE681CC0B2FFF711FFFFF7F3FE207449 +:102DD00099F80030013814FA80F003F01F0363F013 +:102DE0003F030372009B43F0004161603846214677 +:102DF000FFF71CFE0124D4E700F10C034FF000089C +:102E000008EE103A4FF0800A4646444618EE100A83 +:102E1000FFF7A4FE83460028C1D014220021FEF74C +:102E200027FEC6BB019BABF8083002200E9B00F1C9 +:102E3000080299195BFA82F20130C0B2082801D069 +:102E4000AE422AD3FFF7D2FEFFF7B4FE99F8002076 +:102E5000009B411E02F01F0242EA4812AE4208BF28 +:102E60004FF0400A5BFA81F14AEA020A43F000425D +:102E700081F808A08BF81000CBF80420594638469A +:102E8000FFF7D4FD0134AE4224B288F001084FF0C0 +:102E9000000ABBD185E70020C8E711F801CB02F892 +:102EA00001CB0136B6B2C7E76FF0010479E7000045 +:102EB000F8B515460E462822002104461F46FEF7A7 +:102EC000D7FD069B6360B5F5001F079BA76034BF65 +:102ED0006A094FF6FF72236204F10C0097B20023D7 +:102EE0009A4205D80023036027826382A382F8BD3B +:102EF0000660013330462036F2E7000003781BB944 +:102F00004BB2002BC8BF01707047000000787047BB +:102F10002DE9F74FDDF83C90BDF830500D9E9DF83F +:102F20003840BDF84070804692469B46B9F1000F8C +:102F300001D1002F51D11F2C4FD898F80000B0B903 +:102F4000072F47D835F0030347D13A4649464FF695 +:102F5000FF70FFF7F7FD20F001002D02400445EA65 +:102F60000464400C44EA40244FF6FF7321E040EA39 +:102F70000520072F40EA0464F6D900254FF6FF73B9 +:102F8000C5F12000A5F120022AFA05F10BFA00F0A4 +:102F900001432BFA02F211431846C9B2FFF7C0FDF4 +:102FA0000835402D0346EBD13A464946FFF7CAFDA6 +:102FB0000346CDE90097324621464046FFF7D4FE4E +:102FC00033780133DBB21F2B88BF0023337003B08B +:102FD000BDE8F08F6FF00300F9E76FF00100F6E74E +:102FE0002DE9F04F85B09246DDF848800F9D9DF8A1 +:102FF00040209DF84490BDF84C7006469B46B8F1C1 +:10300000000F01D1002F48D11F2A46D83378002B5A +:1030100046D00C0244EA02649DF8381044EAC934F0 +:1030200044EA01441C43072F44F0800432D90023B2 +:103030004FF6FF72C3F1200CA3F120002AFA03F12E +:103040000BFA0CFC41EA0C012BFA00F00143C9B267 +:1030500010460393FFF764FD039B0833402B0246A1 +:10306000E8D13A464146FFF76DFD0346CDE90087BA +:103070002A4621463046FFF777FEB9F1010F06D107 +:103080002B780133DBB21F2B88BF00232B7005B0D8 +:10309000BDE8F08F4FF6FF73E8E76FF00100F6E749 +:1030A0006FF00300F3E70000C06900B1043070471F +:1030B000C3691A68C261C2681A60C360438A013B6F +:1030C000438270472DE9F041D0F81880194E14461C +:1030D0001D464146002709B9BDE8F081D1E9022328 +:1030E000A21A65EB0303964277EB03031ED28369B2 +:1030F0008B420DD1FFF796FD83691B688361C3681E +:103100000B60438AC1608169013B43828846E2E7E4 +:10311000FFF788FD0B68C8F80030C3680B60438A6E +:10312000C160013B4382D8F80010D4E788460968A3 +:10313000D1E700BF80841E002DE9F04F8BB00D4613 +:10314000DDF8509014469B468046002800F0198117 +:10315000B9F1000F00F01581531E3F2B00F21181D1 +:10316000012A03D1BBF1000F40F00B810023CDE910 +:103170000833B8F81430B5EBC30F4FEAC30703D3D5 +:1031800000200BB0BDE8F08F2B199F42D8F80C300F +:103190003ABF7F1BFFB227461BB9D8F81030002B6F +:1031A0007AD02F2D4ED8C5F13006B7424FF000032C +:1031B0002CBFF6B23E4600932946D8F8080008AB6B +:1031C0003246FFF775FCA7EB060A35445FFA8AFA28 +:1031D000B8F8143003F10053063BDB000493D8F831 +:1031E0000C3003933021039B13B1BAF1000F2CD1A3 +:1031F000D8F8100040B1BAF1000F05D0009608AB26 +:103200005246691AFFF754FC38B2002FB8D066074F +:103210000AD00AAB03EBD401624211F8083C02F079 +:103220000702134101F8083C082C3CD9102C40F24D +:10323000B580202C40F2B780BBF1000F00F09C80DD +:10324000082334E0BA460026C2E7049BE02B28BFDF +:10325000E02306930B44AB42059314D95A1B039801 +:103260000096924534BF5246D2B2691A08AB043078 +:103270000792FFF71DFC079A1644AAEB020A1544B1 +:10328000F6B25FFA8AFA049B069A05999B1A049390 +:10329000039B1B680393A6E70093D8F8080008ABCC +:1032A0003A462946AEE7BBF1000F13D00123B4EB39 +:1032B000C30F6CD0082C12D89DF82030621E23FA60 +:1032C00002F2D50706D54FF0FF3202FA04F4234389 +:1032D0008DF820309DF8203089F8003051E7102C0F +:1032E00012D8BDF82030621E23FA02F2D10706D5AB +:1032F0004FF0FF3202FA04F42343ADF82030BDF85A +:103300002030A9F800303CE7202C0FD80899631E24 +:1033100021FA03F3DA0705D54FF0FF3202FA04F47D +:103320000C430894089BC9F800302AE7402C2BD0A6 +:10333000DDE90865611EC4F12102A4F1210326FA2A +:1033400001F105FA02F225FA03F311431943CB0701 +:1033500012D50122A4F12003C4F1200102FA03F3E3 +:1033600022FA01F1A240524243EA010363EB430314 +:1033700032432B43CDE90823DDE90823C9E90023C3 +:10338000FFE66FF00100FCE66FF00800F9E6082C9C +:10339000A0D9102CB3D9202CEED8C3E7BBF1000F75 +:1033A000ADD0022383E7BBF1000FBBD004237EE73F +:1033B00030B5012A144638BF0124402C85B028BFFF +:1033C00040240025012ACDE9025518D81B788DF834 +:1033D000083063070AD004AB03EBD405624215F84A +:1033E000083C02F00702934005F8083C00910346B0 +:1033F0002246002102A8FFF75BFB05B030BD082A7A +:10340000E4D9102A03D81B88ADF80830E1E7202A58 +:103410008DBFD3E900231B680293CDE90223D8E7CF +:1034200010B5CB681BB98B600B618B8210BDC46873 +:103430001A681C60C360438A013B4382CA60F0E79C +:103440002DE9F04FD1F8008093B018F0800FCDE94E +:103450000323C8F3C01219BFC8F3C03BC8F3062644 +:103460004FF0020B1646B8F1000F04460D4680F2ED +:10347000D18118F0C043059340F0CC810B7B002B29 +:1034800000F0C881BBF1020F03D00178B14240F0D7 +:10349000C48108F07F0106916AB3C8F3074A2B4440 +:1034A000069A93F80390760646EA0B4646EA824669 +:1034B0005FEAD91346EA0A06079300F090800022DB +:1034C0000023CDE90A23069B009367685B465246BA +:1034D0000AA92046B84700287ED0A7699FB931467F +:1034E00004F10C00FFF748FB0746E0B96FF002005B +:1034F00013B0BDE8F08FC8F30F2A18F07F0F08BF94 +:103500000AF0030ACBE73B699E420DD03F68002FCB +:10351000F9D1314604F10C00FFF72EFB07460028D5 +:10352000E4D0A3693B60A761DDE90A2300264FF6DA +:10353000FF70C6F1200E22FA06F103FA0EFEA6F184 +:10354000200C23FA0CFC41EA0E0141EA0C01C9B23D +:10355000083609920893FFF7E3FA402EDDE90832B6 +:10356000E7D1B882FB7D09F01F06C3F384039B1BE0 +:10357000D7E9022198B2002BBCBF00F120031BB297 +:1035800052EA0100C8F304680FD00398821A049825 +:1035900060EB0101A74890424FF000028A4104D33A +:1035A000079A002A5BD0012B23DDFA7D4FEA8903BD +:1035B00002F0030203F07C031343FB7539462046F7 +:1035C000FFF730FB079BA3B9FB7DC3F384020132F5 +:1035D00062F38603FB7504E06FF00B0088E7A769D0 +:1035E00017B96FF00C0083E73B699E42BAD03F6881 +:1035F000F6E719F0400F32D0039BBB60049BFB60E1 +:10360000142200210DA8FEF733FA039B0A93049BB2 +:103610000B932B1D0C932B7BADF83EA0013BDBB233 +:10362000ADF83C30069B8DF8433094F824308DF88B +:1036300040B083F001038DF844308DF84160A368F9 +:103640008DF842800AA920469847FB7DC3F3840386 +:10365000013303F01F039B02FB82002048E7FB7D40 +:10366000C9F34012B2EBD31F40F0DA80C3F38403F6 +:10367000B34240F0D88007992B7B4FEA991200297A +:1036800034D0D20741D4032B40F2D080039BBB60DF +:10369000049BFB602B7BAE1D033BDBB232463946FD +:1036A00004F10C00FFF7D0FA00280DDA2046394665 +:1036B000FFF7B8FAFB7DC3F38403013303F01F0364 +:1036C0009B02FB82032013E7AB883B832A7B033AF0 +:1036D000B88AD2B23146FFF735FAFB7DB882DA43B9 +:1036E000C2F3C01262F3C713FB75B6E76AB92E1DA9 +:1036F000013BDBB23246394604F10C00FFF7A4FA75 +:103700000028D3DB2A7B013AE2E7F98AC1F30901F9 +:10371000013B0529DAB259D8281D002307F11A0CFC +:103720009A4208D910F801EB0CF801E0013101339D +:103730000629DBB2F4D103990A9104990B919342C3 +:1037400007F11A010C9138BF043379680D9134BF29 +:1037500055FA83F300230E93FB8AADF83EA0C3F322 +:1037600009031A44069B8DF8433094F82430ADF8D1 +:103770003C2083F001038DF8443000238DF840B0E5 +:103780008DF841608DF842807B602A7BB88A013ACF +:10379000291DFFF7D7F93B8BB882834203D1A36879 +:1037A0000AA92046984720460AA9FFF739FEFB7D63 +:1037B000B88AC3F38403013303F01F039B02FB8227 +:1037C0003B8B984214BF1120002091E67B68002BB0 +:1037D000B1D0062001E01C306346D3F800C0BCF134 +:1037E000000FF8D1091A081D05F1040C00EB0309BC +:1037F00005989DF8143001EB000EBEF11B0F9AD80E +:103800009A4298D91CF8013B09F8013B059B01330A +:103810000593EDE76FF009006AE66FF00A0067E6CE +:103820006FF00D0064E66FF00E0061E66FF00F00C0 +:103830005EE600BF80841E00EFF3098305494A6BF2 +:1038400022F001024A63683383F30988002383F37B +:103850001188704700EF00E0202080F3118862B6E5 +:103860000C4B0D4AD96821F4E0610904090C0A43A4 +:10387000DA60D3F8FC20094942F08072C3F8FC20DA +:103880000A6842F001020A601022DA7783F8220007 +:10389000704700BF00ED00E00003FA05001000E0F3 +:1038A00010B5202383F311880E4B5B6813F400637B +:1038B00014D0F1EE103AEFF30984683C4FF08073B6 +:1038C000E361094BDB6B236684F3098800F0F2FBAC +:1038D00010B1064BA36110BD054BFBE783F31188C4 +:1038E000F9E700BF00ED00E000EF00E00B06000884 +:1038F0000E060008026843681143016003B11847CF +:1039000070470000024A136843F0C0031360704719 +:1039100000380140024A136843F0C0031360704747 +:1039200000440040024A136843F0C003136070472C +:103930000048004037B5244C244D204600F0FCFAE6 +:10394000009404F1140022490023202200F0BEF963 +:1039500020490094202204F138001F4B00F038FA6F +:103960001E4BC4E917351E4C204600F0E5FA0094C2 +:1039700004F114001B490023202200F0A7F91A4982 +:103980000094202204F13800184B00F021FA184B63 +:10399000C4E91735174C204600F0CEFA04F11400A4 +:1039A000154900940023202200F090F9134B14498C +:1039B0000094202204F1380000F00AFA114BC4E907 +:1039C000173503B030BD00BF0435002000B4C40477 +:1039D00048360020A836002005390008003801408C +:1039E0007035002068360020C836002015390008E0 +:1039F00000440040DC3500208836002025390008CE +:103A0000E83600200048004030B5037C304C0029E7 +:103A100018BF0C46012B0FD12E4B984239D12E4B9B +:103A20001A6E42F480421A66D3F8802042F4804233 +:103A3000C3F88020D3F880302268036EC16D84669D +:103A400003EB5203B3FBF2F36268150442BF23F0A9 +:103A5000070503F0070343EA4503CB60A36843F07F +:103A600040034B60E36843F001038B6042F49673BC +:103A700043F001030B604FF0FF330B62510505D596 +:103A800012F010221FD0B2F1805F1ED080F8643097 +:103A900030BD124B98420BD0114B9842CCD10E4BFB +:103AA0009A6D42F480229A659A6F42F4802207E070 +:103AB000094B9A6D42F400329A659A6F42F40032D3 +:103AC0009A679B6FB8E77F23E0E73F23DEE700BFFD +:103AD00028520008043500200010024070350020F4 +:103AE000DC3500202DE9F047C66D3768F469346293 +:103AF0002107054618D014F0080118BF8021E207FD +:103B000048BF41F02001A30748BF41F040016007D2 +:103B100048BF41F48071202383F31188281DFFF7EB +:103B2000E9FE002383F31188E2050AD5202383F3FD +:103B300011884FF40071281DFFF7DCFE002383F38A +:103B400011884FF020094FF0000A14F0200838D1F6 +:103B50003B0616D54FF0200905F1380A200610D58E +:103B600089F31188504600F067F9002836DA0821F9 +:103B7000281DFFF7BFFE27F080033360002383F387 +:103B80001188790614D5620612D5202383F3118893 +:103B9000D5E913239A4208D12B6C33B11021281D8B +:103BA00027F04007FFF7A6FE3760002383F3118854 +:103BB000E30619D5AA6E1369B3B1BDE8F0475069A1 +:103BC000184789F31188B38C95F86410284619407A +:103BD00000F0CCF98AF31188F469B6E780B2308539 +:103BE00088F31188F469B9E7BDE8F08700F1604314 +:103BF00003F561430901C9B283F80013012200F003 +:103C00001F039A4043099B0003F1604303F561439E +:103C1000C3F880211A607047F8B5154682680669B6 +:103C2000AA420B46816938BF8568761AB5420446B8 +:103C30000BD218462A46FDF709FFA3692B44A3615E +:103C4000A3685B1BA3602846F8BD0CD91846324612 +:103C5000FDF7FCFEAF1BE1683A463044FDF7F6FE87 +:103C6000E3683B44EBE718462A46FDF7EFFEE368BE +:103C7000E5E7000083689342F7B51546044638BF70 +:103C80008568D0E90460361AB5420BD22A46FDF7A2 +:103C9000DDFE63692B446361A36828465B1BA36058 +:103CA00003B0F0BD0DD932460191FDF7CFFE019969 +:103CB000E068AF1B3A463144FDF7C8FEE3683B4479 +:103CC000E9E72A46FDF7C2FEE368E4E710B50A44D7 +:103CD0000024C361029B8460C0E90000C0E90511B3 +:103CE000C1600261036210BD08B5D0E9053293429C +:103CF00001D1826882B98268013282605A1C4261B5 +:103D00001970D0E904329A4224BFC368436100218C +:103D100000F0A0FA002008BD4FF0FF30FBE70000E4 +:103D200070B5202304460E4683F31188A568A5B11B +:103D3000A368A269013BA360531CA36115782269A3 +:103D4000934224BFE368A361E3690BB1204698471F +:103D5000002383F31188284607E03146204600F00F +:103D600069FA0028E2DA85F3118870BD2DE9F74F72 +:103D700004460E4617469846D0F81C904FF0200A8D +:103D80008AF311884FF0000B154665B12A4631467B +:103D90002046FFF741FF034660B94146204600F048 +:103DA00049FA0028F1D0002383F31188781B03B06F +:103DB000BDE8F08FB9F1000F03D001902046C8474D +:103DC000019B8BF31188ED1A1E448AF31188DCE7FE +:103DD000C0E90511C160C3611144009B8260C0E964 +:103DE0000000016103627047F8B504460D461646AF +:103DF000202383F31188A768A7B1A368013BA360C0 +:103E000063695A1C62611D70D4E904329A4224BF6E +:103E1000E3686361E3690BB120469847002080F3B3 +:103E2000118807E03146204600F004FA0028E2DA63 +:103E300087F31188F8BD0000D0E905239A4210B538 +:103E400001D182687AB98268013282605A1C82612B +:103E50001C7803699A4224BFC3688361002100F083 +:103E6000F9F9204610BD4FF0FF30FBE72DE9F74F81 +:103E700004460E4617469846D0F81C904FF0200A8C +:103E80008AF311884FF0000B154665B12A4631467A +:103E90002046FFF7EFFE034660B94146204600F09A +:103EA000C9F90028F1D0002383F31188781B03B0EF +:103EB000BDE8F08FB9F1000F03D001902046C8474C +:103EC000019B8BF31188ED1A1E448AF31188DCE7FD +:103ED000026843681143016003B11847704700004E +:103EE0001430FFF743BF00004FF0FF331430FFF7EB +:103EF0003DBF00003830FFF7B9BF00004FF0FF337F +:103F00003830FFF7B3BF00001430FFF709BF0000DF +:103F10004FF0FF311430FFF703BF00003830FFF7D8 +:103F200063BF00004FF0FF323830FFF75DBF000085 +:103F300000207047FFF7FEBC044B03600023C0E97C +:103F40000233436001230374704700BF40520008EE +:103F500010B52023044683F31188FFF755FD022393 +:103F60002374002383F3118810BD000038B5C369A2 +:103F700004460D461BB904210844FFF7A9FF294652 +:103F800004F11400FFF7B0FE002806DA201D4FF4FC +:103F90008061BDE83840FFF79BBF38BD024B00226F +:103FA000C3E900339A607047083700200023037488 +:103FB0008268054B1B6899689142FBD25A6803607E +:103FC00042601060586070470837002008B5202311 +:103FD00083F31188037C032B05D0042B0DD02BB960 +:103FE00083F3118808BD436900221A604FF0FF3344 +:103FF0004361FFF7DBFF0023F2E7D0E900321360F3 +:104000005A60F3E7002303748268054B1B689968C4 +:104010009142FBD85A680360426010605860704754 +:1040200008370020054B19690874186802681A607F +:104030005360186101230374FCF7D2BA08370020DB +:1040400030B54B1C0B4D87B0044610D02B690A4A83 +:1040500001A800F019F92046FFF7E4FF049B13B113 +:1040600001A800F04DF92B69586907B030BDFFF782 +:10407000D9FFF8E708370020CD3F000838B50C4DD0 +:1040800041612B6981689A689142044603D8BDE872 +:104090003840FFF78BBF1846FFF7B4FF01232C61B0 +:1040A000014623742046BDE83840FCF799BA00BFAA +:1040B00008370020044B1A681B6990689B68984277 +:1040C00094BF0020012070470837002010B5084C2D +:1040D000236820691A6822605460012223611A74DF +:1040E000FFF790FF01462069BDE81040FCF778BA61 +:1040F0000837002008B5FFF7DDFF18B1BDE808401C +:10410000FFF7E4BF08BD0000FFF7E0BFFEE70000D7 +:1041100010B50C4CFFF742FF00F0A8F80A498022C6 +:10412000204600F03DF8012344F8180C0374FFF713 +:1041300093FB002383F3118862B60448BDE8104066 +:1041400000F04EB83037002068520008785200085E +:1041500008B572B6034B586200F060FA00F00CFB31 +:10416000FEE700BF0837002000F004B9EFF311802C +:1041700020B9EFF30583202282F3118870470000F5 +:1041800010B530B9EFF30584C4F3080414B180F31B +:10419000118810BDFFF7AEFF84F31188F9E7000026 +:1041A00082600222028270478368A3F17C0243F896 +:1041B0000C2C026943F83C2C426943F8382C074A1E +:1041C00043F81C2CC26843F8102C022203F8082C78 +:1041D000002203F8072CA3F118007047F905000826 +:1041E00010B5202383F31188FFF7DEFF002104467A +:1041F000FFF744FF002383F31188204610BD000021 +:10420000024B1B6958610F20FFF70CBF08370020D5 +:10421000202383F31188FFF7F3BF000008B50146A0 +:10422000202383F311880820FFF70AFF002383F37C +:10423000118808BD49B1064B42681B6918605A6075 +:10424000136043600420FFF7FBBE4FF0FF30704760 +:10425000083700200368984206D01A680260506050 +:1042600059611846FFF7A2BE70470000054B03F1E5 +:104270001402C3E905224FF0FF310022C3E90712FF +:10428000704700BF0837002070B51C4EC0E90323FB +:1042900005460C4600F0EAFA334653F8142F9A42CA +:1042A0000DD13062C5E901242A600A2C2CBF001907 +:1042B0000A30C6E90555BDE8704000F0C5BA316A5C +:1042C000431AE31838BF1C469368A34202D9081961 +:1042D00000F0C8FA73699A6894420CD85A68AC60C6 +:1042E0002B606A6015609A685D60121B9A604FF0DF +:1042F000FF33F36170BD1B68A41AECE70837002098 +:1043000038B51B4C636998420DD0D0E90032136078 +:104310005A6000228168C2609A680A449A604FF02D +:10432000FF33E36138BD2246036842F8143F0021A1 +:1043300093425A60C16003D1BDE8384000F08CBAA6 +:104340009A688168256A0A449A6000F08FFA636966 +:104350009A68411B8A42E5D9AB181D1A092D206ABB +:1043600098BF01F10A02BDE83840104400F07ABA63 +:10437000083700202DE9F041184C002704F11406FD +:10438000656900F073FA236AAA68C11A8A4215D8CF +:1043900013442362D5E9003213605A606369D5F88B +:1043A0000C80EF60B34201D100F056FA87F3118818 +:1043B0002869C047202383F31188E1E76169B1428E +:1043C00009D013441B1ABDE8F0410A2B2CBFC018BA +:1043D0000A3000F047BABDE8F08100BF083700207E +:1043E0000C2303604FF0FF3070470000002070473F +:1043F000FEE70000704700004FF0FF3070470000FC +:10440000BFF34F8F024A1369DB03FCD4704700BF30 +:104410000020024008B5094B1B7873B9FFF7F0FF85 +:10442000074B5A69002ABFBF064A9A6002F18832D8 +:104430009A601A6822F480621A6008BD90380020E1 +:10444000002002402301674508B50B4B1B7893B948 +:10445000FFF7D6FF094B5A6942F000425A611A68C9 +:1044600042F480521A601A6822F480521A601A6864 +:1044700042F480621A6008BD90380020002002409B +:104480007F289ABF00F58030C002002070470000EE +:104490004FF4006070470000802070477F2808B507 +:1044A0000BD8FFF7EDFF00F500630268013204D17D +:1044B00004308342F9D1012008BD0020FCE7000050 +:1044C0007F2810B504461FD8FFF79AFFFFF7A2FF19 +:1044D0000E4BF3221A6102225A615A6942EAC00263 +:1044E0005A615A6942F480325A61FFF789FF4FF4EA +:1044F0000061FFF7C5FF00F03BF9FFF7A5FF20467D +:10450000BDE81040FFF7CABF002010BD00200240E8 +:104510002DE9F84340EA020313F00703144606D0DE +:104520002E4B40F2F5221A600020BDE8F883851872 +:104530002B4A95420CD9294A40F2FA211160F3E73F +:10454000031D1B684A68934208D1083C08300831B3 +:10455000072C14D902680B689A42F1D0FFF75AFF72 +:10456000FFF74EFF1F4E08314FF001084FF00009D2 +:10457000012CA1F1080706D8FFF766FF01E0002C27 +:10458000ECD10120D1E7C6F81480054651F8083C6B +:1045900045F8043B51F8043C4360FFF731FFC6F88F +:1045A0001490026851F8083C9A420CD00B4B4FF41F +:1045B00048721A600C4B18600C4B1C600C4B1F604F +:1045C000FFF742FFB0E72D6851F8043C9D4201F12E +:1045D0000801EBD1083C0830CAE700BF8C38002046 +:1045E00000000408002002408038002088380020A5 +:1045F00084380020084908B50B7828B11BB9FFF7AB +:1046000009FF01230B7008BD002BFCD0BDE808405A +:104610000870FFF719BF00BF903800204FF48031B9 +:104620004FF0005000F0A6B870B582B0FFF79EFDC5 +:104630000E4E054600F01AF93268904237BF0C4A18 +:104640000B49516814682EBFD1E900410131516016 +:104650000419034641F10001284601913360FFF738 +:104660008FFD0199204602B070BD00BF9438002034 +:104670009838002070B582B0FFF778FD104E0546DF +:1046800000F0F4F83268904237BF0E4A0D49516885 +:1046900014682EBFD1E9004101315160041941F184 +:1046A00000010346284601913360FFF769FD019937 +:1046B0004FF47A7200232046FBF77AFD02B070BDFA +:1046C000943800209838002010B50244064BD2B22E +:1046D000904200D110BD441C00B253F8200041F8B4 +:1046E000040BE0B2F4E700BF50280040114B30B596 +:1046F000D3F89040240409D4D3F89040C3F89040F4 +:10470000D3F8904044F40044C3F890400A4C236826 +:1047100043F4807323600244084BD2B2904200D12C +:1047200030BD441C00B251F8045B43F82050E0B2A5 +:10473000F4E700BF00100240007000405028004025 +:1047400007B5012201A90020FFF7BEFF019803B0C1 +:104750005DF804FB13B50446FFF7F2FFA04205D055 +:10476000012201A900200194FFF7C0FF02B010BD93 +:10477000704700007047000070470000074B45F28B +:1047800055521A6002225A6040F6FF729A604CF647 +:10479000CC421A60024B01221A7070470030004070 +:1047A000A4380020034B1B781BB1034B4AF6AA2206 +:1047B0001A607047A438002000300040054B1A688A +:1047C00032B902F1804202F50432D2F894201A6024 +:1047D000704700BFA0380020024B4FF40002C3F81E +:1047E000942070470010024008B5FFF7E7FF024B26 +:1047F0001868C0F3407008BDA03800207047000062 +:10480000FEE700000A4B0B480B4A90420BD30B4BC0 +:10481000DA1C121AC11E22F003028B4238BF00229A +:104820000021FDF725B953F8041B40F8041BECE701 +:10483000245300083C3900203C3900203C3900203A +:104840007047000000F07AB84FF0804300225863B0 +:104850001A610222DA6070474FF080430022DA606A +:10486000704700004FF08043586370474FF080431B +:10487000586A7047264B2748DA6A42F0070210B49C +:10488000DA62DA6A22F00702DA62DA6ADA6C42F095 +:104890000702DA64DA6E42F00702DA664FF09042FD +:1048A000DB6E4FF0AA31002353604FF4EE44916069 +:1048B000D0604FF6FF7050611462174C5362146061 +:1048C000164CC2F80434C2F80814C2F80C444FF66F +:1048D000F774C2F814444FF0EE44C2F8204447F293 +:1048E0009974C2F824440E4CC2F80044C2F804384B +:1048F000C2F80818C2F80C38C2F81408C2F82038F8 +:10490000C2F82438C2F800385DF8044B00F09CB8B7 +:104910000010024050010024A00100281050500057 +:1049200050A0AA0008B500F035FAFFF7F1FBBDE88A +:104930000840FFF743BF0000704700000F4B9A6D1F +:1049400042F001029A659A6F42F001029A670C4A9E +:104950009B6F936843F0010393604FF080434F22B5 +:104960009A624FF0FF32DA6200229A615A63DA608B +:104970005A6001225A611A60704700BF001002405D +:10498000002004E04FF0804208B51169D3680B4065 +:10499000D9B2C9439B07116107D5202383F311883E +:1049A000FFF7E2FB002383F3118808BD08B5244B11 +:1049B0004FF0FF319A6A99629A6A00229A62986A65 +:1049C000D86A60F00700D862D86A00F00700D862A1 +:1049D000D86A186B1963186B1A63186B986B60F0C0 +:1049E00080509863986B00F080509863986BD86BF8 +:1049F000D963D86BDA63D86B186C1964196C1A64B4 +:104A0000196C996D41F080519965996F41F0805111 +:104A10009967996FD3F8901011F4407F1EBF4FF43F +:104A20008031C3F89010C3F89020D3F89020C3F8D9 +:104A3000902000F07FF9034B00225A6008BD00BFB0 +:104A40000010024000700040394B3A4A9A6501223A +:104A50001A601A689007FCD500229A609A6812F0D2 +:104A60000C0FFBD1344A4FF40071116051694905B4 +:104A7000FCD41A6842F480321A601A689203FCD59A +:104A80002D490A6842F480720A602C4A4FF4E061B2 +:104A900011601A6842F060021A601A6842F0080257 +:104AA0001A601A689007FCD59A6812F00C0FFBD1B7 +:104AB000D3F8942042F4C062C3F89420204ADA600C +:104AC0001A6842F080721A601A689101FCD51D4A7A +:104AD0001A611A6842F080621A601A681201FCD5E5 +:104AE00000229A601549184AC3F888200A6822F003 +:104AF000070242F004020A600A6802F00702042A70 +:104B0000FAD19A6842F003029A609A6802F00C02A5 +:104B10000C2AFAD11A6E42F001021A66D3F88020EC +:104B200042F00102C3F88020D3F88030704700BF04 +:104B300000100240000400100070004000200240FD +:104B400003140001000C100055550134074A08B544 +:104B5000536903F00103536123B1054A13680BB194 +:104B600050689847BDE80840FEF79ABE000401402F +:104B7000A8380020074A08B5536903F002035361BF +:104B800023B1054A93680BB1D0689847BDE8084047 +:104B9000FEF786BE00040140A8380020074A08B589 +:104BA000536903F00403536123B1054A13690BB140 +:104BB00050699847BDE80840FEF772BE0004014006 +:104BC000A8380020074A08B5536903F00803536169 +:104BD00023B1054A93690BB1D0699847BDE80840F5 +:104BE000FEF75EBE00040140A8380020074A08B561 +:104BF000536903F01003536123B1054A136A0BB1E3 +:104C0000506A9847BDE80840FEF74ABE00040140DC +:104C1000A8380020164B10B55C6904F478725A610C +:104C2000A30604D5134A936A0BB1D06A984760066D +:104C300004D5104A136B0BB1506B9847210604D56D +:104C40000C4A936B0BB1D06B9847E20504D5094A27 +:104C5000136C0BB1506C9847A30504D5054A936CAF +:104C60000BB1D06C9847BDE81040FEF719BE00BFED +:104C700000040140A8380020194B10B55C6904F409 +:104C80007C425A61620504D5164A136D0BB1506D12 +:104C90009847230504D5134A936D0BB1D06D9847FF +:104CA000E00404D50F4A136E0BB1506E9847A1046F +:104CB00004D50C4A936E0BB1D06E9847620404D5AC +:104CC000084A136F0BB1506F9847230404D5054A67 +:104CD000936F0BB1D06F9847BDE81040FEF7E0BD71 +:104CE00000040140A838002008B50348FEF7FAFE8A +:104CF000BDE80840FEF7D4BD0435002008B50348E0 +:104D0000FEF7F0FEBDE80840FEF7CABD7035002092 +:104D100008B50348FEF7E6FEBDE80840FEF7C0BD53 +:104D2000DC35002008B5FFF72DFEBDE80840FEF792 +:104D3000B7BD0000062108B50846FEF757FF06215B +:104D40000720FEF753FF06210820FEF74FFF06213C +:104D50000920FEF74BFF06210A20FEF747FF062138 +:104D60001720FEF743FF06212820FEF73FFF07210B +:104D70001C20FEF73BFF0C212520FEF737FF0C21FE +:104D80002620FEF733FFBDE808400C212720FEF760 +:104D90002DBF000008B5FFF709FE00F009F8FFF786 +:104DA000C9F8FFF7C9FDBDE80840FFF74BBD00009B +:104DB0000023054A19460133102BC2E9001102F104 +:104DC0000802F8D1704700BFA83800200B46014602 +:104DD000184600F025B8000000F038B8012838BFA8 +:104DE000012010B50446204600F028F830B900F044 +:104DF00007F808B900F00CF88047F4E710BD000090 +:104E0000024B1868BFF35B8F704700BF2839002042 +:104E100008B5062000F032F90120FFF7E9FA00009A +:104E200010B5054C13462CB10A4601460220AFF3DB +:104E3000008010BD2046FCE700000000024B014648 +:104E4000186800F089B800BF28110020024B014605 +:104E5000186800F035B800BF2811002010B50139DE +:104E60000244904201D1002005E0037811F8014F7F +:104E7000A34201D0181B10BD0130F2E72DE9F0412B +:104E8000A3B1C91A17780144044603F1FF3C8C42D0 +:104E9000204601D9002009E00578BD4204F1010453 +:104EA000F5D10CEB0405D618A54201D1BDE8F0817F +:104EB00015F8018D16F801EDF045F5D0E7E7000093 +:104EC00037B5002944D051F8043C0190002BA1F1E2 +:104ED0000404B8BFE41800F0F5F81E4A01981368FE +:104EE00033B96360146003B0BDE8304000F0F0B83F +:104EF000A34208D9256861198B4201BF19685B6814 +:104F000049192160EDE71A465B680BB1A342FAD953 +:104F100011685518A5420BD1246821445418A342A6 +:104F20001160E0D11C685B68536021441160DAE7CE +:104F300002D90C230360D6E7256861198B4204BFB0 +:104F400019685B68636004BF491921605460CAE74F +:104F500003B030BD2C390020F8B5CD1C25F0030579 +:104F600008350C2D38BF0C25002D064601DBA94263 +:104F700003D90C2333600020F8BD00F0A3F82149C9 +:104F80000A6814469CB9204F3B6823B92146304635 +:104F900000F03CF838602946304600F037F8431CF2 +:104FA00023D10C233360304600F092F8E3E7236806 +:104FB0005B1B17D40B2B03D923601C44256004E032 +:104FC0006368A2420CBF0B605360304600F080F86B +:104FD00004F10B00231D20F00700C21ACCD01B1ACD +:104FE000A350C9E722466468CCE7C41C24F003043C +:104FF000A042E3D0211A304600F008F80130DDD19C +:10500000CFE700BF2C3900203039002038B5064DDD +:105010000023044608462B60FFF7E2F9431C02D147 +:105020002B6803B1236038BD343900201F2938B5FF +:1050300004460D4604D9162303604FF0FF3038BDF7 +:10504000426C12B152F821304BB9204600F030F8D2 +:105050002A4601462046BDE8384000F017B8012B2B +:105060000AD0591C03D1162303600120E7E700246E +:1050700042F82540284698470020E0E7024B0146C9 +:105080001868FFF7D3BF00BF2811002038B5074DBF +:1050900000230446084611462B60FFF7ADF9431C78 +:1050A00002D12B6803B1236038BD00BF3439002022 +:1050B000FFF79CB9034611F8012B03F8012B002AD6 +:1050C000F9D17047014800F009B800BF3839002015 +:1050D000014800F005B800BF3839002070470000D3 +:1050E000704700006F72672E6172647570696C6F33 +:1050F000742E4D6174656B4C3433312D416972737C +:1051000070656564000000004E6F20617070207350 +:1051100069670A00426164206677206C656E677477 +:10512000682025750A0042616420626F6172645FC5 +:1051300069642025752073686F756C642062652032 +:1051400025750A0042616420667720646573637286 +:105150006970746F72206C656E6774682025750ABB +:1051600000426164206170702043524320307825F2 +:105170003038783A30782530387820307825303813 +:10518000783A3078253038780A00476F6F642066A7 +:1051900069726D776172650A0040A2E4F1646891FA +:1051A0000600000053544D33324C343F3F000000A2 +:1051B0000435002070350020DC3500204261642079 +:1051C00043414E496661636520696E6465782E00CF +:1051D000800000000080000000008000000000004F +:1051E000000000009D1B000889230008E922000838 +:1051F000AD1B0008E51B0008E11D0008B11B0008FD +:10520000C51B0008B51B0008B91B0008C11B00081E +:10521000BD1B0008091D0008C91B0008F525000872 +:10522000D91B0008DD1C00080096000000000000EB :10523000000000000000000000000000000000006E -:10524000000000000000000000000000000000005E -:10525000000000000000000000000000000000004E -:10526000000000000000000000000000000000003E -:04527000000000003A +:1052400000000000FD3E0008E93E0008253F000880 +:10525000113F00081D3F0008093F0008F53E000807 +:10526000E13E0008313F00086D61696E00000000FA +:1052700069646C6500000000705200084837002027 +:1052800080380020010000000D41000800000000EF +:1052900050AFFF7F01000000260400000000000066 +:1052A0000060030000000000FE2A0100D20400009C +:1052B000FF00000000000000A45100083F000000B3 +:1052C0002C11002000000000000000000000000081 +:1052D00000000000000000000000000000000000CE +:1052E00000000000000000000000000000000000BE +:1052F00000000000000000000000000000000000AE +:10530000000000000000000000000000000000009D +:10531000000000000000000000000000000000008D +:045320000000000089 :00000001FF diff --git a/Tools/bootloaders/MatekL431-Periph_bl.bin b/Tools/bootloaders/MatekL431-Periph_bl.bin old mode 100644 new mode 100755 index 1d210ebfe10b70effa3faacb6a8977f514607594..e2ecf47f11873212ad73acd401e6fb20091036f7 GIT binary patch delta 4118 zcmYk93sh5Ax`6jSCxOEwBmn}60Xa!P0ucyAG=NF~IXqH|Xgf3Nvf^t^ywxJp>4;@{ z4dQheEvw@01EsyA#g6t`D-hEwbs{>W9kp@`rClvm?yb|-$Y?t!Rhkg;xc@;rGg<5V z_x|_(|9?Nv-aGe?#T@*0eBO* z0({qXTPHu3ftX=n0uWpjUHu7EMqO8~Rxg&lW>(H1LPEEY96*Rc7*f!?S3n-hUIGpRi^;IKNF@q|+%&%{i2g6+ zM#$$B@x&L&iTH0wT0~{cpbw?b;}9$2e?^)ic1mMH@>*#%($znFv6<$t5^qEkBjTfc zB3|ty&qW@QwFZyZzV#0SbI_q=ifQ~a$I6%-jE6~EYz00=uEhQhuORgjHKm;)TP4|4 z>zDx*9&>cOZQd{%Kn?0aNfnu>1`uT+Px6SXW4q*bOW`er#|WX@=7bnTo*ddj|% z_!`;b!CTwr_p==47tXL8&7TOl?#e85HRf~A`(O^gYCw#IyeV6fIT8|c#=%Uhp*mVS zrw#LV{~b5p(m(Z!(ui@bbzXJMZ0QRG%T?G$)N-A)F;Kuoa$*>x5Pub^lc#Mi^}p?w zK=cwnPCw>GL&AEUjYsUO(9W%OG(txaBZ9mP&;m?j*lU4wEwB&BndIwF_eGqsbFmQf zB=iaCSC8ucoieCLzuu1x%2CYA$*6pla$oqqJJOSg!rmQ0=!Yi}n^S3C5+ddC+0mQB z(=8SVP!S@(iT@3i5hmZqKOv&{A|F|rpoEOUM^-23HMya%_kRP$?s^_E5Q0A8OSH?h zh?H?yCk*O3=}joYr^!UZNBAmvC(&9e3@8a9c9BzAr>s#`Q#!1Dwi)qD!bpcnk~OIY zk)-NmYyCs}3e0~PP*_zh^8R?3k~;y+HxiRVqo|o?`lUKU8{KDg36oM=huU1IAlFCD}DM`Uf%&eD{?((RZ(FkMz#R+nJ= z^Bl+2i@Wp2WQ~|_pQgANPVoOGAV_gQ=`gewJ&6p7L$r?4J>QJ^CZb3##D68$X6N zLP%_#V-s7|9X)f}%c*hMGKzoA1?Nna;48#1J0t5voZiMl&C5akppbLjHG}sNml@gt zJ3>_X_^jiKsub6p6`HipX#uuFWB=$s<78@>>iUQ4D`FZcKHe{?iKwRdIKMCjxJCqd z=7U?{D=LJ>fm@_booW7V5Df{XKMkAJwLg$|YRx3} zN3VTiGEO%<342?2+Q%3Twk@1`D;Y>Fz<(j%rk3P5W=)Xp8ko4i;Jqmay}%q66D6Pe$uB z9Mb9bHTd);tk7PIX1UY{L-#q+RC~i*F6{ju^0}t6u>X%}=mx}NoMG?Vfv|THPB&o` zZ~sxayDGMbxo#Wczgphs`D&e%OB=b{ANF>U2JIqsYryBiY`~4C?8^o3Z2@5x7W>2A z=SZ)%8Rrp&PE(Q|c*S?scr#M@>0m#lNnpcX1$@L+0ZNm|!r=m`0@Bi1s|p^|d%;>b zU&G!=(yXhXVuIv--CX=J`Anz5mx)&ghXqlmHzlumwz~?$_jzR0Yzuo!!e$$7E8y0U zW9j#>k91~aWo#W32Fy4SG@{3HMyB#!Z(R@UKYrrc_m4QI^MD) z{@k?a@oK@!VVJ-D^ETiUj_>9*WBff?rys_@B{^Azcq`eMWxzipFN3s@^H~|PGczGK z=9gZ;d=ij&!NF&BVEjDUX)x?&^vUB<`q+_ZAO?s9BtV=#amd0N;WLQ^WI!UI1XuuG z9N-7G<1!!-Py#Gq)W0+&W#hmm3Wx?`03n71KuG4a+H`~0_iuaHHn>ac2!f6outcyWNy;?t<| zzg5S-SI!L8^Ooo}{x(nT->7KlMtPf$IeNe+v!aiDB6bygX<_fDLA>C-^7MfY9y8FT z=<}oh;fAx`4^Z8q2$ zLEk}c=k3C;k#$Bj?jWtke*6N-$uEP2Y|7t;Qymle?HI2i>rFc}qist5VuhGk_#168 zt-?ZHZ9DIwr1S`G=`XkNm&vs0XjQwFj2?fqP?V7XC}S(2@gs^#vI% zRMd~ye+p=5eJ_xnbNfFT$Oo%JQn)ZfbsYT0Zf3XQ;=j75_g{Jtg>i{E>&t;vs$>Cs+W^gArqUBzJpm$QMV- zyT?;}xFiY?_h6Hld<>~OZV>W_}n)SHl$S?jmUKkbb z-C|hQyS~@9M%~+}=X@?WD7XSH=naISRrkAFHRF(c5WOJ#C{^SFObe1j>Gm8*AOs(>yH%otZWC!y7K2iUMhbUWKP98e2{cjl;DGoYZV)@ z;w3XoD-tpAWU$yF-vR6;WtEv=wxx2V_!ZE*$)(Dgm=}Xo-*(p{UBTVN$BBna04c6Y z!$(O*l@4T0l|hv;xY8$5TY&rS^p%KNTm=8}1RR}JCK{h4mufRXPR>fXRmd03%JtTb zkiTMWT5=K^Wt?<%$m^fIT0`DXUBc=;D&rrQU3Up<|ETQFeKK7va`M&WUxL>Vun%-FG2A+G-M`O%g3Vr`TP+#7|PyM zBj$HH#7t@ta~ksB0=H8U(+}wtzzGZjmjD+K1$EaTor3?+ErzqE{)^=L2@(ISYP@4-KOFwmnub{M3ZtES}-ZoOLvomTF0?E1SP-mw3zJKk# z)?RDvwf9p0VGvOrzVd=(mo|0k5HQIaX7V7T!!SourIJIxWgtSwDKkJdU)gC6q2rC4JR*~yr z`&12HeJ37ZGYvxeIICwpvTUu6(KED?^oPf%6?k8V(ZJ==Ko||kPSLS2YV_g(nTul; z`g9#blgY>7CfrKci0oZf&kB9cFEfN3;d#$mt>``=MxBreSm5;OFz43`oc^dX%vH-u z+!qUBt~!>}%UlX9@v{rjC$zvRm$|8hm=ip3h0EMW3vnkT&5mVm94sGLw(MT!-(8To z`hvq;50a6H=-A^6m)8ZkaL}z8HP}2LNWNbqa}h~+KXFGG@N-11$OdXwsESrMB2Hls zHV4-wSlBz=O~hl-7LR?gEo($8V7h8iE3mY|@3^Mm*&0k!&r;aK9H@dqa#&HC*5Q{6 zIb&&NV@{nKCI1^p zeWYeff%i>k7|o8kh2QCJJf4f!Mxyghl8G;(i8w3*ZR=XA#}|p&bHz3RpQ$ z8-|7hPIU{R;Pc-gCY1i*jNv)PsGj-y1#DE#U>ZbbA|Fr|i#MEst{5b`pY}lE!eH!ShjSd# z=I|8}h?17rf`|eiD_CqOWokZRQ}`?U$f;P3I@G&cRqjG8dL{gLp@68@`*wx=iWFTu znU77w`$%M5N?N1;uqQDG8F~7hrc_^;4YY)Z!L)0E5kdsXeqc$WV~a|gv8!l3GRBOv2F9?X4by)o zvGMu%cf=g8!9OQk<5RGP^vCNMuaBIG*RGeU>O5c)@FC#HYNY)~6}$m`*GF%CFm2@` z-h@7qp&HIEuR-+LMaMOcXX|9ONXycm_t3cH$FrK~l@CX?FCp!1D5e`=qJ*PrsgWE| zJ{&2rOHeaMp2w)w&d?nMtD<*p@foeMDr`D)N84bu{QYvP1C8GV&9?yz<^CJFkOke* z^2pj+E%gr|iDT$q2SRdlf>mx-_YEfWRxoplDu&+UfODo=vXy)k+5_ulLb_E8BL%+n zF{$UcRtVM=jzX9R^axSq;*+*XRT-{5DNSik=m50BWdDqxvvajvO~dWx$`A`f-&>H? z2GlV0odszNaIFmb!(+2xy9BW2Bke4ySEr@FGKR(_=|7gOHT2$!?0fVb@Qlz;`sAc~ zBfdyPwF;jjs>GMsj-*yHoEU?*k;z1JT)8;04bdN#q{&1#KaTW_h1o$;llCaporbn6 z^Wlc6d8lQ>kcZ1LT3dwAw~{|5<>8OXourbLrezgmxBw~+Q1~wo$`Rcs{dGc?ZB-c&IcQG0x}q$ z-{46v(Mt36=^7&_EY)L2cX~Ih5JdN<#Hm}CKlVNvPn7)GMfcE>=$?nuOj^az`_f%i z*&-JPtcYH19ClrOBtp z9z!K_c8PpsSb_gSzA@u-` zlEWz(DT!m!f;l^aNpJX;D}hFhadr&+|J~(-$@LTy9weI744}2CC2>)mdN_MV7i4#{ z1)0OJe}_^Z#rN5KscjhlK(?n(;op+djC}kg*^yzy50lq`){sACq$nO)^gA)V>A>_O zz=w|0nSB^{k-bJENzVEY{1UNc?ZovYccl)mB~2?+ck$iA0Rd$q`kU^ok?=ueM09s| zV3zSS&J~J8w@Q=?g=5<(BTpCDVaE;TMJR{g#^{mG?>_9;3*iUlE)MzKzxHoVG(LF< ziYKdzr5biKEn^K&putLt%3CbFkO3+Gg&(JU?nJIo7Eb3N?xv!+f$r)KSufNJ%A1N{ z(fzeQ5VrLE^pEZz_oG$CGkCD1htY=*GA<6Y*8@WD^Y*wD!kG%jg_%pI@n?(>cs0?3 zL$HK30h#Zd{@Wacp}Z4U`WVRhEBO8)^Oyvn+&!^B}0oE=%!~MFMYvxDAe&OL1ZDLGkcv#Kk3{~v3!ryV~-WWg=AQTxat5?;wL}PuF=W- zTt2^&6|T<|3$@@Y+|LS-vsd#3oxlTsm5gVqfa4&$~Z!$qI z2l!snnX@zLbzg6b+S=QE&KhE=eBlF2PYW)o+}$U9W zZS8rz7{5jqOi$}3dz5sdQcid}(G$`orSbiqvo0oroyHv_6+9g!{i`=Y{++8$_z9xU zFIA>3@~f`Qb9v60(cOzXgQzCp+^s$Atpw7VpPYTPW`70j1@l6=>~i2Cl(}gd8x2>y zI~%^+%QBQ@So$m*UAfp^1b2(*UQ5p9o1(L(*}=0G+2Eo@N?_RAe)ldCv_==a&%+NN zv+nbhTL`}L4=pO0}mRz8ooOig)&x8UkWUrWsUsb zFOnD6s7+saQF_?_XoVt}?4oyZr(}aoQlSkpi@RB^%6X!%q&KsREgpU0(&=2#+C}Ep zq~LdmxF#N6r11q!0SG>+wOaCQfl`&`EqQTtcW(tdbIcliTq_JzYlYzBqI(?~Do6@E z{B4&N8Q&!{1uIQQK08$L`#C7Ek|2P55S!%Wd#H>c^whW8X)xLWGz_%@4M*F6MxbBW zatmL@Mbc4^&U`52*T9Vl*i;Cw^Fqu^$G-*m3BVRfR)zRkkaYv!4*XSui$9KRg)?#4 zgMwMMn6-Ubd>>xZEFNnyRvl|TX8ncwm?d5CI6~)?LZ0Ar|4clk{?b}3&36F zB~{m*N+AGMJ3P;6(*p+u6P=#uVM$#w#uzQuT(yQ z`$%b33GTCXR&BiChL z6h6oQNsd-W1GQIc;QRTc+5mK++Njz&*5r|?dB6?lV;d2-x(NO{NwAgHm{@#-w3*X@ z9$cm$EYtaA8hP(#khkBvz4Qo7%Guc(zuUWffBD_B4k>#-%IIC$MTeC8A7$U(l}$Ki ziLU-F@N=QQ5^HT?>#H$NCYu^g$&(?5bQ094!C54uu@~r2qb4i|q!^x8@=-LNL*^PU z2mR|!Vc%dZ?KB{6PLH_5YWNQem`Ot1QE2UeHvz{1BY+{mLL%bEp#28^qh5e^is&|K zHq_sPxFSG1pc$|Y@Bm;PAQ#XB*w)BDJHCS03^!WdsBN5D$e7KW Date: Sat, 19 Mar 2022 12:15:41 +1100 Subject: [PATCH 0354/3101] Tools: correct antennatracker name in size_compare_branches.py --- Tools/scripts/size_compare_branches.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index edfea51965d..1f824dd3ee8 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -149,12 +149,12 @@ def run(self): "sub" : "ardusub", "heli" : "arducopter-heli", "blimp" : "blimp", - "antennatracker " : "antennatracker", + "antennatracker" : "antennatracker", } if self.vehicle in vehicle_map: binary_filename = vehicle_map[self.vehicle] else: - raise Exception("Vehicle name incorrect") + raise Exception("Vehicle name (%s) incorrect" % (self.vehicle)) elf_diff_commandline = [ "time", From 1f5165349efdf6884a657d04c7a0fcaa193070c3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 12 Mar 2022 22:48:55 +1100 Subject: [PATCH 0355/3101] Plane: correct compilation when airspeed disabled --- ArduPlane/GCS_Mavlink.cpp | 2 ++ ArduPlane/Parameters.cpp | 2 ++ ArduPlane/Plane.h | 1 + ArduPlane/is_flying.cpp | 8 +++++++- ArduPlane/quadplane.cpp | 2 ++ ArduPlane/servos.cpp | 6 ++++++ 6 files changed, 20 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 4ac15d383f2..884b1cb0b07 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -247,9 +247,11 @@ float GCS_MAVLINK_Plane::vfr_hud_airspeed() const // will use an airspeed sensor, that value is constrained by the // ground speed. When reporting we should send the true airspeed // value if possible: +#if AP_AIRSPEED_ENABLED if (plane.airspeed.enabled() && plane.airspeed.healthy()) { return plane.airspeed.get_airspeed(); } +#endif // airspeed estimates are OK: float aspeed; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 57fd778c60d..f6563e24a3e 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1492,6 +1492,7 @@ void Plane::load_parameters(void) } #endif +#if AP_AIRSPEED_ENABLED // PARAMETER_CONVERSION - Added: Jan-2022 { const uint16_t old_key = g.k_param_airspeed; @@ -1499,6 +1500,7 @@ void Plane::load_parameters(void) const uint16_t old_top_element = 0; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) AP_Param::convert_class(old_key, &airspeed, airspeed.var_info, old_index, old_top_element, true); } +#endif hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8a74bb1e3c4..bb82150de6b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -31,6 +31,7 @@ #include #include +#include #include #include #include // ArduPilot Mega Vector/Matrix math Library diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 38fa07c6a43..81733a15879 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -291,8 +291,14 @@ void Plane::crash_detection_update(void) // if we have no GPS lock and we don't have a functional airspeed // sensor then don't do crash detection - if (gps.status() < AP_GPS::GPS_OK_FIX_3D && (!airspeed.use() || !airspeed.healthy())) { + if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { +#if AP_AIRSPEED_ENABLED + if (!airspeed.use() || !airspeed.healthy()) { + crashed = false; + } +#else crashed = false; +#endif } if (!crashed) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 37adb002012..078dbaa6938 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3014,11 +3014,13 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd) return false; } +#if AP_AIRSPEED_ENABLED if (is_positive(maximum_takeoff_airspeed) && (plane.airspeed.get_airspeed() > maximum_takeoff_airspeed)) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to complete takeoff, excessive wind"); plane.set_mode(plane.mode_qland, ModeReason::VTOL_FAILED_TAKEOFF); return false; } +#endif if (plane.current_loc.alt < plane.next_WP_loc.alt) { return false; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 9511c188532..7381b980832 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -133,11 +133,17 @@ bool Plane::suppress_throttle(void) // if we have an airspeed sensor, then check it too, and // require 5m/s. This prevents throttle up due to spiky GPS // groundspeed with bad GPS reception +#if AP_AIRSPEED_ENABLED if ((!ahrs.airspeed_sensor_enabled()) || airspeed.get_airspeed() >= 5) { // we're moving at more than 5 m/s throttle_suppressed = false; return false; } +#else + // no airspeed sensor, so we trust that the GPS's movement is truthful + throttle_suppressed = false; + return false; +#endif } #if HAL_QUADPLANE_ENABLED From 7abcf9ec46e01ab11a3298612a3a38361dc7da7c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 20 Mar 2022 13:47:54 +0530 Subject: [PATCH 0356/3101] AP_HAL_ChibiOS: add CubeOrangePlus --- .../hwdef/CubeOrangePlus/defaults.parm | 11 +++++++ .../hwdef/CubeOrangePlus/hwdef-bl.dat | 16 ++++++++++ .../hwdef/CubeOrangePlus/hwdef.dat | 29 +++++++++++++++++++ 3 files changed, 56 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/defaults.parm new file mode 100644 index 00000000000..05d30c8e6f1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/defaults.parm @@ -0,0 +1,11 @@ +# setup correct defaults for battery monitoring for cube power brick +BATT2_CURR_PIN 4 +BATT2_VOLT_PIN 13 +BATT_AMP_PERVLT 39.877 +BATT_VOLT_MULT 12.02 +BATT2_AMP_PERVLT 39.877 +BATT2_VOLT_MULT 12.02 +# setup ADSB +ADSB_TYPE 1 +SERIAL5_BAUD 57 +SERIAL5_PROTOCOL 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef-bl.dat new file mode 100644 index 00000000000..86d0a0f0420 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef-bl.dat @@ -0,0 +1,16 @@ +# hw definition file for processing by chibios_hwdef.py +# for H743 bootloader + +include ../CubeOrange/hwdef-bl.dat + +undef USB_STRING_PRODUCT +undef USB_STRING_MANUFACTURER +undef APJ_BOARD_ID +undef USB_PRODUCT + +# USB setup +USB_PRODUCT 0x1058 +USB_STRING_MANUFACTURER "CubePilot" +USB_STRING_PRODUCT "CubeOrange+-BL" + +APJ_BOARD_ID 1063 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat new file mode 100644 index 00000000000..c0fff5ff8a7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef.dat @@ -0,0 +1,29 @@ +# hw definition file for processing by chibios_hwdef.py + +include ../CubeOrange/hwdef.dat + +undef USB_STRING_PRODUCT +undef USB_STRING_MANUFACTURER +undef APJ_BOARD_ID +undef USB_PRODUCT + +# USB setup +USB_PRODUCT 0x1058 +USB_STRING_MANUFACTURER "CubePilot" +USB_STRING_PRODUCT "CubeOrange+" + +APJ_BOARD_ID 1063 + +undef CHECK_ICM42688_EXT +undef CHECK_IMU0_PRESENT +undef IMU + +SPIDEV icm42688_ext SPI4 DEVID4 ACCEL_EXT_CS MODE3 2*MHZ 8*MHZ + +IMU Invensensev3 SPI:icm42688_ext ROTATION_ROLL_180_YAW_270 +IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180 +IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 + +CHECK_ICM42688_EXT spi_check_register("icm42688_ext", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) + +CHECK_IMU0_PRESENT $CHECK_ICM42688_EXT From 95b69ba1fd8b194fabf3b27b2b7e724aa766d8db Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 20 Mar 2022 13:48:12 +0530 Subject: [PATCH 0357/3101] AP_Bootloader: add board type CubeOrangePlus --- Tools/AP_Bootloader/board_types.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 7b3c0d7a6ba..d786dea38b8 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -161,6 +161,7 @@ AP_HW_JHEMCUGSF405A 1059 AP_HW_SPRACINGH7 1060 AP_HW_DEVEBOXH7 1061 AP_HW_MatekL431 1062 +AP_HW_CUBEORANGEPLUS 1063 AP_HW_CUBEORANGE_PERIPH 1400 AP_HW_CUBEBLACK_PERIPH 1401 From 8574afb2e1559dc2431efe2cefdc6ec7126cdeaf Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 20 Mar 2022 13:48:40 +0530 Subject: [PATCH 0358/3101] AP_BoardConfig: add WHOAMI for INV42688 --- libraries/AP_BoardConfig/board_drivers.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index bd58df7b105..4f35e52536b 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -285,6 +285,10 @@ bool AP_BoardConfig::check_ms5611(const char* devname) { #define INV2_WHOAMI_ICM20948 0xEA #define INV2_WHOAMI_ICM20649 0xE1 +#define INV3REG_WHOAMI 0x75 + +#define INV3_WHOAMI_ICM42688 0x47 + /* validation of the board type */ From 0d256da3b59a2e33890b79eaebc77f08092d3fad Mon Sep 17 00:00:00 2001 From: pacolate12 <56329725+pacolate12@users.noreply.github.com> Date: Wed, 16 Mar 2022 14:48:33 -0400 Subject: [PATCH 0359/3101] AP_UAVCAN: correct array inxexing Allows the code to be run on hardware --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 2653d928967..6be6d77039e 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -564,9 +564,9 @@ void AP_UAVCAN::SRV_push_servos() { WITH_SEMAPHORE(SRV_sem); - for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { // Check if this channels has any function assigned - if (SRV_Channels::channel_function(i)) { + if (SRV_Channels::channel_function(i) > SRV_Channel::k_none) { _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm(); _SRV_conf[i].esc_pending = true; _SRV_conf[i].servo_pending = true; From e704e84bf92d4db936b5547565538431d467a3fb Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 10 Mar 2022 21:46:20 +0100 Subject: [PATCH 0360/3101] Tools: fix package install on Python2 --- Tools/environment_install/install-prereqs-ubuntu.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 67287065d6a..c4feae6d053 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -138,7 +138,11 @@ fi # add some Python packages required for commonly-used MAVProxy modules and hex file generation: if [[ $SKIP_AP_EXT_ENV -ne 1 ]]; then - PYTHON_PKGS="$PYTHON_PKGS pygame intelhex" + if [ ${RELEASE_CODENAME} == 'xenial' ] || [ ${RELEASE_CODENAME} == 'disco' ] || [ ${RELEASE_CODENAME} == 'eoan' ]; then + PYTHON_PKGS="$PYTHON_PKGS pygame==2.0.3 intelhex" + else + PYTHON_PKGS="$PYTHON_PKGS pygame intelhex" + fi fi ARM_LINUX_PKGS="g++-arm-linux-gnueabihf $INSTALL_PKG_CONFIG" # python-wxgtk packages are added to SITL_PKGS below From 699651e306c5c5aaa263b117240bbf23f11318ba Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 10 Mar 2022 22:21:11 +0100 Subject: [PATCH 0361/3101] Tools: add pexpect on Arch env --- Tools/environment_install/install-prereqs-arch.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-arch.sh b/Tools/environment_install/install-prereqs-arch.sh index 908fc08c37f..1b901c21584 100755 --- a/Tools/environment_install/install-prereqs-arch.sh +++ b/Tools/environment_install/install-prereqs-arch.sh @@ -26,7 +26,7 @@ BASE_PKGS="base-devel ccache git gsfonts tk wget gcc" SITL_PKGS="python-pip python-setuptools python-wheel wxpython opencv python-numpy python-scipy" PX4_PKGS="lib32-glibc zip zlib ncurses" -PYTHON_PKGS="future lxml pymavlink MAVProxy argparse matplotlib pyparsing geocoder pyserial empy" +PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy" # GNU Tools for ARM Embedded Processors # (see https://launchpad.net/gcc-arm-embedded/) From feb9e78f005ea73fae8321195a6454d35a37e078 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Feb 2022 20:34:22 +1100 Subject: [PATCH 0362/3101] AP_GPS: prevent switching to a dead GPS when we switch away from the blended GPS instance we need to ensure we don't switch to a GPS that is timing out, and may be the instance that is triggering the disable of blending --- libraries/AP_GPS/AP_GPS.cpp | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index d794aa1e346..e3b4e203548 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1124,13 +1124,34 @@ void AP_GPS::update_primary(void) if (primary_instance == GPS_BLENDED_INSTANCE) { primary_instance = 0; for (uint8_t i=1; i state[primary_instance].status) || - ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) { + // choose GPS with highest state or higher number of + // satellites. Reject a GPS with an old update time, as it + // may be the old timestamp that triggered the loss of + // blending + const uint32_t delay_threshold = 400; + const bool higher_status = state[i].status > state[primary_instance].status; + const bool old_data_primary = (now - state[primary_instance].last_gps_time_ms) > delay_threshold; + const bool old_data = (now - state[i].last_gps_time_ms) > delay_threshold; + const bool equal_status = state[i].status == state[primary_instance].status; + const bool more_sats = state[i].num_sats > state[primary_instance].num_sats; + if (old_data && !old_data_primary) { + // don't switch to a GPS that has not updated in 400ms + continue; + } + if (state[i].status < GPS_OK_FIX_3D) { + // don't use a GPS without 3D fix + continue; + } + // select the new GPS if the primary has old data, or new + // GPS either has higher status, or has the same status + // and more satellites + if ((old_data_primary && !old_data) || + higher_status || + (equal_status && more_sats)) { primary_instance = i; - _last_instance_swap_ms = now; } } + _last_instance_swap_ms = now; return; } #endif // defined(GPS_BLENDED_INSTANCE) From 2784f8fa7f8b2de05663f7be3d0c4d2035d7d885 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 10 Feb 2022 10:44:58 +1100 Subject: [PATCH 0363/3101] Plane: remove persistent guided_WP_loc state So instead of updating plane.guided_WP_loc and then calling set_guided_WP(void) to copy that state into plane.next_WP_loc we pass the new location in the call to set_guided_WP(const Location &loc). avoidance was the only place which was not entirely over-writing plane.guided_WP_loc. However, plane.next_WP_loc was updated to be the current location when we entered guided mode. If we update the horizontal/vertical avoidance now it is relative to the current location, not the guided wp location, which could be quite important. --- ArduPlane/ArduPlane.cpp | 13 ++++++------ ArduPlane/GCS_Mavlink.cpp | 9 ++++---- ArduPlane/Plane.h | 5 +---- ArduPlane/avoidance_adsb.cpp | 35 +++++++++++++++++-------------- ArduPlane/avoidance_adsb.h | 4 ++-- ArduPlane/commands.cpp | 6 +++--- ArduPlane/fence.cpp | 22 +++++++++---------- ArduPlane/mode_LoiterAltQLand.cpp | 10 ++++----- ArduPlane/mode_guided.cpp | 18 +++++++--------- 9 files changed, 59 insertions(+), 63 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 7d67005f27f..5910ec9636c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -713,19 +713,20 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const #if AP_SCRIPTING_ENABLED // set target location (for use by scripting) -bool Plane::set_target_location(const Location& target_loc) +bool Plane::set_target_location(const Location &target_loc) { + Location loc{target_loc}; + if (plane.control_mode != &plane.mode_guided) { // only accept position updates when in GUIDED mode return false; } - plane.guided_WP_loc = target_loc; // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (loc.relative_alt) { + loc.alt += plane.home.alt; + loc.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(loc); return true; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 884b1cb0b07..07a23adeb39 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -714,15 +714,14 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); - plane.guided_WP_loc = requested_position; // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (requested_position.relative_alt) { + requested_position.alt += plane.home.alt; + requested_position.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(requested_position); return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index bb82150de6b..05905a4f469 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -721,9 +721,6 @@ class Plane : public AP_Vehicle { // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. Location next_WP_loc {}; - // The location of the active waypoint in Guided mode. - struct Location guided_WP_loc {}; - // Altitude control struct { // target altitude above sea level in cm. Used for barometric @@ -947,7 +944,7 @@ class Plane : public AP_Vehicle { #endif // commands.cpp - void set_guided_WP(void); + void set_guided_WP(const Location &loc); void update_home(); // set home location and store it persistently: bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index b44ff75b962..f775fa29c8c 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -61,31 +61,34 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob } break; - case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: + case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND: { // climb or descend to avoid obstacle - if (handle_avoidance_vertical(obstacle, failsafe_state_change)) { - plane.set_guided_WP(); + Location loc = plane.next_WP_loc; + if (handle_avoidance_vertical(obstacle, failsafe_state_change, loc)) { + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } break; - - case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: + } + case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY: { // move horizontally to avoid obstacle - if (handle_avoidance_horizontal(obstacle, failsafe_state_change)) { - plane.set_guided_WP(); + Location loc = plane.next_WP_loc; + if (handle_avoidance_horizontal(obstacle, failsafe_state_change, loc)) { + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } break; - + } case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR: { // move horizontally and vertically to avoid obstacle - const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change); - const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change); + Location loc = plane.next_WP_loc; + const bool success_vert = handle_avoidance_vertical(obstacle, failsafe_state_change, loc); + const bool success_hor = handle_avoidance_horizontal(obstacle, failsafe_state_change, loc); if (success_vert || success_hor) { - plane.set_guided_WP(); + plane.set_guided_WP(loc); } else { actual_action = MAV_COLLISION_ACTION_NONE; } @@ -157,7 +160,7 @@ bool AP_Avoidance_Plane::check_flightmode(bool allow_mode_change) return (plane.control_mode == &plane.mode_avoidADSB); } -bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc) { // ensure copter is in avoid_adsb mode if (!check_flightmode(allow_mode_change)) { @@ -167,20 +170,20 @@ bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle // get best vector away from obstacle if (plane.current_loc.alt > obstacle->_location.alt) { // should climb - plane.guided_WP_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX + new_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX return true; } else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) { // should descend while above RTL alt // TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high - plane.guided_WP_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX + new_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX return true; } return false; } -bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change) +bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc) { // ensure plane is in avoid_adsb mode if (!check_flightmode(allow_mode_change)) { @@ -205,7 +208,7 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl velocity_neu *= 10000; // set target - plane.guided_WP_loc.offset(velocity_neu.x, velocity_neu.y); + new_loc.offset(velocity_neu.x, velocity_neu.y); return true; } diff --git a/ArduPlane/avoidance_adsb.h b/ArduPlane/avoidance_adsb.h index 23cf57ba196..508fe658770 100644 --- a/ArduPlane/avoidance_adsb.h +++ b/ArduPlane/avoidance_adsb.h @@ -26,10 +26,10 @@ class AP_Avoidance_Plane : public AP_Avoidance { bool check_flightmode(bool allow_mode_change); // vertical avoidance handler - bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change); + bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc); // horizontal avoidance handler - bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change); + bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change, Location &new_loc); // control mode before avoidance began enum Mode::Number prev_control_mode_number = Mode::Number::RTL; diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index c5bcee335bd..3a0900acdb5 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -61,9 +61,9 @@ void Plane::set_next_WP(const struct Location &loc) setup_turn_angle(); } -void Plane::set_guided_WP(void) +void Plane::set_guided_WP(const Location &loc) { - if (aparm.loiter_radius < 0 || guided_WP_loc.loiter_ccw) { + if (aparm.loiter_radius < 0 || loc.loiter_ccw) { loiter.direction = -1; } else { loiter.direction = 1; @@ -75,7 +75,7 @@ void Plane::set_guided_WP(void) // Load the next_WP slot // --------------------- - next_WP_loc = guided_WP_loc; + next_WP_loc = loc; // used to control FBW and limit the rate of climb // ----------------------------------------------- diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index d22a50bb751..ee829cabb9a 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -70,39 +70,39 @@ void Plane::fence_check() set_mode(mode_guided, ModeReason::FENCE_BREACHED); } + Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point - guided_WP_loc = {}; if (fence.get_return_altitude() > 0) { // fly to the return point using _retalt - guided_WP_loc.alt = home.alt + 100.0f * fence.get_return_altitude(); + loc.alt = home.alt + 100.0f * fence.get_return_altitude(); } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) { // invalid min/max, use RTL_altitude - guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; + loc.alt = home.alt + g.RTL_altitude_cm; } else { // fly to the return point, with an altitude half way between // min and max - guided_WP_loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2; + loc.alt = home.alt + 100.0f * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2; } Vector2l return_point; if(fence.polyfence().get_return_point(return_point)) { - guided_WP_loc.lat = return_point[0]; - guided_WP_loc.lng = return_point[1]; + loc.lat = return_point[0]; + loc.lng = return_point[1]; } else { // When no fence return point is found (ie. no inclusion fence uploaded, but exclusion is) // we fail to obtain a valid fence return point. In this case, home is considered a safe // return point. - guided_WP_loc.lat = home.lat; - guided_WP_loc.lng = home.lng; + loc.lat = home.lat; + loc.lng = home.lng; } } if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) { - setup_terrain_target_alt(guided_WP_loc); - set_guided_WP(); + setup_terrain_target_alt(loc); + set_guided_WP(loc); } if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) { diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 586820e3848..0cc8b9457c1 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -44,20 +44,18 @@ void ModeLoiterAltQLand::switch_qland() bool ModeLoiterAltQLand::handle_guided_request(Location target_loc) { - plane.guided_WP_loc = target_loc; - // setup altitude #if AP_TERRAIN_AVAILABLE if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); } else { - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); } #else - plane.guided_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); #endif - plane.set_guided_WP(); + plane.set_guided_WP(target_loc); return true; } diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index aeac4819895..b80e72dd021 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -8,19 +8,19 @@ bool ModeGuided::_enter() when entering guided mode we set the target as the current location. This matches the behaviour of the copter code */ - plane.guided_WP_loc = plane.current_loc; + Location loc{plane.current_loc}; #if HAL_QUADPLANE_ENABLED if (plane.quadplane.guided_mode_enabled()) { /* if using Q_GUIDED_MODE then project forward by the stopping distance */ - plane.guided_WP_loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), - plane.quadplane.stopping_distance()); + loc.offset_bearing(degrees(plane.ahrs.groundspeed_vector().angle()), + plane.quadplane.stopping_distance()); } #endif - plane.set_guided_WP(); + plane.set_guided_WP(loc); return true; } @@ -45,15 +45,13 @@ void ModeGuided::navigate() bool ModeGuided::handle_guided_request(Location target_loc) { - plane.guided_WP_loc = target_loc; - // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; + if (target_loc.relative_alt) { + target_loc.alt += plane.home.alt; + target_loc.relative_alt = 0; } - plane.set_guided_WP(); + plane.set_guided_WP(target_loc); return true; } From 0e62d561d9cd3b48e7f67be10ae6e33c1882e6c7 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 14 Mar 2022 11:33:32 -0300 Subject: [PATCH 0364/3101] AP_Filesystem: add allow_absolute_paths to open(), implement it for posix backend --- libraries/AP_Filesystem/AP_Filesystem.cpp | 4 ++-- libraries/AP_Filesystem/AP_Filesystem.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_ESP32.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp | 2 +- libraries/AP_Filesystem/AP_Filesystem_FATFS.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_Mission.cpp | 2 +- libraries/AP_Filesystem/AP_Filesystem_Mission.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_Param.cpp | 2 +- libraries/AP_Filesystem/AP_Filesystem_Param.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp | 2 +- libraries/AP_Filesystem/AP_Filesystem_ROMFS.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_Sys.cpp | 2 +- libraries/AP_Filesystem/AP_Filesystem_Sys.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_backend.h | 2 +- libraries/AP_Filesystem/AP_Filesystem_posix.cpp | 6 ++++-- libraries/AP_Filesystem/AP_Filesystem_posix.h | 2 +- 16 files changed, 20 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 1719a7b0c54..2d677bef8d3 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -106,10 +106,10 @@ const AP_Filesystem::Backend &AP_Filesystem::backend_by_fd(int &fd) const return backends[idx]; } -int AP_Filesystem::open(const char *fname, int flags) +int AP_Filesystem::open(const char *fname, int flags, bool allow_absolute_paths) { const Backend &backend = backend_by_path(fname); - int fd = backend.fs.open(fname, flags); + int fd = backend.fs.open(fname, flags, allow_absolute_paths); if (fd < 0) { return -1; } diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index 972b8e93ddb..bbd02e78b59 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -80,7 +80,7 @@ class AP_Filesystem { AP_Filesystem() {} // functions that closely match the equivalent posix calls - int open(const char *fname, int flags); + int open(const char *fname, int flags, bool allow_absolute_paths = false); int close(int fd); int32_t read(int fd, void *buf, uint32_t count); int32_t write(int fd, const void *buf, uint32_t count); diff --git a/libraries/AP_Filesystem/AP_Filesystem_ESP32.h b/libraries/AP_Filesystem/AP_Filesystem_ESP32.h index bdc03fbd33c..69877fb7f59 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ESP32.h +++ b/libraries/AP_Filesystem/AP_Filesystem_ESP32.h @@ -21,7 +21,7 @@ class AP_Filesystem_ESP32 : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t write(int fd, const void *buf, uint32_t count) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp index 887aabfd988..29a45d3aed0 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp @@ -279,7 +279,7 @@ static bool remount_file_system(void) return true; } -int AP_Filesystem_FATFS::open(const char *pathname, int flags) +int AP_Filesystem_FATFS::open(const char *pathname, int flags, bool allow_absolute_path) { int fileno; int fatfs_modes; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h index 5193555947f..663dffc8193 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h @@ -20,7 +20,7 @@ class AP_Filesystem_FATFS : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t write(int fd, const void *buf, uint32_t count) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp index d4cb9f34de9..ab71298b9dc 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp @@ -30,7 +30,7 @@ extern int errno; #define IDLE_TIMEOUT_MS 30000 -int AP_Filesystem_Mission::open(const char *fname, int flags) +int AP_Filesystem_Mission::open(const char *fname, int flags, bool allow_absolute_paths) { enum MAV_MISSION_TYPE mtype; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.h b/libraries/AP_Filesystem/AP_Filesystem_Mission.h index b4ff4f53ca9..a4f0f869fd5 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.h @@ -23,7 +23,7 @@ class AP_Filesystem_Mission : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t lseek(int fd, int32_t offset, int whence) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp index 5d3203227c1..5d9f66fca33 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; extern int errno; -int AP_Filesystem_Param::open(const char *fname, int flags) +int AP_Filesystem_Param::open(const char *fname, int flags, bool allow_absolute_path) { if (!check_file_name(fname)) { errno = ENOENT; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.h b/libraries/AP_Filesystem/AP_Filesystem_Param.h index 98e8dc6af11..6f3c30f046c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.h @@ -24,7 +24,7 @@ class AP_Filesystem_Param : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t lseek(int fd, int32_t offset, int whence) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp index 5b23b4e898a..d4e3dadb3f2 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp @@ -23,7 +23,7 @@ #if defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H) -int AP_Filesystem_ROMFS::open(const char *fname, int flags) +int AP_Filesystem_ROMFS::open(const char *fname, int flags, bool allow_absolute_paths) { if ((flags & O_ACCMODE) != O_RDONLY) { errno = EROFS; diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h index fb69e23360d..897195915e8 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h @@ -21,7 +21,7 @@ class AP_Filesystem_ROMFS : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t write(int fd, const void *buf, uint32_t count) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index da14a5bfc53..0d0550c8f77 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -60,7 +60,7 @@ int8_t AP_Filesystem_Sys::file_in_sysfs(const char *fname) { return -1; } -int AP_Filesystem_Sys::open(const char *fname, int flags) +int AP_Filesystem_Sys::open(const char *fname, int flags, bool allow_absolute_paths) { if ((flags & O_ACCMODE) != O_RDONLY) { errno = EROFS; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.h b/libraries/AP_Filesystem/AP_Filesystem_Sys.h index 1eef1fac359..e097b74d4a2 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.h @@ -23,7 +23,7 @@ class AP_Filesystem_Sys : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t lseek(int fd, int32_t offset, int whence) override; diff --git a/libraries/AP_Filesystem/AP_Filesystem_backend.h b/libraries/AP_Filesystem/AP_Filesystem_backend.h index a60fad1d176..f359891cf9c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_backend.h +++ b/libraries/AP_Filesystem/AP_Filesystem_backend.h @@ -43,7 +43,7 @@ class AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - virtual int open(const char *fname, int flags) { + virtual int open(const char *fname, int flags, bool allow_absolute_paths = false) { return -1; } virtual int close(int fd) { return -1; } diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 6454d86d603..f0982159b20 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -49,10 +49,12 @@ static const char *map_filename(const char *fname) return fname; } -int AP_Filesystem_Posix::open(const char *fname, int flags) +int AP_Filesystem_Posix::open(const char *fname, int flags, bool allow_absolute_paths) { FS_CHECK_ALLOWED(-1); - fname = map_filename(fname); + if (! allow_absolute_paths) { + fname = map_filename(fname); + } // we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage return ::open(fname, flags | O_CLOEXEC, 0644); } diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h index 42fdad3ec6b..61d6bfdfd37 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.h +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h @@ -29,7 +29,7 @@ class AP_Filesystem_Posix : public AP_Filesystem_Backend { public: // functions that closely match the equivalent posix calls - int open(const char *fname, int flags) override; + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; int close(int fd) override; int32_t read(int fd, void *buf, uint32_t count) override; int32_t write(int fd, const void *buf, uint32_t count) override; From 19db542111ffe2f3bed857dc6d8056f9fe1198e5 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 21 Feb 2022 21:08:24 -0300 Subject: [PATCH 0365/3101] AP_Param: Use AP:FS for accessing files --- libraries/AP_Param/AP_Param.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 1bbba058aa8..ab351c50c83 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include @@ -2109,8 +2110,9 @@ bool AP_Param::parse_param_line(char *line, char **vname, float &value, bool &re // increments num_defaults for each default found in filename bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults) { - FILE *f = fopen(filename, "r"); - if (f == nullptr) { + // try opening the file both in the posix filesystem and using AP::FS + int file_apfs = AP::FS().open(filename, O_RDONLY, true); + if (file_apfs == -1) { return false; } char line[100]; @@ -2118,7 +2120,7 @@ bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaul /* work out how many parameter default structures to allocate */ - while (fgets(line, sizeof(line)-1, f)) { + while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) { char *pname; float value; bool read_only; @@ -2131,23 +2133,23 @@ bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaul } num_defaults++; } - - fclose(f); + AP::FS().close(file_apfs); return true; } bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass) { - FILE *f = fopen(filename, "r"); - if (f == nullptr) { + // try opening the file both in the posix filesystem and using AP::FS + int file_apfs = AP::FS().open(filename, O_RDONLY, true); + if (file_apfs == -1) { AP_HAL::panic("AP_Param: Failed to re-open defaults file"); return false; } uint16_t idx = 0; char line[100]; - while (fgets(line, sizeof(line)-1, f)) { + while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) { char *pname; float value; bool read_only; @@ -2179,7 +2181,8 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass) vp->set_float(value, var_type); } } - fclose(f); + AP::FS().close(file_apfs); + return true; } From e8115a99bc2f7fba5a058401237f7551fa6986ea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Mar 2022 17:11:21 +1100 Subject: [PATCH 0366/3101] AP_Scripting: added set_override method for scripting useful for test code --- libraries/AP_Scripting/docs/docs.lua | 4 ++++ libraries/AP_Scripting/examples/RC_override.lua | 14 ++++++++++++++ .../generator/description/bindings.desc | 1 + 3 files changed, 19 insertions(+) create mode 100644 libraries/AP_Scripting/examples/RC_override.lua diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 77b5e2c78c9..6bbe08c9fe2 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -679,6 +679,10 @@ local RC_Channel_ud = {} ---@return number function RC_Channel_ud:norm_input_ignore_trim() end +-- desc +---@param PWM integer +function RC_Channel_ud:set_override(PWM) end + -- desc ---@return integer function RC_Channel_ud:get_aux_switch_pos() end diff --git a/libraries/AP_Scripting/examples/RC_override.lua b/libraries/AP_Scripting/examples/RC_override.lua new file mode 100644 index 00000000000..52b19f2945c --- /dev/null +++ b/libraries/AP_Scripting/examples/RC_override.lua @@ -0,0 +1,14 @@ +-- example of overriding RC inputs + +local RC4 = rc:get_channel(4) + +function update() + -- mirror RC1 onto RC4 + rc1_input = rc:get_pwm(1) + RC4:set_override(rc1_input) + return update, 10 +end + +gcs:send_text(0, "RC_override example") + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 240b3e6cd06..bf2816c272b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -258,6 +258,7 @@ singleton SRV_Channels method set_range void SRV_Channel::Aux_servo_function_t'e ap_object RC_Channel method norm_input float ap_object RC_Channel method get_aux_switch_pos uint8_t ap_object RC_Channel method norm_input_ignore_trim float +ap_object RC_Channel method set_override void uint16_t 0 2200 0'literal include RC_Channel/RC_Channel.h singleton RC_Channels alias rc From 522173328cf290b2261ad0e33c9c0e6824a3e908 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 21:02:19 +1100 Subject: [PATCH 0367/3101] AP_RCProtocol: added failsafe_active() API --- libraries/AP_RCProtocol/AP_RCProtocol.h | 8 ++++++++ libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp | 6 +++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 55b1960bac9..f7ba5cf2dfb 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -67,6 +67,13 @@ class AP_RCProtocol { void process_handshake(uint32_t baudrate); void update(void); + bool failsafe_active() const { + return _failsafe_active; + } + void set_failsafe_active(bool active) { + _failsafe_active = active; + } + void disable_for_pulses(enum rcprotocol_t protocol) { _disabled_for_pulses |= (1U<<(uint8_t)protocol); } @@ -147,6 +154,7 @@ class AP_RCProtocol { AP_RCProtocol_Backend *backend[NONE]; bool _new_input; uint32_t _last_input_ms; + bool _failsafe_active; bool _valid_serial_prot; // optional additional uart diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index 6931ee9775a..3459f1748fa 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -64,7 +64,11 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool memcpy(_pwm_values, values, num_values*sizeof(uint16_t)); _num_channels = num_values; rc_frame_count++; -#if !APM_BUILD_TYPE(APM_BUILD_iofirmware) + frontend.set_failsafe_active(in_failsafe); +#if APM_BUILD_TYPE(APM_BUILD_iofirmware) + // failsafed is sorted out in AP_IOMCU.cpp + in_failsafe = false; +#else if (rc().ignore_rc_failsafe()) { in_failsafe = false; } From ce0b7a895725ea632080bacec4a35a8e2c353734 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 21:03:09 +1100 Subject: [PATCH 0368/3101] AP_IOMCU: fixed handling of RC_OPTIONS bit to ignore RC failsafe bit this worked on FMU but not on IOMCU --- libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 6 ++++-- libraries/AP_IOMCU/iofirmware/ioprotocol.h | 4 +++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 4bf32f98c83..ea30c0077af 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -301,12 +301,14 @@ void AP_IOMCU_FW::rcin_update() { ((ChibiOS::RCInput *)hal.rcin)->_timer_tick(); if (hal.rcin->new_input()) { + const auto &rc = AP::RC(); rc_input.count = hal.rcin->num_channels(); rc_input.flags_rc_ok = true; hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS); rc_last_input_ms = last_ms; - rc_input.rc_protocol = (uint16_t)AP::RC().protocol_detected(); - rc_input.rssi = AP::RC().get_RSSI(); + rc_input.rc_protocol = (uint16_t)rc.protocol_detected(); + rc_input.rssi = rc.get_RSSI(); + rc_input.flags_failsafe = rc.failsafe_active(); } else if (last_ms - rc_last_input_ms > 200U) { rc_input.flags_rc_ok = false; } diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 7dbf319e666..4256b6d1953 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -159,9 +159,11 @@ struct page_mixing { // enabled needs to be 1 to enable mixing uint8_t enabled; - uint8_t pad; // pad to even size + uint8_t pad; }; +static_assert(sizeof(struct page_mixing) % 2 == 0, "page_mixing must be even size"); + struct __attribute__((packed, aligned(2))) page_GPIO { uint8_t channel_mask; uint8_t output_mask; From 33b7f8f46ed5917adb92cbb55bbcf7733b643d6a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 16 Mar 2022 21:03:21 +1100 Subject: [PATCH 0369/3101] Tools: rebuild IO firmware --- .../iofirmware_f103_8MHz_highpolh.bin | Bin 39912 -> 40872 bytes .../iofirmware_f103_8MHz_lowpolh.bin | Bin 39912 -> 40872 bytes Tools/IO_Firmware/iofirmware_highpolh.bin | Bin 39920 -> 40880 bytes Tools/IO_Firmware/iofirmware_lowpolh.bin | Bin 39920 -> 40880 bytes 4 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index 90686cb849c44658aac13d3cbdbe2fcddd8cf200..11d519ae9ae43178ac490f8e8dcad5ba86ad7e21 100755 GIT binary patch delta 9610 zcmaKx30zgx+W*(uXJDhK2SjCd9}qo&+5t3=^I>o^91wBns%e2!I2BH%m4OP0rKT~+mz51G=Wwps+?=x`r``ZlteLwfUe?ENIde*b%wb$^h zb#DEQ+gHzxA{-G`g%Q4d2I1=p37^vLw?JM4{lk3x_Wus=d~frpuz_QC5PlJO8+-xo zgPkuCz6N{&u7YM@C?R}k$>vc#gUmbI%9*md&0n~B^WnZ-HWi8?Nz+bm>^KXkc zpAP6+9#8ne;|L#EK=@x@sD7H-{X4l`y!0Iy?`E}~#EHis4s>_V3C=y3s{3K0q7Sdg za5WKWuWHH({9ig=daF+B?>IdXh>7roz(=_FL&9siigxV5Y^)Ep)x?IZpnZ%??B+Vp|ycjnph1AgZokPR+Xu2Th(N@a>MI`1CIw}?h*wTl%5Obo0I z(ppOGB!Jp~G!h|~oejwCf5vqzRVUtU?35E0uv=L!bk6FWxm$VG>LC zP8!W^3o*sjUm^E57LO<9Nb*Y>;ic}b1FTzRA6P=^r>rq3x%+-!ZR*Vi`$G-g z!FqKcOv_nr_e8p&x}^JuI$P$6;+nC!dNHAB>jF-C-=+DwqU}XnGl-?I@q#iU>4I`m z*dBH+!EmWksj?Q_P0^LXXO+DsVzh4eiW%0kp(3@*B|%i>4eABO%5i3GNR3ioCy6Oj zJc&tF4{O7n#ZDqcs0l?zk&W%uzn@h+qg=EmFE3M$T8Wg?vRk=8w3^;f?81z@M7OdD zg#?~sPln|BHF}MrPCCb`Lq^g8tU2V#?hdzB@lQU@z1yO>OWDj&2c5@04DB6I>Ep~= zJeG}a@a<0 z+F#w}sqO}y#N0xG&53nT!#ma;316O8M-Ncxofbo8Xtb_~?tdI3=6__mqW6&cTPWjL zAQ4P8fjNO$L77JBGer;svy9Rw3Kwn45@Li%qZH<5#gUdME#a+Qgb#-Ag?|T`2i#UO z{Ls-1H#;7=m}_*g_$X_LQJSE{Ef3F*aD-%%xPbaVF%}KbCCvjJH zF{3bp6LzzC!bd}?%OIMu?1m~M9D5293d#hXBrBSZNW0uO3rZ3G9bp5%)56z@w3}~Chb+Wjx#pD zMTj(3?K@*P%6La%k~!LcHdaW2?H_#H1(lD?PlUdYG5#XZ%-qC+TM zUOgekOlcEa*2jgJ@Jy_ko8n`y$G*e~dstB4Op~vLX7bKpj8_O4oXF<&jo>ExSViAH zT;?8jxbHFg-)v^T=eVR6cBNkw_f8A*_S?uk*230XEZn9&?6}2Bce5tT`}FVZwYU#B z&dWx`hjGPTHaWh4OY^c5@o#XlXV>@yiUv&R{{h{>)c)aIR}YIAP!^i!u2e=_IZ>B< z`6yxjX6z-+b{&!a#_9%)D=7hcMlf;4jjndxQ;+igE(13mQL9u!Z(t@DUcdQ*v+ zTc+5?(d3xInMG?9dqH(bQeR5lY;>}dE7)7@OAg_>-)ZT5L~?eo(TiQ0{dAkGsyv3p zJvw0Gy_R^9ih}68WwWdD&U%}(ID-%q(ov}ZYtiJiLGFRo_!`zE?;x0N*AwlEeYh$= z@M?Q!nNl&XOj&0wQ(m`XUDa>h%MLsmKloBh*DS9b@0;^dmn^;b+1=Adul%t}>il!^ zBsCd(Kl94h)ze4Grex7%wut%zs=S2x4+*DVRL2epq;wP;VvC^?d)yYoEofn-wxoeS zdlN;kEc=MX-axm9K)&kr$}V5&QLlWZH7>@R0&RSYSN_e%uG!u*O!N8VFWK6mX1b5< zADY;&IGXOG((D%fFJts{-|h-3y?}n{j#!AR6$LXXGDsgQEPnWm;J>!?wiqwPi3JOVXl)))N&gYsF?@txCp9SS+VIJo zL1n{62-Z8QuuvzdJ}T_RR&KRo=b5={)<2s_y|&q|a1h>}+O2i+5TtYY*efG`3ti#0 zU@rZD-QDIqg(a@dEsH>RhC*Ugq?2o$H=EC zcA!zcxSaRd^P|Rc!anxpD1j^Uusfro0{40K(%*g24q}cd642+VEPQlKiPvYZ>Td3t zsh9lJoao@JiV%Zz&qZUp=UR(CSfVQm$2PB*YJ3)vlLorbucGkzTQg414ahzwyjiH1 zp7vUcbVb@>prwd($xZ1z5j5k(4qxkUnLiP%ov=}(#ncf}goRVD#hGdhIWf#R+FW8& zAI+rF;8r(D9Sr%AR=2-82y$Gj+gDA79MkFwY7*o~HOY~botzm~knBj#9+VSSK&29& zmg2Xqk($I+xY&i%c!U!a#-)wtGF>p{By}d5K>>EW#J+gdY?k?p*vB5!wu)@p8pe3wjtI}m|kDEO|CXCzQV{eQZ zOKaH8G4)Z2Cr&jB-F|8OLI+q6aLG*#FExPq)Tjb_FSB1 zp6pO==qc9IagD1~nc^7O_eJ$bzNtwcfAivjqFeKidfb1^NaLhM>eZ$l&HfyX|K}wl z{oTWIa(<>|Y)EcwiNSXIsKN4PJ?EI92!smS<|8n z^TUwqJ?zKaK|#sxY(@W8r2=#17duRrI5&$QTg07nv&ykWC8KjvMWP+-(m7Y&1G;2p zsTvcrXt;yM-k6D%wIuz4FlV=7w+?aG@^;PAH}571>Dmh9o}FEIv#?A_9Y>{dmsZA( z5WTe5^@u}%|6K7I<&M=4-(Rx!cbvIB^4T+rCDHzeuKRAKe44tA0;Ox zq@Pjr-Exjg{9B4>4`Me#Y*Sd22pcGs;!=f`F!mp zj|F%AWO0Tn->X+;rM^>K+uSr0E>oMEr}h)R0I=G88~uhg=ZABlF4lE?SV?D>)YK8H zwYCVU76F4zXIWoQ1Z{3(R%aN+E@Ch7lPb4tS8+m#Zg~W@CIOpMx)Ph^mdkxQY}4sc z$36F4B~`g&$R#>Lvzw%~M?B5XSZ99Q2oTrLI@%1P`ERFxnl&0 zWOhBTZ>F7&d)$&+?%W#lm@h>{jNK!{*?@j~T&o#$VH{;(-&!bJBjcdcw0bm$@eNrYY5-6Z0vpt)4&WKBeDO7yczd zCz?HZ(!6}P9O>a<4eYRM-PcS$q75U$d<`$CawiX$%@uIjsRc+?A8~;2F9dHKO&z)d zZLmiiVCSdWxF6jt;Bler%@c%l#JqTb4SW0}ZpQ)UetdGbgC2eJ9YS%7G}$?64=bG3 zOWz_ZDtmESi0yGj=h!|sJZy66&DV@pu^cxiPMXY;rx(T!`hf6!P1+0H zt8|&F+!N=2+7hj@9Oyae>61Lp_GPSYdbs~gk7oLCQa^TedTh@YIl12CG$E&6e7vf3 zuDwc?e^gk=6MgATN36vEq0Vb z`}DciW)BgI(&&@xRW_#}Uhk9lsjRY~&!Deht)T_=0(yhzr3{L5HMF=mV-q$Btv;P2 zyyzkxB)(ByH_0X(83eED|2m~Xc#Z}S5#w~BdSPhZTAlA z)(mP{B?dP>C|RIoPcG_%0=`jp7rW(NuGL~`b*C6Lny?fxLHt_Lms-Wz4DD4)T3cM? zl1-`^OF2~Gq#>$X?&qRnCi;`*(c(C411|Y5ttoh<=&R;QFXp@Cu}B%@N?gu~N9I#Z z_z#-|QNUhQa!n}}ZB=d6Wwlke<8wrYA@aPk>d!dN+}4yj#04Yn%tQHfZE>4EKwMYE z9alQVSgLT&+SKR-Jc+&A1lg*|2J^P2N_(MaX zbA430?qiS6v`ii3nJQ+9U*D!i!*-iUw)o@$xQC7dw!c|C zkZP%qN80<^-B3F3`l(4PqX%y80b*QLa@`+QrZMQxLGK_OGR00lIuw3nYjIRcJcV<* z)p(H_CZV`E@hFN*LP7s%(XLaQy+1Yui~rr^lYjh(otia*HZa$$`_#sMpFNs}ut(^#ppPm}U{koj>Em}PEixpZMBKp!;M@~|!X~jvGJy{M- z-Vr*pV0JMleS@W_h4sxg5;G(Ax!KQg(ig0;D3YFFzZdnU2Uxecz3H3mk-28Nj^)E$ z&X&M^ifx%|4u8zE00xlP%ER?p0~}>a%QzTQiN($a);KqT)y^w3-s*;{+F_PDe|3lW z<@`}4nwGE84vT-P+Iu3Ej@O$GYln!YyU?yXSTFpk`?6j4UcL2C-RXAS)_VJ&y5sG- zjrHUH)E#WsmDLxt>AInk_u7@u*B2jVZ!CD8!>9%=oX3?OW-l%LsD}lnnW;vXBj9^n z_HvFJ1t#Y{mj84;UN{1u`B?V@Lf<|!yo+YiX=WVJ6T4C!SsX(HTdh13XD9Dv>$NC_ zmE#@fF7%JIoHe_=9mP z>bm((^(!kmO1H8NFMSVpc1b^4%QlpJ4tG-NyKwK9rokP$>N&Vot9rw2T=fWCy1F~u z9;;*E4qg2e+?}h3>f9%4+11rUjG8giSjocHh>?H8d$bnRJHm>#&)uZG$fs7e+nu#+ z<(i?o^~hAWCJ#mUmu1mwY(iPg@LBD~&5Yxk@ubfr()?N+cPQry23p(dysa~4@o}Bw z;tV1^>L$`HnBJ(1Rh2!}U8}J(%&AA{+7J><1R-?5t=R+yn>&A1IU^e!Zo_Y)N zu;N#OS?Mc!Fk;~#C}2M<53kC8a3GpG zE~bvDGcn-<3+*Z1RW!Ax8q-Ca{iHN9|4=A*m9 z2vOxX*bD2!!$*0ARz@W8)$R9W?@vVZHkQ{}7d>iRMC z4;HiGkhT)eZ;01CUz-=du{HEqo40!794+qWjf+dPUBYOFF^`?Tv)5?$6MV8Cd>+0I zzCV1`V>EYy?}6V7z6=wXw;RnC$jv7SOhi?F*XN(vA}_|9Xiu+C7Qi$x9^oTs<3?ZG zZ!TP|fZ-t$sm+tF#pttncKNk#fzrv*p%kUG^;eZAGtX=0l0q1o)|%6IKc9B6xrB>~ zUvnhp%7loh%RS@ser{OW`ReJrJ&FAuAqSh^2q5kNNnd48)gDH=IjA^6=`+qEK9EPN zbQ7$>tq)|IYQ>L{#zssKq><$jc;>V^QHBmRj88*3pESRryJ)}MGz~4}Yfh)%{d_{n z@SX`&>~V!8aAN1noijsHXsV$BubDJ7B{1FmGoif5!$wDkQe2HGc!zq>RGM(F@qw&U zd8ewDu-=>AHukXM$6agnC!6AF$+wRZ{t__Q2pMvH4RvliFSkkRL!L<{2~HoXCoed zDEytE2C@^J1(Oi=&t|W%s1k}d#Z@!fFhKSPT^`bDae)Z;00Y2akOIblEYR^uv~(SE z=vcy=K_TL2gSii-TL^g(SO!+)Vz@gp#UVoq&>FN7tO5@=XdS{EKtA~YHK-i%xnL6L zDElVlDPa0Ty0;3;sCG%tvd!2zV; zM$eAY$?QplPy7q!Hnrbq3AQJv1Fc^#h+dw(k z44g=J7Bqrw;BBxB`m6|i!HF1k$Sc7rux=_UoPact1Hm4o-4AXf-68l@pe8>UPtOnv z(YmT9;7>u})7jxIPn3+t6JhCo!k2+{;5D!XYzI5R``|FB1|7XFpMyA1J{$FdkHJ~c z2nwP98h%IGkqZg0F$JWwGZ=n*d5bzY4wvjo@3*k><|^KaEa;Wnd*(_;g8Ii&r2}2wccC8~)sO7Q$ZyG@k+k@CRK$ z4`2qdAPx)wiC{1o3PyqykOgu#v7hNtzbnlN`$|EF~$?} z9*~Q!)gkp&=x-zZ1>{Na&%*y2d<=3Cw-CBTU=q@Qv51Y`8r_pF!B&N$w2`|G|0=k@ zn7zF9rxO3=Xv4E;6TZ|WFa=BpgcuLOe3Gw1bqov?~02pH$N&o-= delta 8598 zcmaKx30M?Yw#VHhhL4qb5aS?1EnE{i}hD^G)XU_xKr!aT2QkIj*VMH8E<~i35cH9ME?N zKNQRbLF)*w2P1(M%m*t$<+@!H`}M6v4yE3Y%ev&PvR%I)80^_V_~J^!hi)YNgLiiQ zuxi)Ez~G@tguj(b_+J(f{y=Hn3Tg@Hbz0)ZDxmIQ4ZVa}&q8gM!m`72PN%E>HcL`R zS550^CF09+NOtHy*?4h!gYvw2YN8_{geL?N@w2B0uh^=(afh)Pz0JW{9&f9^Xmpcr zy{&|>uX>N*>O8D(;0A56Bs&{roj}FcBtJv$Ly~V2{KN$kI~_PHbX{Mic4{Gk)bgDl z(NPu}ltVvcPX|RNY9xbT*7KR%S^M1*i@(^rfjE>rO3Ph6qQ0JJ*})*0-eCuWzYMFC zi19>Bg_LZz5_P+oP_d%!jXtAksGsZ+rz4Bp__Bh{J_RZ9Y$1%l1ts3!&%o#$gI-@SuPjpM{N~|eP$+*3S z@Haz8#Z8=S}pdHS;2sCC|)ls9k88y-^=`T2JS))OVXuq zh&~`eW_24;ly^?548|Tvk+}d6f9EFIFrFo%#c}uYgHXTl=M9&(8AHJd28t4J^htUFY<+n}X1xbBouPwA`dYJ3MJ?2ie0?ne*% zd%|nn<%4X=z|3J^xoIYE564(&!PjQOR|OMY$uvS9C!}62aNX5X1i+=xRgIMGZGu}O*lNk3(+ ziKpWBx*o5fth<7)!pB|gY*GPtsgv~`{1&&m^YF>Rloqj5Lr%qw>U_Ld>OU0|I@yau zDl4-)zgJ?`k6pcdkU zP$AsBglI&qY%H#j((-7kv9Mob-HRy$DILM8Q|+AkaNWESS}yDdPw%s$Jq*Je)b6j^ zX_lSSSmmgp6SjLEf4qZ_JsuVJV=;!<(@-V?i*grYyp3QtzMX85EaPOS#C{wV!Hqx6 z{6;6``+I|}Zs#2Dv#$kN)xv-Oe9_PC{81*20ol2toQkcSd7OQui|5GBR3Suf64WPU z=W0*j*%CW(q^2IW5K@-G*M~@j1GwZ*&Uu}m)vX^LN~wjhG$XBI@245L`!4oXTFNl3 zCs}Yi6J%ntT%*-m=)FB|=ODTKoZH#UmzM9I4C@?NV`Mgd%puJJuh;n}c4v&9E@PUp z$x~Js=y58JbgS=8Rny}~s;Fpl>jc$Vvz^3uw(|8}ryeCXyQz-HMl!m7r!*^1EgY>S z?IY-PU)la{_VU;mF5b=dj@4-^e71)!v#^jP#%jdYPNja{&905@r~R8}kMxvTBYaj% z2GJ8Yr1OOS2NhR(n0KtU>_fL=Xvd+`;{LwXBzT>NJt1|q1~pnz>S%CLQE=%ND=Bwf zeNA}tx<8z&kjy%LY?n<5e{>y@vWcQSf;OEfCEs_c@Wu&I-d}>C(~}~#cvPM(smCP= zs&*ZnDXDvJV{eX|8UCDmfXVMxqEN70XHb?G6>nT%?s4O>7o5lVji1A{dDvUyb^JB2 zZn;Xl<6$3;@7FihMtDnlAEK-v0`u1%_Wk%DBCReHwy1FI>1p=XNQ;$im1#Od7SBW&O#9aq%J zGAG4_Zg;80vmS$u=wpg>c;ctn+DXRWHG3?zVfucVYO&RmZ3wqkVPAexqQjK?U7Ub=T$Q*mI{dENd(wK&6NE>aaeiCjvK2ePwu3Qu(FSo@?f zZaZeUWkeb89hHga@P(XrBy`!Mgl^9WgdM){ktY@U&%RLfq(Fb*NwKA5rDjGIq}o!m zMr217P%+n+UXFB5obS>J$+ma{fs>&grx61ryn4c6Vd0^{^}YYH~T3aKGaEnbmXvn48+iYi$###q(LJLD&)O>|?V zO}&Y3NH3ghcn#&)hU1~X2&ud%Wjij+A$%sD+YGyeJo6DR4Rx z!p)JGIU@=2Q(m?pV+Pk>V&^hO(0;5nW3ZOU+grO_wkv3VFZ%lx!R?1-ZfWB&1S{?rrWo55UGBbv;r$VP#9c$WhwkGnf=+i z5X)$u%tmDCpsbPEoU9}$GiA0ZYeB$V7cpgABjQT-AgefLyz9L5XLCwww07z}sxb?A z?<4JZB@2%5Cbr7f%$0iCtn6U}3%uX)t*z>$mYXMw?k+v&>ilV81}Dz;e%)Hr9>CG0 zZ*CE>vy&aq{yU9fEGL1LPJfnuz`mUx$$!TAqr)RQcju1id$l7=Qop@NdYh1Y*nh>3 zI+&1K#7Q0OY;Mu>SnSErM^mzK&euc3Qs|)MMoqpYtQRH z;W^2ESDZ|Ay|V%_*~1%@Ia8syT|BiY`x8-X{Av*QL!8JmcL6$&tAjG%6WG-J`F*nN z$|1BJapkJ8j@9Ph$=lcAZB>chCY2a5}6Y!)Ad*k$~B|P zK~q#>o`dSE($gF~(ziD8m}c9WN~X!qhYp;S4%RR;jV7@6nbBO6#DZr<#h;PH*1%(g zKYNt$O1lk^mw+ZsW!mn*wFb@+<7YQ>dYS!w_6*K)j7@xE4J~B#PYj^5>pp)XA1+GW zpgF%#y0I>H-e8qb=FSzDaVw`*jYX+Jags=;64qGwdR`|xUvzOJbNXC#VZ{-O zOF@W*6B}9V)BR_8ou&?DW*pvNEK2mbY9*HHBJDKgJl)ozvQ-yHN6kr}Gg)a@xx`60 z8iL?i6n6RveIVy|mPhA`L{)gtpXNS%c~z%V@UW_~6ql%3ynP-DgmN~LeZ&WRhEc=(W8nv|n2P$;fNRjY|!lSzy%E;UtWbviG( zm1B+*$FY3{3H`jzMGYbL5Y(p@o~$h|w$#eb(;e(a!9e;wa}_Ku`wG=6r>G?%z-E!+ z$6;|vNtoN}_VL(jKJC`ap9Bkwrj#rn##{sMP~*To5yxG`|zAKYr;8vC{|anMz_Mldz% z@L=z`O@am|JJq>RE~upIh~;pDlbv2ReBl6Gg>x4UrZ?Fu3+HFeywK%f^Yv;oC+?OM zk4DipHW1&*Ej!Pn&ms1qZBm9G-iE$WNu43l`1t0o^N^UN>#}pR#0-m))UvZgVtI?= z%f7{F9fBb*LBAC@p4zA`vZQ3%POPG#Z9h+o5k}a8h3L|1f8*ynrMJr)+DV_d>tm(_fDgx^+Ndd2g`EBy!^pM14Ux_t8}EK(|N0NgD|JImr*tlY~V)Ws-!MA z2@TVfOTD<+EDW%AmO1~}SuUh?XR>x>s=42479y>k&c@F5zov2et_r?ES{Tw!JyuTD zmA3O5(t?n7ptG0JRC~3Jq{#*&uKkx=p_>(5qyG?ttvsBvR7o@2v^dzes(e?No^=MP z#Iem|3I^12;tK2l82@@+2*#WHol7I+)#`K@Uzb|Y)}{!F@HH-a*!@K@rU2RJY&z~b z6y6EOIuX8a5SZOZ=S4vJ4n>dkNU^R>1!KAn1O;$*Q@*;;@C zH4SYObEC62pVeVyapE~wWR^wS5Cqi^sH&#Zr41)DqL-&ewyku zfxJn?=i!r3ahR+Y?zH;%G|>Gm=bG}uyT9?MPT-4Fn>e}ZY>=Qnt~tSpCp$^JpAc>} zp^0o4C1?i|mFeXCMO1nx(H zLSk)d<4?6AQ}H~#JR`6vmDqVhBxJ0wIx4<#fr_tVnGutC4bJh9MByB&OF>0fe9q#J z?(bT|h5v2!I)^z}V9|KGhgpk$p>MHC#gph7R#rTKKFi)Mj)mRV`g5^0_^SM>S*DN9b1lQ!>^AcVz2-n$h2+V* zERp0A`yQrWKA2sZF$LaEFBc1$nWpwS7rI&Y%rut0yu$BvAEJBeB>Q6dh8}gniiu^4 zm9O`0>$3eWZ=vEj$5R-|F58W6+i}OTUu~au+um`k`_*=_+qTEC{a4$2-L|ccw|}*r z?zWXXPIuY*U}+udHm-48ILVTpf0@HrAA5cY_otJrZ^?%d_tC?UdR4X#Z|O@nXS-7n zVqeBuN*p+xj;_3<`U0`yRpWvbA4Ksxh?-a~)YYys(ommU62e)?p)9qMbb+o{p!Jem6TiSu?W+sMfs%Ewx0d8dVgp$9>Djt zNvS_Sfh%PP)=UfNv6Qk0Yff z0r*DzaRuA-(pH+rLSJ?&&)@5nFX&o!`<2M(C9eO99xL1~GEbr6Gp;*DP+j*3DHl~7*kq+&vDQt-_yQL(Zu~j4YTM{5b8NQKl`No2d8QLp;}Et~r6P>n zqJ%kH7{c3Ih9eB!8jW!1*3k$TZ&jXX%U0hQY%S!zaWV5Y3w5xS+hVm19o*RvI|t8= z!|B@_L(mY0yWA;uY}@o+N*`__+8oBb+fp8nj@>Q<*LzFskLB30eMZb(WajH-=VDov z^_Mo1a#5Rpk5b;Q*mu9KWg)MtnfCRx#qEfzY|*9BwONl&8aP`N+Er}COe`wflS`^e zx`Qfqcmv9`7Lk@Kman3=c?%T}Q?XH-wp7uTv`^1bfx<66;UJ_(Kg#T{Z=|=`zJ zM}B}W716jHN8fL~i_vzYOb^QGis#RYDo|z3Lq8rW{a}qd25}F=*#GV@SQUd<-(T2# zmNx_ov2Zf?OKRa}ZD95KNHq(po^8-Dobe@B+qekXneNv4UUL!hO>t8r(U(hlvl_9H zEa#2r&?cAcRLQb4$hRbA=WX`V8&l~c=6K_bvPf3!Oj5-DF7e-8;-@YVx9eFYZPTt* zWy%KPr-w6+qoI%6Pw%huI(tL%kp7SXkSfStkh059-yhQDQZ_?By$Sj+@NAVA^{zJ{ z)8kx&4^>fauQMJLfF}??@0Fc9yh;foat+IrDJqhNT(Q}x&f?kH-F-qIHcg78sHN+9 zWqd^WkKK9|Z6NFwcEHa*99NOyb#Z%=XxXcy2)`bj8BO@BU?ud`;3a^oh40r6`!9f| z;h&{{lV?5BDuHJVe1@^VT~7||`QYk!!o#;#+A8~(#zH6HPc(1|+y=kZp$D+HgUJ@c zF9yGrXV+xH?*ngwf7j`e8U694HiPi70{&+MN=1o?C$sr)jhkK!%W@ErN%$P_Tg|*` zCH!mXPhdX& z4zCh?430j9zUJdCfuSH7j09sq=nS-r^h?lRg? zT=#E)Ti{#J0`7r!-~?WvI`)sp|6~#JfKL`;>H`h*01yF=f>6*iHtUyQ-GT$4vYVG6 zyVJ|aK}61gqhL8$308xi3fiEzgU`X&fG$OYpeH>Rddtfg2S_~#1rZ<;^yK-q!?0Zi zjo=cfU5bCEl^)v>ISM9#6W~lYwUG5dkyC*ckqK)s(5tZoKqcr&@SxI0#O;t2(gXD1D7XaM#~>eNb4sy{ zK=E2U1kyf%9p|$J4w8lXDr0f)>^CHD$0`DI!BmhBCV)f`0Typ(e|yK!*K!Kqec{9? zK8;d(@*WoaZtkRIa#>ehz6!}f<3IYjJibdWxYLh>bR`pS5+#38JU diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin index c3023d68fcf9222473d9d748f6da08efa3294b6b..17226d5a92c470aca27fb4389148e93df24cc50e 100755 GIT binary patch delta 9608 zcmaKx30zgx+W*(uXJDhK2SjCd9}qo&+5t3=^I>o^91wBns%e3y;Z!)4Rt730mYTM( ztJ$DA#!Y>5xfG@y( zu;XRI*MKj;RnQC!C4>(x*)*zWka>R=PCN#6pu2lcaPGlW-47ELeRxHN ztBFW^R8vmi|FZGY+jUxh$LWbcOoS%{63@jS5?-@av||ru<2zfYR{DHRe$s*#@-JT# zA$dRBxuea#yl&s*N$={ae+in=3LG0o4AZkqEi$Y(Q@RGp=K)I`M8}r<|~WUCJ_{b5`ffUCMJ-KPk=?En3ZdCU?Yn ztJv-*JyOR;b?HebvPoT3dYet^dbxWSH;LI7QKlqXvxvUgN~rW|by~L}G|*3~Q%H;; z>dkh=I+{p7vDZwICFRkCUjY__DZl}~68@cpc6+yW`Mdew(|iltr=S|_1g`^b*{91m zX*9Pr#1vP5h1}mzJf4^%$uDVym%6+5vwnebbVYT3Ufc< z>(zZQEoZsi6Y2cwlI|bsY?&vDYsTj4#e||Q^Ev4QmnL;Z+lsbi5KCd>1!Y9i1?8f! zE$m!^;ZmhiWi7ayqAP=BmAxinv~Kr`8P>C*BDKpUK~&}S>IKEhab|5ujZ$AHi78V& ziAhxtTf?2jP9jC92}MScjqTOHpH)1gT(l-HD^reIiImf_OSwR_n%-3G!i>8_x1tG! z1fFA0hUEG+dX1t^I>)L*M$!SSIpoRi4!2hEPd+WY+oFX_*vwD|oy$H9?Hy3*gawdz1w2Q42{+m(S46&y!?+$SM(lIe+y+C3nYT6 zCNL*3D=5<_eWnOvV3tw(MB$=MSwf5uX_Ug;tT@sVr6s(zlkmZiUdTJhJm9vP;fHQ! zxY_Z@MO>qc#Yb5~jM4-pZdrJCgd-%A#0As`ilLbO!xgqDDmzTePW(_*d)-Mi?ZEg0(hb_kA+!tYWfhtEaTkA{wL!g$cdaao?)!aPMd?WEVT%JBhoplNp5> zoUn_{6+RkDT?WyNMK@F#;n-b}P*5i5Bw5jHMB3@TSx}1S-|{D zm?>>yOZ&Jm51x%Rb5nfmjo6ntVK)ovn`!d3&`jPLjPVKqgA>`@z7gDHAFJrwhs)f} z4);Aq|C`P1_dJ)>!mjj-;@)jx-hLan$6DAri-p^`n;o}U=`Pk}`GEeNy&iXfB{kWs~CzxHK<25&tG9dv=abplHB^{s-uGruGl#x_Vf|fU?j$ccn7g%89z<%SQ?G zH)AJhw(E%WH&!=bB)8qolmYW27mvb$0D6rkd>+^Ys=!WgpnB!N&Xj)5)<1HS{=^nr zKj++THX?Bb+VftbnG^P~PZ9_AH%fbIi)*@wK?C;I2gT~eC^1kBw$3F+=`AH@PMKmG zN0Va;XBMqi>;=^!Nqs4Gv(d>;u3%5KFFAzkey64L5y{!TMlW`0_S0>&s`3~X_vnC$ z_gdmbDhi_Wmd&opJL+xD;tWDeNJpgttVEO32E7N?;_FzEyn|r6T~D+t_Tj4hz^m~UKRH@}6I+L8wT>`fHC zvg{)kdjs7T0{yDjE4zH9N4@fu*0>mN3as%hUimj4yJmaeFwN(azhrBMn(1D)Z)jq_ z;%K^;O0!z@zl_n-y}K%?^dkDDJ7RT`KJF&I#wYhij&eqa_29>~rp;(!QNtp*i7jlz zFrnm?R$HGIt2l$iMH-~1y;^xTl))XrW=Q0kjmku;UL*-(gHOKLQmIU{8pN-vNI$yo zf^v+|zfx(3kBX&L^j*J!T`oP~7ZHnCshqODanvWD@@i?CaXj_#{nTU;ee#YLQ+0K; zzIhPU>baHYrP5)KFiVU2$rEJ3WDq_%TBcY9b7Y%M3xDb1>wxr6Zb)NZYlhajEH$6g)rTj+AH1#{^K z?Cmz^DJ*eqZdn8>eZ@A7j4`hJfbi4yvlAnQkfTUqa(dg7sIt`RAM9|mKSn-7u>+0j z#pQg!UKll&6ZWz%M+sb+hus+!6}Z={m;UaHb`Wz!k$^r=W#OY^O1wUMRd;jGOugi% z=0pc)RfHI%doCK&J=a=vV6mM9^FlJEYd%GJhgiJ7J?ni>V`|2n(lPi!;?2a$=Zsw7JBlKAK6T z!L4DEIvDyRtzmz45cIg#u&q05L(d1WD$ z?zqn=#|mP`_Q^`Zf-h5Q#~GwsiZ$BoFz19VG-t0?8j8aT^6s5mB#56X6#^$ED_Y8k zEQ54GVF&HDQIqkuES-taU+XsXQtY{Xpf7>c-fcx!A~;r;a+)3tKBOvbLB}d=X>Pi?8B6k+*mK0 zpK8?`q_MSZdukF_;bIq3feI)+91>S zvVWvi_V&8`vTqa}^~m!6IXjgL#Rdlz!U``VgO;$CtxT7>-EQ{6m@sa=kG(l&EUjTT z$J9q9o;cMkbo-_86D7E`si{+yP6}~fo2PH)sZOHq>QBT7${lgBYZ*md^X_poNb@eL z+e@SbAA2lw92ezddoy3)EkP8vk6T!LR#f~_FNII_p0!-LX4TARTUxgvqSS0sp24OO zASP6uo2Qdb!Y|00fyMZo!Y*bBP~K3OH!B{>vsl2{PxvqO5=&YGkuI@K*>iB3d9p*f zp{H0+$2G1}Wr|~9-T&-uBaM?5s#lw~H~Vum{-2kK^mh-- z$@!ULs zO6d`>(Nk2ZOcr*gCN-FFW?WH&W5=Z3m}e5dQG&Mea^g667NMR>_=Gp+8Qyq& zPWS9pVzuXwyU1y>iF!t)V=3;dxAjTa=W&~}=G+-n<#9KEeVfoM%p4*;<*sd}{FjY> zaQCIxr;Gp9l+*s2$!{E)1YO&O*tFc)(Nn$Hi7ihxY{W2Q(_5R5CgzRV3QME}e7bJ)nzcmZ~u^ z3x_*s?2VaNS&P#j2(x!7cIyy_EpO*j`sQ6EAzfR6+;g)EZx)s*spF_r?$XNG9-^1_ zxE^un@1H9^qujCj;r&b2{*E)ZM?QB(u_W66&~@LX^iSkqADGzBaYiBeXV}8L*pf-D z`Sthj6zip>t}v9)8zo#!)FI`S#9k<28A=FF%DcB}@h&AK$q0K`QV>e;=A-1Kg!D6t ze*77we-ic}7sf4x2%WP7vaoa3YV-cI2~H99?wM;-Uv zbCp!(jv<%m2+ePj)*SIP--Gq;fmG={?f)C>GR|(d9HIJ&nT1e7)S(Dx`f|qz63Og( zUf)bR9rw5;x7@ik<}qK2h#0#^b}1X{a>smgX%?!vk9dzy%PIaDe_J7zkox}OCruN@ z)7Y~*eIGM|<&3|kdBp=KjOV0%Y}JJ4XfAV2=uK0qLnr1_T3bDD(tS$5sV@9Wf=)Dh z@}#->ZaLD!!yDLc*SfE{d_)^YJo7cYpvs**TsBw0Wv3P(Rei*M!oL{2VKjB<3ber< zv7eovYU6%%vw+8it~XB*(h>90em3m!kGSponfvj{-41&6&36dJH>AnVNxNC$v|joa zSy9ID>}!vIpJZGQzxfrm1wg_fp5?S`^HurTHPQwyGq0Hv7;;e#gFFQdts4V zzU5(mpOzl8*n`h!4~bcQH?TtMhm4F1MACfCXcf!x<-|#oS@QJ4*g*#f&)1~A*u6@Z zsmeWZ{--V0D$9YLlb$)r<7{8b>ZXVL&-7@n4=43wSEtAJY>|`eO->VX>cz*aO6St4%M39_-RQpMwY3 zcTYqmc;v_6^O~GAIUwP;#$f`kZ0gBDeD&9qAMq2pL%a~x-u}a^_mcxM>bwTgQU~W! zdLM3-0UjI`JjnCKqBcj0lPX(O`BCMJa)y{N4nt~mMS3xOeiv~|Mn#qO34KSk|54A_ zlL^$s>YtpRJ^BP#_l87=`lq9c6SHour}wVpElQFax(q7UQ}{TDHUy1ZPjJ9Rk!_fM1~>qyt4AoIL_SGlsd!(Bks&Y`E+e@8xJ6^E8>nT zonkCiIA?8YbOJ8K-fMzx)pUb-Yf~lOqxLSf_eNMvVYdJiZY?%mqxbC?= zDqZ)nM`v254)RPDv&64&Q=?&sFZjL=X9&{ zA~j4xadF~N6qkg8{?Vd+PHpo3*c2@Ocau;4@gsKXsS&h+xt_XDZS411qiG0xWOi@L zv%J|oBAUE2u_18Tr*CWD>L%x@LxeBJzU7mj^0Ly|(KMayoSoJ6S@l6v#~yy+5OdFd z^wAb~w3vckQfZKqDaMOK;d<4(so=b&tnObID_(CR{rP$ld$WlM_pl`$LCF>qoBi~} zfWG*^H8cm~Gxk2Nd!LTt%rCH?o*u>hx{UQLS~T>Fzi#PW=8Duv!3UqFIZzyBt5}?FX~PAvu<;G)3?|obIf!t%SX73 zEk^h>+dRh{{+MSz93ZciN9eN#ILefkad4&*i=7RuaZUoOom*zS)eWC&hgs^pRUPV= z^G20uR=!3%EdHr#w~16bUT-?A9U_|TLc8r?z3`{)%XZuQ_0~Uar`v5?>g|8pjxN3+Z#TYBUwoLoIsXL?qZ+heE?0V(y}aO~9u~M}rW#$2fcJ6P z%QgAa@J9$4_uSF>= zf1#r_A%V5kQoV0sA?-Elly(c**5~5(<_tP@jF}fF1Zgn~%COvQ)P4;5iZb@Z;hORfC;Sne3}Uy|Y9VL8U8EIGjKJvey6SmVHMrv)tvO;s5Y_7(PtAUSv(D(mBtKqT%Vk zG=*S#Qt4y1a(O>`i0xY*+3Q_Ts(7uaIh0CUktQwu=O!%$tG>GYD~>yMnC*Y*2jdph zb@ScoS66V9ZeidG3y6S0!J5~+Vxlh!xtE+|>HD{)=l7+1nBmZ`y&9~7z!iu)d*{I#er&hN6owaPm z>Y=)I$W*sF4@LNwWzlPFLRrl4r`nyH8OJr}NuNoid9^t1Q0x^9w6@iGTW8Fo<2uL1 z8AN*2O{80Jy-^pdDto%S)_~4%yTig(!-po(@K0Dmc^3VaEiKo&)#H`n2=iY_Mp)h& z*1f`@=dRH@w_;5=!mVqDAZ%Qtb@Be1)?uh9VkLHMSmW!s=F(K|zCLOETrJLuxIC$J z5p_(Rq4}%Y?B;7!`L@d8p62vjdol0S(tLT8iWJ`NZk!_>`VMA(Z8g2hcE6U9xcS5c z{IG^wmhd}Gd+~Fb9eFy?Tov>?JFGyLJr#A@v^W;It{-O$X5zZ&?wUcW>4mRkKDsNM z5LJGYy|^wse3Vycbz6&IvEvGWm`fEiZVXj9kyWh=5B!@)l~tcA`?sz;Rldrut{X%D zU@_|tX)EFU`gl$F+C=<@)~CnX#HtOmwYZ-*EGp4<38NX#Ja+odUZdGh@X3CVJfsfN zA5!%g&7B}Uki8&fxWK&KXtqFaK1tvrs`7h2|I8M75$;5LdVR70rh)NzK7uxG@U{Ko z!qo~G9wL$2Jn34DKAUHkU+)$uog5uXQA%5XRe3V=ylyTjgrjM#Iequ@X$PB2xTyHm zM`Er_h={t}Gd}Od9+G5 z!W-Q3K(?t?{1jG?ep6^P9Sh_RCGv&_cfEbo$-TCzK5D znLx!JS4aXUcD~#>GbDwk8X9n$NkdZt)6G8<%8NXFbaW`i)tG{Ns0U4@3HKTw$U2pG zs%i=Az40Am4=aA!wN`(!F`kxu`zYZr0fUY3vEU2nUxP*fAEos#3-(+v1&sMW(p-Z5 zDws43KTd=HRL*YL_kq}vgwFy%|XNh5Q?82hJyc8&=}aWz*alqo!~#Ext)UF zvB9t4|7!H6v|fbYoJRQNB7VpQT0!M_Ue72SKK86;0ShzHZt$O~xs-)pM8K?U#6yNc z?f^B=o!~5(glGS3_9}}ip}13AIin2&bbrw0A)6K#i02+)02mBXz!;DPI$nvEu0szU zOL#LVMEoo;=b>~9pf3bV!SY-TcSoi;WJm#8gI0i*;Nb?X#q)ZQ5B`4*Do1=Sm;^e? zz6pH_nEsIM9q2o91q?Hkec%wN0yUr`?BK_*ys;5O17f~*2H`N6n)hEj;u zRXqVY1%*#%hc`b_G8$KerTYk92G)Yt!Dg@x>;NBt!=M^;^tyaD;z0Q<)C)caXF($< zg#ByCj*Dc-XHhFyUW^hU_btMBLf;K? z(X~3Hz6$$oJbwXw669IPuffM47jX+m0{TB<_*e%gL>0)eEC`uc->yTH${YC7R zEkBj`FGCxiL!0oXCV?qnIw%Ayk*&uP%z~wOyI>YYy(LPUtHTSBQExLH`wUC7w4g#&XyP!y!-wYCs*}sIKFMk@fg2+KNV2fp0-a zL@soH#C3tp2f5G}Lf!{~pa<9i+dD6!VW92>R0vxrfRAE_w`iw?&3^0M=(8J8Jopt{ z0yjY;aDsinwSo2B79Di&Fh1qr$7#MCMKivAlTF>0=O6_C delta 8598 zcmaKx30M?Yw#VbB9cI`(>gITCK>0`_e`EnHhhL4qb5aS?1EnE{i}hD^S!*+-}k%c+;i8u zb(eFiPdK=H?{gCgM|2ya2tT)w@UIsTKB8ORfW8U#CrPOO*SKHJ?uk*uP97xu=YYOL z_@Q7f2wqQkJs1hBU_Mv{D%bCx*spITawzqFT-GITm+k(;z!1+y!WUN(UbBht58v7S z!|L6afuX>N*>O8D(&_->sBs&{roj}FcB!5HhBa&|t{KW+lI}-kLXoc&&jB|z-mKpaXQrRA<3QD0BA>`<^w@3KQ7Uxrmm z#CS5MLP|DUiMriPs8~_=MxW7C<1fA`6QfR0>n)Ob5)qr(e}%@*A7dbVG8hDcfvhKd zneKn{ZMXfSZh7Q4@@<7}9e57>4rH)=O{_XV+%B`#nk2fe?x3cOQag(cOAR~XZAfpq zZh4|V2iT&pk+h8M4@;&?>OKqmqe_?lL0Y{nM=cC4+P9n&-|bMOs;IhX-!x)ccYSaX z`@MEZ)D`KLnTXHIs?rMSJhXW-5l?uIN;e5R-+vYz%>uN>neo@F(t~g71lhS(zA2eG zPH(@LX7LyAHxXln83P3<)(B?R~>bA@J7d8sn%TZ^JG;8;x+Gq(2h#C-dz{}~Cmi$GH zi={*vI8QWtDryNm$?Q?l{uI;qlbE`1qIPlvhIUq_cV1VCiJiKFG&|;AJ#X83F1j?M zk)ERB+iZJmWPGWH=tAo?*&1u#Oh54irkKX+CmxkKL#S0})W!OXZi!uuHN`0zchnI6 zRw&WcKt8}E8rmjL3qa?;?qDHttGPOfZHzN({lt-SVrg_%j7^(K5`!EXAyV)YV`cVf zTvn7)oCFZvJrq}}RH{*`+fl=Y4H%|Yi@ju4Fd!U?*UL%=?BL$_GJlfoNaJnC3I-Qc=v3lxpN)t|I$H|L5g&v&HH@_B1tM0v z5(>}27Rws-dK$#O)ThR9V!P~*S_q_Bh_)pP7O7~Q$>1+$$SlbarRKyhWR__#m^kqx z*-sejYjH?t!Xn8YdYJGvn<=@cNH7PiKU}xRprtgV?zB-)>8tEodLu~25%wb=-X(n$E$5?2=*Ji?3g%DlIG(w&vv|cT6;w}$s9vII(IK=!1U7*dZ zY0x^uRdTzZ4TX*^5If zB3nB4N|Vf-pi2GnJYj))>=^A96%p&1X4nMoRtL))wmdd^B6ers08TJw5_WQ612gMx z59>|oXRKv-3ti7XGyjDf=wf@57g9BIChNH?hgnq0Fq5Ac?7JXCLX(d9o{22-TYe^(oo8 z#uId|#7-QksYfh?lx6Vsp;F-hF8P!5Ugu|Z8%Ap=wJ?@uq*d(wG$Z%G#lA{Q8K(6l z3vOqEOiY&Rv|011QkcR)%T~W>4~FNRJ6Hug6f>vPU1UT`FgKYj}lwlRL5f@8C}0inw6&(j@6R( z5p=q*Y=1Xa-O;+as4*SV$6M{lwNzrGDPcu8-}f{kvzc^o-e0_^g%; zq9<=k7YO|~Dz5S{?^tcwhi=8tjzg!#{e7!R@H&rpLhEV`YP6)((cq$@;L zn(*dze>z_wnRWWuE}Ihm=sGH86GeLrZ8}j(zVA}ujT550zXU<2Cq-)Um^@ojk4q9% z?K(PBQup4@-W)eG{5kgklmG2Rp> zoD`?o;Zlp|JO&%l#}w)C#LuvGlZ?M>_E>7e^!+l`Vyh?H5N@r)zWky@jbSS^trE^{xk5Lr8Y~Qq|;-IVxx&wu3afZuWq$+wExs)6aWoPRYp6J%I_DNyf z4$N@Nh%(+gDihD)3pwvd=(0x%-JTH$JAC0|Pb&1EeWB<{f&S2wVoS+N&5SBYwWVf_ z$c`$YVy-W}9O;}m-=!0hZSe*ICqq5XPYjgs>IsL9og^qLhYq^?`zvijW>*Z78cM=3 zkkyjEko#yuAr&unT#+sm7^lZuQ?M~sNDX;v@v6+4Em0vP>V*df{ZlYbeJy91s0PNaaN-+i_tI;WP2vX4oa}t9hN8H~&WbX77 zZjQvv8A*tr_Ob;TGr0Z|JD)Lv_G7IXgSAB7(c0y*T}Au*(BH2h?;T}HQ}%Mr61y}d z20P@}Q=D;IJ1V4=CB#T4UXk{i)$I?|pWdMHe(eoJr7W(WrcS4G*<`^H7twgJUDxOS zjXz1@<*lu~YE?Mno0qBEd8!gyq`J6iF_haYvGJKjA(qE69BDsFRVRq(abM?RK-x*0`j(E(#y$U2~<>Y*x%~x_w&*k?MC#E3wjpgu%5}m%;y%+25=S zv5e-)Y($n0%37Jt$x4DUQ)Zj976i_95mUx>BCcW&vx;NJyDmt7GpD3RYp33)er5sh zeWd-KWWf>M#8%syxl%8il|5`=f%iMUwN;(ea_dyly=CWJoj)zi;KbS9uUl){138-X z&21ufcCr)M|DZ997Z#PvM&qi{HkxpEk>w{EHs!Hy$x+E-PPL85 zJ@R{X`%yA@iZWHXwTlZ|FgxFul=3xNHSQ8aZ0cXGmRyk@m;*3yqB+rarES96E0QT0 zH5rdeLy`xw-FbtDZ}XK=|MH+jEzXm)C^Z13ZY8Tw>Z{}klqyB3@D!9GX3CG|mqKh9en&O((~B6C7zx*m%`xn@*3 zXo^bAb5MO%dYXer`nDz>(`Miy3e1=hl^4- zXwEN`ZmNr&H&`WK*{0}+tID2owoIaOaSaF2n zQV?q4#6}kTZ2wtar>R4k8Amo6ixPdVT8X8)NIOkAPq%ldY}LilQFGGgOjg=eE^!i$ zhG2LWg`NIFAISNg<G?%&}Nb1 z$6;|vNtoN}_VL(jKJC`ap9Bku&^#rn!~0RnfH*To5yxG`|zpWJHUI{UUTanLolpI~a# z;lbW>n*=|c>{RDMx!{tnBbLJrPIh|P@Pz|#70z8anBHQqES#S;^J15S&DX2VoVZ6) zJQ_vY)IfYEx9q%tK8M67Zph9p5;H7HQp?T~iRCSd zFZ&j!bts0s1pQXrcxt1%$dZz2yReFew*5RUMi^lW5u!`01B{>VlHM+FXeWK*ZkTPc zMfY$a{ZFaJyl)ycZ4kn5JY1eD=H(AA8YmLWe@aJ7I-R#WHwtrVdl_W|!3J&=u1V^0 zlh80txzvkW%)$U`XPNUao#jGWcP49BrkV$>W+Bqr>1^!W@M{{U@2cP%q=g~v^b_S& zU1__nBP|$d2RnNiO|{qBNSbUg;@W?u6}nl`{q!GVu$6~XmMUpxn-_=pR+aAx)3eSX zl{mI}Ou@ifPF#r{0OMcJ3n6%OzjOHsd9^wn#@D45w6!ThB7BWY9`;~Sj44p|Ih&4q zPI&-li!Qpg-0NJ70an@z#g&p1#U@T97g^2XcQEO#c#{>h!%ek(+@a}Iss+0I4)h14^Iu|UcPxzhW#8aIl-d_l} zn$SeHi}BCJ(RbLO=k#SnOmcnO$}5#^Frio~;Dug|oS6LqO`+ztFeXq~b|*Eh{S?K$oz0iz8yHJ&THPdCj`irhGFB zwezP4UxHo3>s;$$*NY9fjQ(6~4Y?*iZtdApb*EU?(ov(Hb{T}pcuI=(nJFX*V=#EJ zE~>-PziCKgf^esm4B;Ilp{14R9$^}~F?J97u=keE3i=y7Fu(S2LKTOi-?BLF##%ON z*+lN(8n$-X>M<{^Rr(M!kS5r&Q+%g6Cw}g-+7!Jea#6wJ5>CA45`1xWdm1q=Ojx&g z9d7)phwxW2=}tG<$Ah_9YVO8eT1-(wJg_1=ed^SY<8P@gkE!ytwQqT zU6Dv~iG45AuNcg(&X@vkr+WS8w`x9x;u`LDK5yKV0{*8gg|)NR}A*zv3Fy>8n!$J@W! z&UD+#9cQ|1eXz6+cN^C_E}mjZ&%exJtdBpxl>6%`*0R~EhP>dPRCYVR(*k3@#=BGiVvdr9Yjqm7wc+Q8>zf2=EAXfA4x#3dVfGN$~aRmm~DZq@qFIF6iV6U+YY{|wr)zFikr z&Qbad({K1MgdH0O!M)dS{0qXijqf7dx+w$UwN2{~POTV#aB;=(2;VnggUa?%8%h1B zO`Uq6pk&&5cCx~#p$8^$|Y_3eM)(|V&C(+mW95qX4==&7Plj=vPG9h*JeFFW#DX4XjicjGqI>_PcN+| z=?<#c;SDI$T0~l|SiYLt<}Fk_OvOfR+EPVV(mp*;1q#3Pq=S$i{TQ>qzKPyplXp%_ z9{B;jR7B%)9Q~m69!A@ZGCe4#E1o|qsz8-B5B+$g^n*3-9K<~eWBy$Sj+@NAVA^{zKC z)8kx=4^>fauQMJLfTs|@;FX;_y-Eopat+IsDJqhNT(Q}x&f?j+J$*EfnkGe3)YA35 zGCrdG+aA4&HW2m-JLqp8j;qM(pjwbvyunPJb@Djk)!uM;3{TD#f z@Xyl!k!J(aDuHJVe1@^VT~7||`QX}k!o#;#+A8~BjcF#}Pc(2D+yTGUp@*=ygUJ@c zF9yGrXZK{n?+0&!|Etp@Gy3C8Z3f|E1^mwjl!_7&PiFJq8aKTdmK7i(lkhp8;0Cx2T0k3k z2zp+LlCMX%<15a`zz+n1V9=AMfgS-OL2M3&yC>lq3@t$EkRCDtJlUb4h$n+zkpBO5 zC=q$uk*DK{&TWOwgE)okX>T(0sX00fGn5>V4`ze8peIwubY|F>RMry>oQ;1*pTd0n z9bP5)7#w>Bea**P0z*ME7zxGz%?z}Q^vlp+g?<~d4NQQZ2u6ZJ#EZci#SSKeNZ38m z81R`Gq#0xquZJN444sDtAt!@+WVC`D@cJzDA96OB3+4mAIhbqnA?LCM`xlhm#b>SS zxbEKsx52ld1>6VizzMuSb^M=?|H&fc0iP_y)CYdh13?5h1~i~&Y&I;#x&;S8Wj8ND zcBhw-Lx`LO$G{4(3akM=6|_Nb2cLtl0bPa$K~H)t^p=+~4v>1F0TCb)^yK-q!?0Zg zjo>nIR++xli+MOwUG5dkyC*c-Su5_1CaMm(Aax%JS8!WNA5{m=s; zHNX$-2DPx&gUiSt{5Xcu;90;&w<1=>d9h3|xloV~~%sIi*-e zpm-e~0%@PXj`P_92gyQxm9e;Y4j7VmU=@M6U@FK56F?$}0E>69zrSPXYdMYYzHnj` zpGGM?c`pljH+RxT*WiYG15(g5h$v>%r~zC88VzoVCSyRwsEHc& z8b@Q2(NPnZXh6h(+G31OmU+%dmKmLBM#mV$sP7V&s$S@R{{qSLeKYU%_x~?QzDcEl2xwDP2Rv=Cwpzfq3bjE%z5~ zIUCqzO#%@Ik0)YqArUXUTC;#!`~%z$kzWP{PF5En7o31-c82DJ<$jc=`+I?^k6M@M zXd?UrZ%|I~|I&&4{d%pxUNK($&bI-O8((# zA|&sQML5>t$?N(}9)Dj~^K(dX=PP-4NNhSM=|P<2#X1EZ-e5^9%gp11lx2pr6m_MI zX@XB9tQN81MT8wTwz<@0DcAD^*`(-b0dal70CC2P+IL2(Am! zT1qV>kXnB9B~mWC5SZKVyyJM9PJYlBkP{iWPhBQ;vUbYar@mzNx{EeFQu0v_CA3vp%SV`6! zEUI}7;XkoAgJR0p81Y8HVlV~Rz@MalCGm;A>y!Au4F8(uJLoCQM5m634bos=F}?w$)kl2TD5Qqn;BuFbJ@ocy#lv+1cO$& zFHdr_zegAaYc%sj%!+f~B>QktWm2yV!lzz#w3kunw~u|@D_P)s znXgnV4BE%$N@s>Cs*h~IvKyw3wCyiUEUc7t{9c=m@V(Akg%x5Z>Z7%Tcy?*`k(=Fd4PAlpyClypi? zZF!dvKF-^xcpu8xrZR3Ynsj_6Dj43@i~-KLnX0{FKM{AoPRZmCh!_OE!1LYxHN%bJ zlrE^57;B*P8e7`iAryL8S)3%~dDzyt*Mvd)SxBF(pogtAOSFe!z{0`cBsRBCv@p)Y z*7fNvOxw?n_Bl@f$!7L_SunM-t9^S4+gh2s?3_1f z;tx}mjYx?JK09pRs_`u?MY?M|lle<5b^XdDHg#~32!fi0jK>;;EwRu1Sy=|#5j zKW@>hY_a)s!RcfplM3|$Kc}+YNs=(=19mQHKtEspZe3YzXE~(*frgMcz1&j{mcz_* zi7(%*#+Fv9DdXwD*rJ&wt5r*3O?Yx2N;w-d&@L?bpvE&WTnN3}+Np}$Lu>VN=N3QR zX0unxVDUrx7u;!0kf|)m_S-2Iud=5h#a@<4ND$Id{tT95kUa%*H!R4vuq;Iz!Hm0+ zWKk_6yvk#@wtZHr>&92A>&=zwRx_4X!$%*mLqieg7K^IP2?H zKJ}7%|C~JT9f<9pxs@BK8R!9aa9C2`G9x`e`LnJ1pELCIz`k{qFGRm|Rc1Ts?Q9ZjJxVX+Si|V>ZsPd1 zw4+*C&*9NRb}Jh>Tq=LPO&8f}mWxSzj1Qmg*2)`zGK4DZh(xK~tQMH{GD(y#d6dho z+tg`hANfz!q%S@At$Li$-%HB^II$wD#E+Qs*n|j*3^_WLF?bgz?;Ed|z z`Kieydz3w`K{Yi-eaj%K)$>E1oAP}wX|`ti$rWP4WRT7st5nUBA*NlYh4)?07 zKdrLsBPI)eUN(HBWWMi}=Ic21Qt1Qi=4LZCpP74S{m?`jwEcFujfj@Cu5FWtAf3y@ z-WYj5@_8n43J zyoBF5*)O9OQ0zjZdkBRevR6ir69ye%UyYW8*In%H=$^rc+7@69oWS=WX8@H-9~8=7k_IlNd`5{0c^&p+{)WP$f~pkpOb^LAvO zDhccu^`quT=F@D9nMJh3e_c?Uy|YYUCuL6Cd3h0)%DkR#f{?%rg`q26R$a`wQi$ijiP zf!Tv{A`2;B>d{ht;7QIB_-AfOPO|kj5;!jU5k9=!f%!oMKFLAiw8cgH-kf#5x!gItUOiqIJFd4i87sb0tsC#de^AXvgUyf=xzLckTD?>jS(x|m zi$#+BnYvCAc)Y5mjJEpluT=JtC1vyrcsZs{MCzw?8*&A<-QJLwAl6>)x-LYr=Mpgs z!=MdK-2o!j_+~|vTUpYkcPq;r zdHl6}mvVxAoO((a?`AKgne{$=Y#rO3mMpyHVBe-C;Mt%uA$@`{%E6vVA4Gp(Z>A@P zf8pKmWKO1_{vBv<4&uoN*q_t4_3}9UvTv3gb1BN9(!J`pWj;14MHYRJ3|hiEwsNc@ zeCTAaWJC&YdD!-hakQ4*%4q00@Z{+hsq4>;=hU!@rlx>u9glQgpQmpTsgCQMHRt4L zDjajL>zO58@*WDsq-7t~9U$D`Vb5fZ7s5R3K-MdwDTG4zNh?dR_Dp!$P2pMn-n>S= zZr0SNTHE#^qSkIv7hu;2loPAJn5W}*_y*Qutj1+3yKI#p;5c<#6Cfq4_633=VCcIcjzl@Xl`7&Ps-V2KBjjX1lvSaB2>~Q_YRC=|7M=oMfQ`8 zRIb`##k6)y|& zc=!)pUEAOp-kj&N_1V(U>{W7&>z4<}8I&UH8R3Ugo!6T6$v5T+Z(B?64ytxJTP`*e zYQ@YUe5$jqg^FJ_`oZbvZ%vo~(p325HIw@SDTS}3Ija@jzM&oYI6k^9dmgZEd7Bd!X-riaImVG2~thNY+mu45; z!qR)qq2=BkuIImZ^s`~a{Lia*%?URBqc6(NtEMD9CPKfYzP9ttqh30%j!a4%dtS99 zRjPdR`D8UH|GXNWPx-XALiCUBmg)I#9Wlt<3z;t``62Vwq)235hRk8f5o~8(oV8L7 z-+Er{mrS|YVZW`W6Dcq|(5mMb>)L1f*&w)@BDH8^MZ8`?#j}XdE~5AGy$abEHf=et zWOV%Wu3gUHbZgjvXKm-JWAn$~95lu0X@VPnTF3tmww*4k4U~Q1mq_pz^!!~_kk_Oc z@W!<7h;qE0~)yh#JzyaPm0d8p@B%+)W)RF^p!iyJ>+xMP9@um6H0a}GIk~jd(${I z&gxX&^5}T1cWloSu7{5D>f8)+g^twxCcd_++|}|B>Ia9@_}8@GeHauR-cF^D*H6wu zZ1fI8INy_-A#sw`}*~kBC zf1namctbz=TvLI37F$-p-(p9qwQR_QAA~6$)_vjxL4SzNo%j-+!fs6LMRRLtK|ZB* zH4`U2qO_^T`kO>uK%y&;&&_u#eO>IesnJq^OUM=qh3vFK5pURhh={L-Z5l&uxT$BZfK#;kgyc~K)$$7s-McEbBpzd{&5T_o;hCD>HCxr$c9lj&PEMPgs$=? zRT1@}s(SMjsq6eP_%Tb{}S&W<-ZYIufv^^(_@d$HN?&dvo?M zJ2RtaqDv`+V;f|r0|OK9Hx8F@InxmK5x(zh*Hrn*!VyvGDX+m7p1@Bszvue*z2Nqd zP4(~~`BC^vK5)~hMeUxDz<0NLl_Bie=X%iw_UdzqG>Cop-1KbQX^b+wb+J}& z7Wf+~l|;KHy;|4q!F!c#e6R-D`!}oU+Uz#eYu0R)_U?VX-LoMkwb7$Ac-gqZ1ieQ& z;AOup>^=%+WxM=hzuTd{TTzExJsZ<)Jxh37^LLwfWKz>gIjr&V z3tH}hCB2dV8+BiqQ!zMJmCF@10kK{qVX1PWd{Na`nB}@m?W&2dEh}*-kzNCqb*w7z zpXghf>s*A_OV=c=Pyg^Bu}$q>OeXEfeIbd6e5+A2&&|gx%$i zS|O)Yx7TKEuWfhv=P*XXcvW5bt5GntH>Do7FxdY5R7PF9ZS!H+y1;f;4Tv>WV@pi& z8WVBP_4_8sW=-}n>}=YG*Qvcy?e&rCtHzS+Tfx4uWcSgrLAaC<{P_E)Fegv3l;UUb zA@)}B(6J&WS*6;yFskk7C5pFK4kpP2An>YFnr{%5v$jgYViovYS46f>4v9*U!>wei z5svw9Jd`UQ7BJH^HQO~+w#pZqsjttj6q&s3QHEkdQ2vugFW+oxKiNqGE^68?EZ3oe zj*HAwkWo?Ux~k5y{(#RxffqYTZ(j_C_Q3-`Ys1W*^ci+|rlH)U{L=c5CQ)na%InDX zGZ5u~dV%Xt(gfPr7GqxeU|Z7;uWoWey|yV(=KvgAWp<20y|zUO{5JCZgiVN;ez}Aj z*M0{1P@1VB0cpM3rysxV_^C-NqZ>ZV1LgSYf%U&s2jMD(pL4VEv%>mio~Asb%@^U9 zCLw>kd<^-Mk@Iq^_IdQ7`^Tm*`JYW5<^CDAdDcjJiCvuah^Da5W{;T=ek!m&wFvu3 zksFl+$kbpdfdi%6X^}Y2ZZ2Zk1=Ui+*<)TM1!q@EwTPXz#XYk`FALdco3*d>Ap7|v z_|U?B>QNTBS-~759m`&xW9_oW`?#rNOTT)A{dvxi?yXLvoQeUU*ekN+1bG;It_CNI zd_K~*4uzP4*zo5IIt|BHtWQgr_N~fxKHp1lEN3U5A1z#1#+=VDirceXt1Y?@jkDz> zw>eG%zvZ&pGB+4L<14|*!4U>P>ee^{4PD4q%t}80}!xQD^ zhGj?D+!tOEF#JEgFjrW2luej_rke?#T2QSnN5VU5aSOJag+cZMEN(#q?mKQR_!J)x zxc7K*L}$(0)4V#OClkbcDlJzKpPXWiNz{EB^x_T@{M3+p1WE!&W_yaL=k?I`w27yS8emuja;l zw~a-vmSbvhIjueFZILCrN;hlQ^J&|jc%XG`#p+?YH;}1*bsmcFue8$ZY+_~Xh*>TY zyZS-!y5bX>Z>7&7d>%ab+bHKp46?Q>>hX7uSi9(i&UU$&@F7mZZ^JW3U97tD`B1IK zPVmYjBUi)4CPu>Vv&1!4`XyVsM(b9$*GC}Ce|;drHEm)2>kM-4TCH;{)*X$`t!4J7QaIO75KLig+#B8XCgfpdZ-lXmH}qiS{6SE_{a6-N zo&ETbQLsg#9!*Eg#G(yrLcP#ZktRISCr_BG+03xz@rp&%HnmuDXSJCv)_Rp@uYe1i zvkxp~qFqb#)iEkl$*b(gk<%gXVTLzXawSz&J@kYVm9;@Qo@+L!5FG3cx?}SE{n!4a9+I1cJ}IqsHo9ysqM{{Kx4sO z05Mdk2HYKbl_XZZAu9M2msjz6yo!I@n)52x*tHEAxF(L>__nqZPHs$S+YH*HkWFn* zquZmoo91Y?uQx3!*LDbBg9vBLqx5k58vJlJgd*0?h^~nJ5qClyfY=Xl55zjRw%hp{ zOpsgQ%WFe+*yEqos+8b%)adpo{Xhwrf#=g`<2#=AKVXD96NZOyQkTcC$7VckV4uAe z+||`qMtg6)$}D#CEkk(`yi{}T*$1Cb`>3T{=$Wv(D)#Ec=$_wqPsqD-X-TJRXCHJY zmWPCV)UrL0I0L!9+LET-k94(Bd7|2Tyh(nnSiN*JoXH)Jl{Bvz|4aHdVtOErqR6=J zG}}>z4mC_jM>*$Ow(BlizHgen&$~n=klwGZQhwT=Vtj1X}*x z5F%ayJ}E?u17AYE2pR#LnYN!5`dlytWc(j#u0VecOd5`lRq*f1*$@3e5I2g5R)93+ z|58~g4D&$*{(%?<{#`*C&|ASy3lZ($-=%3z#h=;W9{9f+y(PT|5#PoinagGTw+^&| z*5G*~3uYt6UC=aOVHVmA{#`X!toU;T%+7`#aRlN$pcb+nTmX~s?4QlvVLi(!t`_GP zx8noZA9Q|7r`dw>+zs>xgF!0D09MfPO0;wxa>O_y8bA^3vq9<8=^!rzOTqG740lJS zcw|TgT7y=AmEh?Ht;h35kPrU92CacT7fb>jW#5841x$ZR_a5Xuxe|sM!a;BZRD)X3 zk#SNU`|GxZ@&apa#)M~*0tke-{>e+F^c>r$xIs&EOJ0$3^TuY3iW|IGk|$*3kBUoK zDivfFXmJ4iN~=JwGd(%m*Pm46g%C10kQCnR-cdT4J&A}(zroxFkAPt^`ad0g&&OK_ z>%qo+4AK?|Z-e<0&=DkvL3{-a1{q){SOc~IJJMYMjbJBu4=jb=8jUwtfKi9M0;~k< zr=r4%NCP<->_^&zpc(0oAg%_r`C+($hERmoRnI^?1%*#%XWpJsJ`p#C+YaJ_7`zAe zf`i~=Pzz3hbKrB((d&hC&?&HRHtGjQKsB&~T4N&^x{FzHh++tR}!^qCVBEA9rB*+U92g5!cauJvf2BW+q(AR@4kaNL7$fd9i zLp>`XUqCz`&np*Wal8%1PVgSs1NH-f>N;K@nT-#rh0D+Ya0GN1;vnAuw@^Vm;y9$U zBEATkp}Pl`L$~x*G#qSs1(m|q0zLc{3p_^)C2a0H?;F3|gmD8c;2LNK-+@oT5zxAc zCG0YWxR2u74?dmd*HJX%%62wm*SL5{Ht?sBCEjvP{q*l%?Ef^lo?YA(B?e*5t-~Kj F{|$go87}|; delta 8472 zcmaKx30xFMzQ?P22596E1Qi&No&j_KH3N9yJsh^cAc$AgL0F^PHWGjX5nnqA#oxN4B7Nt0-%=j`|E0rK3>%X{J!n_Xh^IZX|qB4dHJ?e(}z( zA6D%;9~e9&iSXA8QQ17gudJwFLCyZXPRhKr8EEaSv6nb~G1N9WEI)km$xO{Zr^{O1 z=1KN;BCS(G@0>PrIKK=OW*F z+6iG_1q|cr-KVNRd(Wm2zZuGH`n6`aWvy)Jy`Y`Iavc zC<_gmOwY0TL6M0*vOzTI`5dm+dZ*m%Cj~T;7B!Fh z_}_fnZU3ZO9{P=ZTVPuYo(7MDELId6qxF+26}BogiLR}GJG7EgD~kzB3p?a#%)EWY z{78G=Vhh8D(@OScSPETS|5?}{HA3cx=?#|2S~0owjb)tlu3eRy(!Hf`Od`g$SCUKF zQxSusF38tRM4GQ?Dyrqv(B>#aI_f?wUnT5x|LJrj^NWa^7Jp@PX3%v(RBUUMtFnpX z^wvA+W?$)U6N#&qohb+F9<{bLU6seCUX`y2d!sHUZ*18u*O`ic&d^jtzTKW~MTyYc z?1PBOe)UdYQ6o)d0g>Zq8q1BGA6Dd0pK{NmhIY3au47G+7Fy2yqGAKz@^E_fNxo93 zlch!(IJb_?jCzV5XH8Ktz7*5K!$ijw>1|#vv4^TQT<mOH z``J8U0e5vj`&c+fhp-L$u!K0-N7Rc%I^?)rTqS5E9pcn5+@jj{IulAx!WPQT>h+Yc zFZF5BoOE09^_J*VOrp+`D4ONcF-C*0G*V$nhA1s3T~b(%!C=IGrTB=Wk(Ww?9SI9% zt8kF;`!`V{L)L;)u=Qa5Yla9)x%v}vdP-Na%kg&Zfty`S5V)Ie_I<*O+|>hY$-tbU zS6noQw}zwp5#VbR;Wq~pp?ne{j}p?L6*;Ne&DsXWbFKr-chDKy#+nALH8i`DL@ElR zI`I_So|bfLPr0r#mt1VRaXYus#cmmmT=qd0oM@s4*@VPD&`Yd6@x*|w&POY#?5?1z z@IEK2O)BOZ9IQ|B>s*=R(D7tSOWBFRCk7-r9xaykZ^cGC*mFawBd^;`p^JMKV!EayN#Lt znd#5mKquRsQmmzttS~u6;I1BIQK>_XK2k(Wc|(xcXUNf(J_%ZJfEX%ha^Z*S=M0PB!hUcE)JoPcjBrq=pJt~?u}xw%BZiEt zazEO`1CR8OO53pnL#*j2(-%u|7h=4HU=O~LVwTNg6r0R`9MPA{Kg4`TCKdU4f^%KA zS)Ro&2IXqS|M>a5ugmtMLeBc<7fMPRHgM*)^_I_{COgx_5WP{<9#?Fu-GQ~`R??D| zcF0UfWftEMBA3K+DgSxeWBaUr!^lud&5WhT(e>=g!jluC&% z?cIr5di3ySDot<+qNdhlCGn1SzQJSDqr}TDD)87oMmFq}r%%y}N9suDFk0X(8{lH= zMn`iIF80P~A)?xAyW=#83rJ#&kMyNOt>5KjS4Q`X_@{ff{G`c8{H%@)qQ|bvX9)d0 zl@_^~XLLm6IhSha#8GqI^Hqn*McHRX=8j}dQP`1o|S zY!dV_T{boR(Ro;v>GqH~E&ymCI@8b)&tB2XZ2?AH@U^x>8gl=_erCPVa zLiEw40=oPpTRS1{@iw=)E==DqM=Q0v^9|v-o3S02$}}!)vZ?gMvzk&JPCl*lrdw^@ zPYU``s$2TTq^7dKyfeaEC0c2!(^RS{eFC}E95)nOdp1vm^{jJ37`G2o+&rw(E1gPKuLBg1U6*pqo!!=ombsdXU^$9*&XRA^VDj4>pug zsn&i$K2sc55TBch&9Pc;oT8PQ71m~s3Z|0HqZ*@gwK&`upOHR35JPVaAj05YIMAS1 zVJnV@{v4$GrqnIDWHRA%&~Fp${1L*>MZC%%>BrcM8BeCXj}yj(rS-l{MWg7nJy0mZ zpK(?czgnep*$#T`F57$dLg~dKr|lLC$ZXJM)W~Z};(O#@QD>>qUXKQdNIb+&E57JIe4 zqMXFh@fYOXCT-_E?WI;4@6*{xG}1~p`)OhUoy{`DmH~awHg^iW@3wv{hgY??_o~z2 zXm4Ap?c}LOx-Hiam=sO9-7*`SQyOgkC7L6hhpFZ$k$!Zs(>YVPf4P`8cO4(!hr(9l zVw-XYB+YhGxIXWiYUDPPYTn}V?j1yK*d?#PQV$f9>n<*Z+oiC-}h!&CdSPo^YfU62Q-Qt4^g zdPmA2LO@<1=6ragI(4eybg6q3;c-b&6s{R@9+w58_3_;g-4}Ls+9$A-qPe|iTGhj7 zE8^-!VLf}h=;o9oc2B!T>eZ}~27*l6&1x|Kmt`NNELqd8|zIu=SYwkIY{DB>uH>>h|9kz1w-qMIk>XG_;yT-D&OcymPb5@48CAhuG zI30rET$DI`#omx}9aXwQiD*jh`q9D%>sC2zQ{6{1S9TKi{QL<%%*jdfnRR{)tzciy zFG<+_2g36WS-%Ub)8r_&Z|pQK>shttldyBr_9hO1SPfN6LnQ(<~qf8($3Bo52Qb__Toj{y+5#mlIZYDauVjSwzCR5$^}_H(E^XM z7fS{tyKKAR%7j>HT43@It)m58f?JA@5?*Sksr3`NQ@k)htbwP`Nq=E~E*UcDy30p2 zo)yrXk6lL52j?;soGA0~nXUtplg_vmn}_KZ#NzroW1wH=Mbd+sW>GiX+bbx4Pt<%@yTpH)-m$uvH`t?Mu&M!8k7y)3ifg-Ke)R*r3M zVSMFX9JL`Bs4BEs^~0%!3X$dIlXhb659#=MQnWbC5-jQ}_WH$ru~Xhx)!0dT4`?-6 zVoLAe68SfIzv+!h)VM(mZ@s^)P?}PdTslx9=D*2@%N@4cj*a51x?XXLfnfKI6EDl! zDx=sqNxg_mFPp^JTt}ttZ;mQ4y*pEGSEl{<+D&3)uEX}JW5cg$oW84qZ;%#-v=1IB zr|C-Dc?D@fNIT}}6=$rw+(FV6LmaN^7uunlRNY5^4uh?BPF-N6ZRW;o()JoSoK5z> zpRL{{)KO4*ROh6Pm_q(_oU{%rSe@{^7>swg(CH&isS^Z@v{Nfa)TN4va2Y;#vug{Z zjsA+)X>7S;Q(`%buJqb6k8LG}T77Z|E`*%qYT~58X0~;83-1K(9L!6hgs6Fa)jFZkg zNW8BYo@+#%`A*ihcmO@al*RffL`rgg+s>(ph!o&36pN##nAeug|L6BPWR;a#FT~l|LO`tX}V7aD0dFJ>?C3ahC}c6YJ8> z{!|x&1`F9KS-)0aM^ue1JT6X+WDcv)=fV!Y6h2C6@QTCt@oTFf&Zd)5*6!cts4 z^UimuA9+HofyW77j-AP4t8%lWWd>X&KP$@(zN!4u-m^KzA7_RoBl94KC6$iK zIbxDH3WFEpWGj|L`4UueuQ%JiWO~3YxJf>p;e<+M?EaEi?#LPzymUObay2Vhx@y$q zHR{u%2hs#feyaDZ<)m+&xfWI5AGxr2Q8_35!zp^>+Rk(mw;*BdqP3iKnc0`dV7utQ zESA2{Mk0KNJ-$p&x3T9Du4j7@E@f7PbJ(}bA~72FWy$Q!)NFVk;ZEk7ny&TOmbqB^ zv~(7~yxR9QXYViH*rSeEF}_l@@(teoRk7aT&A1%26r*chwyWK?b1mh++Aek5 z&a_nhYCGR;JKnPMSKFVuZTnmH|7ttgZF{|?w#(KV8~ed-hN>nK!kNj46;w@XX4Ypd-i)f%YpL)1TnX(zrCI%c)79tl?&gTi6>Qt;C~)#*|Bb5msXz+x`hsSKl-4i3btj(@3U0-v8laI4aYv-xryS184L!Fso zTWMz`Zo$ZtmHmCYnj@}$_4>~^PJ4o7R{qoXNjyJyU%jDY93K2W7DVKi5?rw^s z#jN+seG^Wi&|rK|R=2c#NWNj!pEQtoS$MsuHZ_!)2^OP#ZT z7~Wcd0|B+CyfPB`%3e{swe1z}K)h1I{ng2ay=orOV#nu+Y)$Q4S?k1?mwh;nhJ}f} z@FfegPHMDswINmxZWxEtHvr?)7)FGrPO#&z7W7h|dJWOGFve|7eKb0Ls~FtiDYrh7 z<*j$Zm}l$EMV(02SacP-y1ZYG8#qf89;e!f ziI_E(Czk9bnJrYc!wFEQzJ#ZKt68dWJJb+K*{dL0r5YTkO-~T`IY(QEKaF2pl zF#5OanGAaoxIC8dodD0O{9g|X9fyCWz(?Q)_^k%rhrJVIm;eB* zqlahp$CuPB{Ou(YJ{PDJB_f`}R=z%_pbVDfpl=T0CxhR5%nP}Mp8@>ykQdSlCPJSL zSpv#{9h}N#j@JiN9?2*CxRNf8K|clR98Mi4LZPm(DO>vd_B4qUsyf` zKENLYfu6Kb=zT#Xh?$Jx?n$@|!)>58NDr9+9&OMN#8bdSr2l^nN<^Mc72!?|a#LK{H)ebU1BtRR$ zL+0RtkQtx>8FRs8ux&cp4><$O26KVWEX=jJkh9sUH|JIUj89H&>U|t^fFHp_-~{wN z!ux?>5C(esTelEv1Jo_RA5q{w=mc)C4R$+ZPu{X6=m)3)%eyIo>`t#FTM*d;wt*R7 zHkb=~Drkj%9azDqpaa|oJ?TD6F$>pWHbMFWJJQ^MLhs4*YlC5H00+P^@a9tdS**7B z2qN1+0@wxibn_;Cb)@*Z^w47ErkQ zIpG;LdGE7RQAu>%AL?#!8Ua5Hgfwz@4Eu;2Kol*osf2LA3TpTZ^HIA z*n|8JSFk?&4E=_bW6A(^EtwNjdrZ#{Yumx-br;sfMX+ywp zunV?PknWYJ9eSNwC3qY90dNG|cos7O>0f|xsN0VCr_hff?KaAug1!>AQ;65C#PV){ z;ViI%j{rwCJui!du0aD<j#fZh_vlRH^K$%v+;3I1u+gja|;Ry{qGEvAT@lzX2~$7dZd` diff --git a/Tools/IO_Firmware/iofirmware_lowpolh.bin b/Tools/IO_Firmware/iofirmware_lowpolh.bin index a014de80d17d2ac2237bc41be730edb8472c79e9..e377063cf2df2cac0d5693495a6533569297d393 100755 GIT binary patch delta 9612 zcmaKx30M?Yw#VT*WiYG15(g5h$v>%r~zC88VzoVCSyRwsEHc& z8b@Q2(NPnZXh6h(+G31OmU+%dmKmLBM#mV$sP7V&s$S@R{{qSLeKYU%_x~?QzDcEl2xwDP2Rv=Cwpzfq3bjE%z5~ zIUCqzO#%@Ik0)YqArUXUTC;#!`~%z$kzWP{PF5En7o31-c82DJ<$jc=`+I?^k6M@M zXd?UrZ%|I~|I&&4{d%pxUNK($&bI-O8((# zA|&sQML5>t$?N(}9)Dj~^K(dX=PP-4NNhSM=|P<2#X1EZ-e5^9%gp11lx2pr6m_MI zX@XB9tQN81MT8wTwz<@0DcAD^*`(-b0dal70CC2P+IL2(Am! zT1qV>kXnB9B~mWC5SZKVyyJM9PJYlBkP{iWPhBQ;vUbYar@mzNx{EeFQu0v_CA3vp%SV`6! zEUI}7;XkoAgJR0p81Y8HVlV~Rz@MalCGm;A>y!Au4F8(uJLoCQM5m634bos=F}?w$)kl2TD5Qqn;BuFbJ@ocy#lv+1cO$& zFHdr_zegAaYc%sj%rPEz`q+H3!wY?^zFGF$q6KTyev;X&aqu z(wvuce7#dDOtE7X)QYzCRZ+{*>**oN-*5HFiZJR*=)q?(Uj9|nRlSQe+(sG4gNYPW z8=Mnt4axH5U#OBCZ1v^mRlyi!m13nBUmopbWih6nT0-|;B8F)h_7U+eGWT!xX8NI< zqnzwS%p&0%2TSN_4)^7g)c9pl+0nM}ED|5s5G+Sv_K#B8qMq52T6W?`q>j3^VzlDi zd)Z%m4v6;QVJgn4PXxE!ov7gc;A+H z3E|_seTw&?jBP682BS&GSE7R9ZOs_qjGL+2EA|s{_v@5Q{(y)<;0rw8-Cr}@7*6Se znu)OnO0Th{y&XcKhn2-iLY{|hjeAWPw4a6a$qIVdO0z_J7zQjH3{GNm`$P-lJZxQ` z-omu~>}a3k^q*{I-8%Y+` zGQz7oc59nwrMhl>rMli+scto6X*GQG0XsA#VepmKE>^da;F){NBKhZOIKyKlitoIvDTyXLXI_z4(}$8Z%aF> zmGvAREo8T{k;A3(*V}ZFt!BBH#K-vX>29sO0VqSL!hT4U+RbW#Suc}B`I1Mu+`3Jj zX7-W)R89KQgWsyh3H=@Azw=PJqME+%H=y&C$KoPllDDa+&0CLol+$i4O$$z_PM)8d zOtMGW(;8G$W7M|{qFOya`Y)R@?E zkH=CSYUrM&=lh=5+k4YAIZwES75;bo}=Bd)a z?BmiqMS5Q5Hkas1v;#p)5$aHy#)?GJ+!G68t;3c6L^20p-$$>gExZIPr$Ms?)%xVb zGW!@q`2g>bEXotxLX&qe{>MBtMgBu-mgwC~MX=bQUYs$|nG zep$b9_+n#TSw#6io#)l#g|Xv$Ta&TkE7iL3KKuvOY&6&mIgtwu*{jt{Ws!w>55HI> z$)Bm~B!S1PTFPjv5C2MKA6ZgHzkrux>O`b|TDKurVB75tc?n|e<*w^OBzrCqvoH+W z(9|6uVm+QKFjT#dFk|Ypqzaq>W-QVQmC7dBrTo!L5nk~6$c1lKM7fnEZF;w|%#p`m z%XcX!*vF}-gz;|nLYi6c!^hUK-D%0fYYz5pS^}O8DihKt2%{YAne;*Q2li%qV)z%{ z4NvA|3hLj1_U0g-e1QEqeOoV&!!P@0$uXCr94g(bep}{aqf%ti_sF0ntYa(3D#C|O z_DV*i@Ro;d&lpE**{zI*o&!&wZjrkF+;~n6t7vKpsMhgF=k&RKI#j;6vf z2fLnG(k1VqP)u6(QQZN;4IcJP)_5Vz!wzJA0le)7Igu3jX*iE`iprwZijDREyikGrn1Xc2?CB&w>1I6LagBI8U8Q3i7EXO z;eTOUvPj5y7@+pY`Sy-c+Y=<}NQN3p3%pK$B)p_1G4j=7w_6sHS(vG-cj z?iPQ6Cj4-P@N+JflXHi@!iMI?mHVWeJ?3M2r$MkyR3$sqMzRihtVe*V^U`7cd{PhK;*FOX8m+BU?d<<2oobz>_w&APN1!>r?8f08(( zQF}9%$A1z0Fyu~lGk0)Eyfa(Xzq?I^$MUlUUP~`0OBh!ooN=;kHt>Ef9c-q_fMLu@qe=1d`W>|<$8rD`!_!RGCqrEl3s631$bPdgYwU-;rWzLYb!+m=x&*w|JD(M%)OBLa*`i1UrmZc=4HqnmK?!$=EYen)$pz7 z)qcs8n;rJsYC4euvjeSqezC57rk@Rht0_{8Hde&z6;wQn_~;^fAK$BxePPp<^GZg? zPw(303{JO(4S3dezB)F4{LMjAoSr5)@uzkC?_k^Mvf4n|7k-HZe?iaRRRwuXngMT2 z`-)iCK$G>nq>f!G9jn*#2`USpo1(|Nbg+WCDTAnCU0Od2zC{kO=v2Kc1tOdA@Aq)ly1>P%m`v)n^ISM5}?y*Qy{ry^r#lCU?8Q{${o zkn$9l*1JmGriD6h`VAXn%}&2QpstIAz1521c=IE{Zz``w2@!Qt&x`gr~1EW}3d zFog3xxfv2CSzW%?x6ptSE+=;?-P$aLo>UnYi%ZctHrMB7d~;kBwVt4~NCMNloj(9pfBVoTyNw%W|tRT7@5`CYSBoo!cXROIBe$*EdPw3Aka zAJ7@w$WEMLT@kVISe^zIIKOpDY{^rJnK{eBlZFB=dW&f8PskAK6q7 z50W2+ujB(ajY=-$sa@3W2?>06t5+Gqo_($tZD6lHmq>%yhtEyVww=Z(!&?_?^=5&; zp;Ae-YtpNA?H;^W$;Jn3kiCDiny$@mL%n9rR%!3v*V{cCVp1DDN`se;D@@RPlmlM& z+rr+1{szA*2;(#eZ6;V96enavSy|?0tbVONoh_>5GAtz?5?oc^onJ5P)9B2qM z>a3Av0{@%^7l)P~Io)1(|Kbn(n$knqk-PW^&w>5vac}FHSt(3zWAHSgG z9$3;F`M**3l{poIV^z6aQ4I8C(0L9eT7-B%hWEK_}a1(hZ5;EU|Gki0w3yi zDoGBC#ZLJomu3sZ!gVM`Z7D*KqK`L^|2E&DOh(FRN76Duu9`==&Gm7UBum&`?x+=V zN_Bf}*7n+VmwygpB#c+pmA@JVLwi%|VGD!p&rfC4wc9oyhOG;1XVrjMQ#H246t6K6 zw_Lw(f^61gAH&Y3ZFrs9E7e{fxxQ*FxxN+b8%uT{9UFv83BixQe+qN*6iX?71|MQ? z6%QRNVv<#=eG8-7j$Wd8d*xt~OaKC}I;HsrQ8{a?BrH~e&viv)>*SEA6gk{Vwi@A> z|HebP;$Z`IZz+a6^oCIsa_dGzwlruLJaG~lA9?YeRuD(JYz zJOvpQrLL>$Eb9;W929u5ll1n*U}zsa@Uu3|>`9+thi4kfJ<2bw|7a4mwywO6Y(E20 z4yYHn{v=JHjcqaJr4P0>?eOX*C)8`30(B0+u~lZrDAa3Pl)!Hz&rjHdi0PM0xNq%e zkPoGq8WND!t9|7bGunI+erXc& z$IHi%KN&eMw`!k9AG&{R3X}iYr;!cpA@-K zNq|fZmJ&Enx}6q@RZ?(vrBsX9XtP-^X;N2Fh3*!c{C2;X+o5 zu#ml38i7&XUz*4^%&qjj-4%C8M_KoIt2)H@=Z!Acv|_Dxdi>zkt`{l)&xY`$+Nq-H zzI~!Q*I@cp_tg_!T|>&Ry0cGoM;o$!)tz{v+t)DpSKUWXbni5jwClQ}l0Q6AZf;n1 zl+AtN6#>Kl(+hKjbw}BR`DePB;Hd@G>T)E!qZYSdyIB}yKfvM^G~mAD)`Cy*@ql}e z7e{o~ygkjUBYI*{Yg%56rNM0uV31%TLl#iYq_Vh$wzh->+BQwiiiJhAN5N_BTC%;* zWlx%uefl_iv?wt|vt(3ab*50uame`SOkRAf^SkIk+g&C~I?h@aXZruoQ|gk#Le)`r zf5{9j8n<-2f8$9>R&n4JuzO2;;WE1OOaBNc!12F+EfF);vd1sIPM5G1FDqK#Ut0E8 z`WKeFJR<6x>*J{5@{JPnSjtbkZkCJ~`*Tw`rYq&OY~}L4bPqeYJf_E1SDJjiq$Ps# zjYyL|_D++QVr|W}<$n@{(?{8%-~Qcq2kL5mzvhh<0;N0H#@GIaaCUiLTE{k)e~xfc z#qSY5sz^sTY~{-ct5^0y*tqgP5Yknl2)nI{ML2BL^9c8>8m3cE*0F1=hWcu5%y-*Z z?+-?UCyU%d*XrCu@$R_>E1x5`qgtAP(bJuE}Td_6@;m)-~5jL*X zy7*{q+c2yv5{9_hyX!3Ylj7RCnDEmMp(@BOz-<)ZE_8zPsD=_L`ePQdeq4aohVU>V zNnvd6`sAk{KUgn!DQzvYKb69{eu7{MW8>cFt~VhMD|;i1RlK1GBj*o-0`AAMsOs#; zhm3+P67^_0VkQ=CSQF}nmWnjtkv@6CT+L>NEss|$qPD5Unmen_Y_Zm>Gj6?LX)HWss?uaFYP;B#3tc7gNaRkpKNH$+8^c1vwJ3rBpSZk=*W*?E+t!>{xyG(-$iOvm?8djXm2h%nLfdB09))abdm7yy z&D}Idvwgj3QMtB5_!>kwV;-f4+t=WSvmq3*c1CnX?2ot;;sC^chCV% zwldm#>s4m4n{OG)i{Pc2YtKITeA-7Xc46M?G$;NPO7%Sn2E)?Oty=^V@vEx0{)W5$2k&Hz&~Y?}iZZ z3h+rGVjTDq@EG`yRVH5zzpOV4n?2pH2sPAy^8Q=VG`!GQ}f9 zD$p9V0;~j2H)uVcH-dcd|21e0?73hP=qURZ@j z=3|hyKzJL>pMZ`aK@8$6U@*u4JHZ;T1=x}90%!y~!FymS^wwy+!2*mr|BjPi-NDcp7t7sTK_uooNz zAA?$O3Y-I%G+B! z8xOhQ7Bb}{p7ew%h^GTBz6VuI-2YOX$DDb1mdl#MOxF!4WVFwkgm} z2mO)0b|IUw!zkTDF;{>#au*SQ32uPW7w6AhO5zr?>K#UQZpYE`8!w}kFQJ8aZ>b;y zSV1nBzZgATg1NR7?;7Mow-NCT=qEv5h&UMb>5z-SY%mz*9f7_cY=N8$4ni)4Z5ZlV z0r>*r`FLKr7>na=D0YJPz#gz42vpbc`p9g2NG)8327n`=!w?7g2DpU^;t|IoofYv# z&IQnk~ Cl^ARQ delta 8472 zcmaKx30xFMzQ?P22596E1Qi&Lo*B>q)C}N(_i)$(gCJf}6BTqd;1x7ciE9EHFq(L# zG@C?|cxFAbAhN;8h)K*_*Tj9YYj$;W;i^HRCQYK5X0Cp}9w5*CyzHCL=Ue}O{p;we z>N@__Q`^i*?{nh_M}+lJgwH4;{HkKYyMZdtfEQtZl!QJ19>4$2&T&ygcD_Y;8)yf8 z-$qT~#%~G#5csbnd<;kd6Tqx>JID3wvkW=Zdf%w*k|!&7zBeGaWdq@hY6yQD^7FTM ze!p_(`M}`8Nrb;%h|1;?enmz7a%%DKbwcK)O+eep8heS;7C~*5!}7x?oygSubDFHx zZJOBGMx?b$NPg(Q*?6g-QSI+!EfGiv;R%66yzw~URokX+Tw!cVfH^qN?P>Fqj4twx zr;QNyWx!Cb-p%?1ZitvFEA}&rAX4cC+1F6`fE1ZTUnxgsCjzI1uIr=LPAw#mT5kCg zfwIt`N%S0>7ZjQ3BO64sp3mWGZFkBoeo{aqX;$;7Pwr|F^|mB}?GIAuO}0Pyi?HWp zVmumMEvJ}siMGQ`s8n6Qz4r(j>MQM5h*1!=dW&oxPo!4%kC2!-BMlgCFc1U*MNjxj z;s5eYxBcU8dGJ^AZH8?PcnUlYvRF}QjMh)8RM^VUB)X>ltVkaDOr&{=rlMLt1#PxMq$BP_@>Rl4^`AyZFuw@n)c7l#GJ~!QqGDgIT$Rln zr?=fnxA;nT&k|#`>`XaO_o%h?>{WS8>Q(uguqWzb@`mPJa-F&OrwmOsrm@_}d0|Bk^(pr}YS`&k!?o;eq?ML4zo@vtH$9wQeUh&f z>SU==2F|TxGolvJqs$hi^QD-+e~zyII%*pir+3t3I<9D>7>7`tZo}Mb;H{f$bro4> z=rJnoWv|3U##e+AA*3NRKQy;bj*oN%Q!F&sM>?c%hLBvrD8%?mE}317F~zDG_wFP7 zwGblILf*q98r-f-^h4uYI$3b+O0HgJ8)D57KGHBHu|k&@ZH>qwiGj_bVx;IJ#VG7j zY+jUFocIyp4vMQ)s@15~?cK+Q#0}AEr3W5X92XAdj)zsmZQ=HNn6F^qKHJBVgj8G zg+48slWr@%-Vz;(S=3n*MT=ZI+GOyRMkp-F5T)g$OA5;|7);o&6d!RE@=|G#BVmDT z6Alo5-$qJg$XZYewj8K`)eu1`SAX27r*s9o9N)=3aI=dE0(aBRzDsz4ySkq(9*{HS zii_s(ws3Sm0(@mA{H9HDA}3Y5S?hp!&b6QU4m?d;+1Y_>3{9>ik&1$- zPCUiFyE)y~Q?9GbB^R4!+Qw~gv0Ek+mwkW*Cz|O2Ha_w9^b%`JJRZBn`Dg`|-4%2d z-s@zwNyS`)gY`*%jVp5;JeEvpDLX#scx;m6(PC-;R&11mJv+EM@_OekdAykuHECa* zBFtZpO`^l1AyNYi9Ws{ttdmV1vNT3Fj_@_08Q8(d@z|h&6-=$aJ|uwBPucCqZeyl@ zYW@>9z{z%{6lHtbjY)_hv3qM#tduRj~_PslxRhTuP*T#cfit(gw|xE-*^wrO=uM*bm||b$4y-M=k><3t zgBC(6v-pM(xg?HD`SU4{{nPsOBSI;)FqUql>)8A0My}P#zD!RY65&n}UG@Zpm@HT5 zo(SjxZkK(aQgzB@@8wM^a%I3e6V_;jjTw2sXRgO%|BT%nsi#Ys_#7izJ@Yp^^H0+S4P1cHs>qy5?THq}k;9_e> zMRO4@_WCFxqS|Y_<1~x&Nn(tT^o2vM-{oXiM)iyMr+b(Dq}fOOw2lmgxhuH-zVI!ggFLQ)Ad9bLsJCG^IM6d|K%Zx7xZN z74)N2x8(JSXUhWfP780AXr(DmbE&5E3FK09+)(Uo**p=}v5xU!++Iv^%g{=fdw32y z;thXx4}<=bHoytJIC;xubo-q8H0Vk%{M)1O5; zCrxw;Vv051K;T?;;NSYJY= z+Rh8|>0)C+d~PZ>$7;E8vQ}zRSgR!}m`Zk!YK+d+;&5YpM*6rw4819U2!ndzK!aX| ztvDX~vykeWQn%!iNrcZqzs<1ohY3Fi@hX3$A7w9OJel$yP8c(m)_XD)O`_BOK%oeK z!dX%LN|nxKKj5{y?C*9KN-q>S?YCG!=2h$HV4k zP2mD$b}DNq?Zw)%k|Q2?wzPG5YS+-l*U{d4kn$mxl)Z~YNkyg0bj}r>$ES4cQ$M!wb)FJe~+wzedUe(stt4@QX zy>*GUgQptlwp<@OF`9C_WHu(JG}!WUG)FoPQOyw|{orD!awc>Caxrc0T0XuHg{{WL zHs;1A&2mz>KJS=o76B~oQ@yoXK~VW&sS~xI{Z1B^!0TjeeYmL^1q`Y zj7>^l6$Ojv2ke`INWV`xKQ!3Hl!9S>KJ3hswR?BTdkMLV4On`=lZl0;+`Ud#TR6W` z$|#iX@x$^nMWUW+(q6gnXZp;7D#b_>vVRs98CN`JH5cAoV76*I4v~YuNzPU$Dz|!J z$!*NdKgepieGyuzN;X-w4=loD^fAcquPkjJDXXJ3#l zDHr63A}Z}v%f{=q5AT(0r4l(B#p6)?T8bZvf0+`A;uR9nQ zJ5mM_0`dYe=ffM-sZ$N7OWmUgk4u80a7~EwxGWH@kMBO{zObv)K7pka&FMYErXEIH z5LYh>>)2aGHzyzN^t5TDUQHTl0BAj1-ViAI;@lN*CTgT=g%fKs^;i??C1X=FP1Q(~ zo2hQd{)9gg=kv zrat!~WDD>~*O<06<5B}Zi9MLs%IOvM_vur($?vkH8LR0swtq$(Evf%-MiIP~`mmV~ zDg9l&|LkNlgs0ZxxCC`9A?ey@TlL0ZyQgonqoLn&3oTv zYv-hM1x|K)ju5>2144RIzI>PcYtA{2{GJ_{JG1gLCsr7t@clz9ob<_AxNZ$u&%>P% zt!J>8F2%lC_ea5FAKGd8k%k@IU@T4aI&9^ZJ*5#7)g$$Hof_+&GF{Zn%$XV9mf-d# z<8%mub5Y{(6?;R@aa8FFC88<0>qiS8tX=7_PjMf~T+u<;bMwagFefL?W43uQw1Ryx zuOwmD?+MR0Wc@a*PLre9zwV^QtY_4kPr%Me+s^WECwH@wVx9kGmwK>avt>2K3H?0w zryE0TAtBO|75Eo#cgBu?C(#C;f%}sbuiL>n>j3@ABXGf==sU?8hdK~oV^hnB*44{*ugs7bIyFdpWkb z1@V=4any!jpsLVj)eomuDnyo-PuzjIKe+v;iP7RvYp|%R*yCsXe22WZsx?V=bK(PB7#mlm` z$|N>UR4?MvOJ*@H*HLNzo1;oh@6MFlm1*C-HnSL+>#%>~SpQ2Jr|+uZYovuC?fpl} zX}Z#OTtQk8(vCWM8BKMU+ex}&Fyg9yp$)oO)qV8mFxYD6)CERbXKctOt*?SZ*<{~) z+3HD_(A=zCL@>MY5YnPs6#OQnteOEeaoWRejstA7%WG5*{^YY^n_swMTr8a?*JR ziT4%5b4{o--^p4R#nRJES)`v#q$KAzZM@pJ^@C7s1mMM9XEh&H5$G7<2Q{K=QcbPyju`cb* zk98qvu;3nw&7<7L8zLcNyv3<>xQR*!UGO$d(iQlzgA&D4nWpAc)bm%bTj%4x)fO)P ztIcCiZ)Ufi8bfz6ed$B`8tY#+o~~w(m&K(n!V3*)p!)Nt6nna&#XRG&XB~DgD8F?cafwtR7vFF_^udb4edrv==Co8;3GPN-DI?k|qx4zFgxOU7|4R7Vsx#`cD37fuDSdd+of*X z>E^0mZ0Eae$C`KiV*6vaZC~@gUu-A3ZLc-gcG-GkV?WSs+|qph7~`H^%VAhwczQ8+ z2rPWp@kt0ntTCc-{In{H;Y4TOPQ^_8JE51SA3|shginS(Lt&+pdR`}ODs+G zn^ziXsA?i1oP`WuPSvC)W_!l!&8S+vmioTWmeBr_o7C?&U41U^ZjRVg!M3hS?xUuh zT#u!dPA%$}9ou?#Y1OHqTj+rIqYrATV4GJ@^#9-3_0`8Y`54={X09r}UGtj%uO|J+XkR8>8KUvMTJ-3mjv%YKXYTtu@`}bi>oCnL2vk#UWEH#Nw z=nfXkw!rHVVk#!$T<0Hjqr@Af=4>j>blxl-oqe}00#lPpMJ#gNKsuIXtc&SC#F;6! zmUcwq7K}Vu+26IPIgItI)_uxx+T$#<@}Is>;`zCI>kU;LrBAYi_5VQVUOy01D`CT* z5Z>SL4#KT3W+7~OaSg)U>NtdD)sOY{ewsCCtRJ_Nw2#}hX>Y>WSjSFQt9SX9>ZcHn ztQo1vZd%7y)r{~}bB}$MovIOiXcK%C=C0|FEPXeQ)cpPII##r?5V8FmbLp4t?nWam zX1!nPn{Wb!2H|_Mx~1hq@(nBhsIgv~Po&`vBHeM|Q!;AGed(z%wfX=!ZBbD#!Z9NT zBE8H$ekqqOWI>zME}Yyn8e#jUG=wpm)v#zYLwI}hV=-!3AFHM#n)|rj&wvYA>g@T% z@aB9R2&g^fZ=Gii1VF!{mR$YazF7M}K2F@CV$Eh}A zCKiqLiN$+JW;0doa01k+FCnc^s#-~{v%Ni4ZPXf3q4ru$yL5_*R8Z_kn+fUB53>)q zzDRGd3EL*7Bz}O;3p!kAb@$pNyc-wF^q?N4=yqOIu_kXe+VMbb2eWM($UO*S-)=L6 zsRpT`zxYxuZwMA+;1<5gblY{hOqXDM@u$#DrLbhXsFCQaWW8C7*f3VKT^D-Rsn|7& zVh{2zP{n?OJ->Yd&e)H)?^c)1>>Wv}*w`gbb&0RLM6aET)U=g5S5~UqiLai=S?{sy zT)uig91#5>)q^4o(jT%nWG~17$o`Oia4&g_uigY56PT!P;<(2@$8BGV???k&9=jPV z1@jRJ<@7`dnCMq zQNLc#B-o3<>VJ(LimN?*YfPlz<*X?H~3GD z9+uS~UsAL1x0guxT%cBzh$_2@QyVfh63 z0Dlk!deTCn_XUw4W)gGPtybP>T?H~h0!tT~#z^7u6rjYTx9)<)k zcs3pgnE@J*F&9h%Tc@G@kki2|FbDX|#9W&LIg71)V{YY7_~g{8-p4^Z_yIfwPC(x! zydMY#VW6kKbqlaIK;3-&5e4pp4&Vk`Vef?O$y>G<{Qxy!SvMt+-RYHNGa|deRxlmR z0&_r51uf980~`1Rw1fMgC*5ZWX5m`QCP;tKi8ME$(0lUy(qPycz51vDrH(+}U z>_+~F%UPejhJJ&~F=c={azT*(Kwr-C_WrYS>$BLjR-#RKQ|mzu*bKIUlgJi>w83B) z*a_Q6NcRfV4!usT61)X{KR66-JcF5l^v}Uq)ZK~rC(w@~?KaAuguVi{lZe-?!18W@ z;S8{W4*^FtJui!du0{h^;o)Ew=t*cn#aj{I3Hdo_f&LKkImq3RZ-4}p)mNagYtRr7 ziL|$1$2n|)`(&Zs%GiLn-!!Cb!DGNIFaZ>Su^ Date: Sat, 29 Jan 2022 20:48:27 +1100 Subject: [PATCH 0370/3101] AP_NavEKF2: avoid direct use of Location alt field --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index aedf27f06db..71ed9127b32 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -285,9 +285,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const if(getPosD(posD) && getOriginLLH(origin)) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid - loc.alt = origin.alt - posD*100; - loc.relative_alt = 0; - loc.terrain_alt = 0; + loc.set_alt_cm(origin.alt - posD*100, Location::AltFrame::ABSOLUTE); // there are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no aiding) if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { @@ -322,10 +320,7 @@ bool NavEKF2_core::getLLH(struct Location &loc) const // If no origin has been defined for the EKF, then we cannot use its position states so return a raw // GPS reading if available and return false if ((gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D)) { - const struct Location &gpsloc = gps.location(); - loc = gpsloc; - loc.relative_alt = 0; - loc.terrain_alt = 0; + loc = gps.location(); } return false; } From df4911cbcbbdace01c9c7ac21638a33fa291ad91 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 29 Jan 2022 20:48:28 +1100 Subject: [PATCH 0371/3101] AP_NavEKF3: avoid direct use of Location alt field --- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index a7ef0ff101b..1b3bec1f01f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -278,9 +278,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const float posD; if(getPosD(posD) && PV_AidingMode != AID_NONE) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid - loc.alt = origin.alt - posD*100; - loc.relative_alt = 0; - loc.terrain_alt = 0; + loc.set_alt_cm(origin.alt - posD*100, Location::AltFrame::ABSOLUTE); if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) { // The EKF is able to provide a position estimate loc.lat = EKF_origin.lat; From 3dae386ae3356d685cd36ad8e510d8f3d9e37db4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:02:30 +1100 Subject: [PATCH 0372/3101] AP_InertialSensor: correct compilation with HAL_INS_ACCELCAL_ENABLED false --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 12 +++++++++++- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 ++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 40fdb8878e4..6de297ffd99 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1960,7 +1960,15 @@ bool AP_InertialSensor::is_still() // return true if we are in a calibration bool AP_InertialSensor::calibrating() const { - return _calibrating_accel || _calibrating_gyro || (_acal && _acal->running()); + if (_calibrating_accel || _calibrating_gyro) { + return true; + } +#if HAL_INS_ACCELCAL_ENABLED + if (_acal && _acal->running()) { + return true; + } +#endif + return false; } /// calibrating - returns true if a temperature calibration is running @@ -1976,6 +1984,7 @@ bool AP_InertialSensor::temperature_cal_running() const return false; } +#if HAL_INS_ACCELCAL_ENABLED // initialise and register accel calibrator // called during the startup of accel cal void AP_InertialSensor::acal_init() @@ -2003,6 +2012,7 @@ void AP_InertialSensor::acal_update() _acal->cancel(); } } +#endif // Update the harmonic notch frequency void AP_InertialSensor::update_harmonic_notch_freq_hz(float scaled_freq) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index b851e76d27a..6f46f473361 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -326,12 +326,14 @@ class AP_InertialSensor : AP_AccelCal_Client // Returns newly calculated trim values if calculated bool get_new_trim(Vector3f &trim_rad); +#if HAL_INS_ACCELCAL_ENABLED // initialise and register accel calibrator // called during the startup of accel cal void acal_init(); // update accel calibrator void acal_update(); +#endif // simple accel calibration #if HAL_GCS_ENABLED From 38a1c4412a074e7b35603ba9bdab7db44589ba6c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:02:30 +1100 Subject: [PATCH 0373/3101] AP_Mount: correct compilation with HAL_INS_ACCELCAL_ENABLED false --- libraries/AP_Mount/SoloGimbal.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Mount/SoloGimbal.h b/libraries/AP_Mount/SoloGimbal.h index 9041f793c76..399735a793d 100644 --- a/libraries/AP_Mount/SoloGimbal.h +++ b/libraries/AP_Mount/SoloGimbal.h @@ -51,7 +51,9 @@ class SoloGimbal : AP_AccelCal_Client _log_del_ang(), _log_del_vel() { +#if HAL_INS_ACCELCAL_ENABLED AP_AccelCal::register_client(this); +#endif } void update_target(const Vector3f &newTarget); From fe863b95675e70cf14285d5d9ed8de01d92be97e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:02:30 +1100 Subject: [PATCH 0374/3101] AP_Vehicle: correct compilation with HAL_INS_ACCELCAL_ENABLED false --- libraries/AP_Vehicle/AP_Vehicle.cpp | 7 +++++-- libraries/AP_Vehicle/AP_Vehicle.h | 2 ++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index d0b37269b9d..b8cc4b7817b 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -302,7 +302,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if OSD_ENABLED SCHED_TASK(publish_osd_info, 1, 10, 240), #endif +#if HAL_INS_ACCELCAL_ENABLED SCHED_TASK(accel_cal_update, 10, 100, 245), +#endif #if HAL_EFI_ENABLED SCHED_TASK_CLASS(AP_EFI, &vehicle.efi, update, 10, 200, 250), #endif @@ -482,6 +484,8 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const #endif +#if HAL_INS_ACCELCAL_ENABLED + #ifndef HAL_CAL_ALWAYS_REBOOT // allow for forced reboot after accelcal #define HAL_CAL_ALWAYS_REBOOT 0 @@ -492,7 +496,6 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const */ void AP_Vehicle::accel_cal_update() { -#if HAL_INS_ENABLED if (hal.util->get_soft_armed()) { return; } @@ -510,8 +513,8 @@ void AP_Vehicle::accel_cal_update() hal.scheduler->reboot(false); } #endif -#endif // HAL_INS_ENABLED } +#endif // HAL_INS_ACCELCAL_ENABLED AP_Vehicle *AP_Vehicle::_singleton = nullptr; diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 58392ff97a8..1fdbad73970 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -386,8 +386,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { void publish_osd_info(); #endif +#if HAL_INS_ACCELCAL_ENABLED // update accel calibration void accel_cal_update(); +#endif ModeReason control_mode_reason = ModeReason::UNKNOWN; From f1ec657c41f861dbf7af48a679c72b1d45dd7ee8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:02:30 +1100 Subject: [PATCH 0375/3101] GCS_MAVLink: correct compilation with HAL_INS_ACCELCAL_ENABLED false --- libraries/GCS_MAVLink/GCS_Common.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 955aac0d20e..9d48bf338a8 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3329,7 +3329,7 @@ void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg) void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg) { -#if HAL_INS_ENABLED +#if HAL_INS_ACCELCAL_ENABLED AP_AccelCal *accelcal = AP::ins().get_acal(); if (accelcal != nullptr) { accelcal->handleMessage(msg); @@ -3959,7 +3959,7 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm rc().calibrating(is_positive(packet.param4)); -#if HAL_INS_ENABLED +#if HAL_INS_ACCELCAL_ENABLED if (is_equal(packet.param5,1.0f)) { // start with gyro calibration if (!calibrate_gyros()) { @@ -3970,7 +3970,9 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm AP::ins().get_acal()->start(this); return MAV_RESULT_ACCEPTED; } +#endif +#if HAL_INS_ENABLED if (is_equal(packet.param5,2.0f)) { if (!calibrate_gyros()) { return MAV_RESULT_FAILED; @@ -4253,18 +4255,16 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_sprayer(const mavlink_command_long_t & } #endif +#if HAL_INS_ACCELCAL_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet) { -#if HAL_INS_ENABLED if (AP::ins().get_acal() == nullptr || !AP::ins().get_acal()->gcs_vehicle_position(packet.param1)) { return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; -#else - return MAV_RESULT_UNSUPPORTED; -#endif } +#endif // HAL_INS_ACCELCAL_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet) { @@ -4339,9 +4339,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t switch (packet.command) { +#if HAL_INS_ACCELCAL_ENABLED case MAV_CMD_ACCELCAL_VEHICLE_POS: result = handle_command_accelcal_vehicle_pos(packet); break; +#endif case MAV_CMD_DO_SET_MODE: result = handle_command_do_set_mode(packet); From 9c067f360f5981a40d8341cfd5a146c3a3276e13 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Mar 2022 18:09:45 +1100 Subject: [PATCH 0376/3101] AP_BattMonitor: added option to send resting voltage to GCS --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 13 +++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 4 ++++ libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_Params.h | 1 + 4 files changed, 19 insertions(+), 1 deletion(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 3ca95b9cb57..8f11bf0e6e3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -438,6 +438,19 @@ float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const } } +/// voltage - returns battery voltage in volts for GCS, may be resting voltage if option enabled +float AP_BattMonitor::gcs_voltage(uint8_t instance) const +{ + if ((_params[instance]._options.get() & uint32_t(AP_BattMonitor_Params::Options::GCS_Resting_Voltage)) != 0) { + return voltage_resting_estimate(instance); + } + if (instance < _num_instances) { + return state[instance].voltage; + } else { + return 0.0f; + } +} + /// current_amps - returns the instantaneous current draw in amperes bool AP_BattMonitor::current_amps(float ¤t, uint8_t instance) const { if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 4df2770d1fc..cd362215f5f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -162,6 +162,10 @@ class AP_BattMonitor float voltage(uint8_t instance) const; float voltage() const { return voltage(AP_BATT_PRIMARY_INSTANCE); } + // voltage for a GCS, may be resistance compensated + float gcs_voltage(uint8_t instance) const; + float gcs_voltage(void) const { return gcs_voltage(AP_BATT_PRIMARY_INSTANCE); } + /// get voltage with sag removed (based on battery current draw and resistance) /// this will always be greater than or equal to the raw voltage float voltage_resting_estimate(uint8_t instance) const; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 2ea402bc68a..818839b43e2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -143,7 +143,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: OPTIONS // @DisplayName: Battery monitor options // @Description: This sets options to change the behaviour of the battery monitor - // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot + // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS // @User: Advanced AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 1bd3c364bfe..32ae4102238 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -24,6 +24,7 @@ class AP_BattMonitor_Params { MPPT_Power_On_At_Arm = (1U<<3), // MPPT Enabled when vehicle is armed, if HW supports it MPPT_Power_Off_At_Boot = (1U<<4), // MPPT Disabled at startup (aka boot), if HW supports it MPPT_Power_On_At_Boot = (1U<<5), // MPPT Enabled at startup (aka boot), if HW supports it. If Power_Off_at_Boot is also set, the behavior is Power_Off_at_Boot + GCS_Resting_Voltage = (1U<<6), // send resistance resting voltage to GCS }; BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); } From 73eabb1fd09d1ef28c6692d6746e4eeec4d027df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Mar 2022 18:11:34 +1100 Subject: [PATCH 0377/3101] GCS_MAVLink: send GCS voltage to GCS may be resting voltage of option enabled --- libraries/GCS_MAVLink/GCS_Common.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9d48bf338a8..2dd7a960109 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -258,7 +258,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const // for battery monitors that cannot provide voltages for individual cells the battery's total voltage is put into the first cell // if the total voltage cannot fit into a single field, the remainder into subsequent fields. // the GCS can then recover the pack voltage by summing all non ignored cell values an we can report a pack up to 655.34 V - float voltage = battery.voltage(instance) * 1e3f; + float voltage = battery.gcs_voltage(instance) * 1e3f; for (uint8_t i = 0; i < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; i++) { if (voltage < 0.001f) { // too small to send to the GCS, set it to the no cell value @@ -4919,7 +4919,7 @@ void GCS_MAVLINK::send_sys_status() control_sensors_health, static_cast(AP::scheduler().load_average() * 1000), #if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) - battery.voltage() * 1000, // mV + battery.gcs_voltage() * 1000, // mV battery_current, // in 10mA units battery_remaining, // in % #else From d82a306cc22a37ad789a634e6ed376727d98c950 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 09:24:25 +1100 Subject: [PATCH 0378/3101] Plane: when shutting down motors force outputs to minimum this fixes issue #20354 where a long spool time can lead to motors running without control for an extended period of time. --- ArduPlane/quadplane.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 078dbaa6938..80b0b58f120 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3853,6 +3853,13 @@ float QuadPlane::get_land_airspeed(void) void QuadPlane::set_desired_spool_state(AP_Motors::DesiredSpoolState state) { if (motors->get_desired_spool_state() != state) { + if (state == AP_Motors::DesiredSpoolState::SHUT_DOWN) { + // also request zero throttle, so we avoid the slow ramp down + motors->set_roll(0); + motors->set_pitch(0); + motors->set_yaw(0); + motors->set_throttle(0); + } motors->set_desired_spool_state(state); } } From 7f5dc33e7e610cd2d73d3c132d5021883c6ee475 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 8 Mar 2022 21:46:48 +0530 Subject: [PATCH 0379/3101] AP_Periph: allow can_printf as periph member method as well --- Tools/AP_Periph/AP_Periph.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d1a30466cbc..50b5e25f7e2 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -53,6 +53,10 @@ void stm32_watchdog_pat(); */ extern const struct app_descriptor app_descriptor; +extern "C" { +void can_printf(const char *fmt, ...) FMT_PRINTF(1,2); +} + class AP_Periph_FW { public: AP_Periph_FW(); @@ -257,6 +261,7 @@ class AP_Periph_FW { void show_stack_free(); static bool no_iface_finished_dna; + static constexpr auto can_printf = ::can_printf; }; namespace AP @@ -266,7 +271,4 @@ namespace AP extern AP_Periph_FW periph; -extern "C" { -void can_printf(const char *fmt, ...) FMT_PRINTF(1,2); -} From 39b771c3a98670f888e954b2847b65c5effe2dde Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 8 Mar 2022 21:47:19 +0530 Subject: [PATCH 0380/3101] AP_Scripting: add support for can_printf in lua --- libraries/AP_Scripting/docs/docs.lua | 3 +++ libraries/AP_Scripting/generator/description/bindings.desc | 1 + 2 files changed, 4 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 6bbe08c9fe2..bf509bcd18a 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -742,6 +742,9 @@ function periph:get_vehicle_state() end ---@return number function periph:get_yaw_earth() end +-- desc +---@param text string +function periph:can_printf(text) end -- desc ---@class ins diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index bf2816c272b..2e5c4fadedf 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -473,6 +473,7 @@ singleton AP_Periph_FW depends defined(HAL_BUILD_AP_PERIPH) singleton AP_Periph_FW alias periph singleton AP_Periph_FW method get_yaw_earth float singleton AP_Periph_FW method get_vehicle_state uint32_t +singleton AP_Periph_FW method can_printf void "%s"'literal string include AP_Motors/AP_Motors_Class.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AP::motors() depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI From 2dceb9a3ed3f9c16d3849e829dc7495f8ddfca09 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 12 Mar 2022 13:08:06 +0530 Subject: [PATCH 0381/3101] AP_HAL_ChibiOS: ensure that common ram is selected for bootloader/app comms in STM32H7 --- .../hwdef/scripts/chibios_hwdef.py | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index fbcd2aaa011..478d4e537aa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -710,9 +710,16 @@ def has_sdcard_spi(): def get_ram_map(): '''get RAM_MAP. May be different for bootloader''' + env_vars['APP_RAM_START'] = None if args.bootloader: ram_map = get_mcu_config('RAM_MAP_BOOTLOADER', False) if ram_map is not None: + app_ram_map = get_mcu_config('RAM_MAP', True) + if app_ram_map[0][0] != ram_map[0][0]: + # we need to find the location of app_ram_map[0] in ram_map + for i in range(len(ram_map)): + if app_ram_map[0][0] == ram_map[i][0]: + env_vars['APP_RAM_START'] = i return ram_map elif env_vars['EXT_FLASH_SIZE_MB']: ram_map = get_mcu_config('RAM_MAP_EXTERNAL_FLASH', False) @@ -980,14 +987,21 @@ def write_mcu_config(f): cc_regions = [] total_memory = 0 for (address, size, flags) in ram_map: - regions.append('{(void*)0x%08x, 0x%08x, 0x%02x }' % (address, size*1024, flags)) cc_regions.append('{0x%08x, 0x%08x, CRASH_CATCHER_BYTE }' % (address, address + size*1024)) + if env_vars['APP_RAM_START'] is not None and address == ram_map[env_vars['APP_RAM_START']][0]: + ram_reserve_start = get_ram_reserve_start() + address += ram_reserve_start + size -= ram_reserve_start + regions.append('{(void*)0x%08x, 0x%08x, 0x%02x }' % (address, size*1024, flags)) total_memory += size f.write('#define HAL_MEMORY_REGIONS %s\n' % ', '.join(regions)) f.write('#define HAL_CC_MEMORY_REGIONS %s\n' % ', '.join(cc_regions)) f.write('#define HAL_MEMORY_TOTAL_KB %u\n' % total_memory) - f.write('#define HAL_RAM0_START 0x%08x\n' % ram_map[0][0]) + if env_vars['APP_RAM_START'] is not None: + f.write('#define HAL_RAM0_START 0x%08x\n' % ram_map[env_vars['APP_RAM_START']][0]) + else: + f.write('#define HAL_RAM0_START 0x%08x\n' % ram_map[0][0]) ram_reserve_start = get_ram_reserve_start() if ram_reserve_start > 0: f.write('#define HAL_RAM_RESERVE_START 0x%08x\n' % ram_reserve_start) @@ -1197,11 +1211,12 @@ def write_ldscript(fname): f = open(fname, 'w') ram0_start = ram_map[0][0] ram0_len = ram_map[0][1] * 1024 - - # possibly reserve some memory for app/bootloader comms - ram_reserve_start = get_ram_reserve_start() - ram0_start += ram_reserve_start - ram0_len -= ram_reserve_start + if env_vars['APP_RAM_START'] is None: + # default to start of ram for shared ram + # possibly reserve some memory for app/bootloader comms + ram_reserve_start = get_ram_reserve_start() + ram0_start += ram_reserve_start + ram0_len -= ram_reserve_start if ext_flash_length == 0 or args.bootloader: env_vars['HAS_EXTERNAL_FLASH_SECTIONS'] = 0 f.write('''/* generated ldscript.ld */ From 79c45049e0069e7f4dd6dd4775333e1ccd29c2eb Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 12 Mar 2022 14:17:28 +0530 Subject: [PATCH 0382/3101] AP_GPS: only use PPS time when there is atleast 2D Fix --- libraries/AP_GPS/GPS_Backend.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index e47ff483f26..6bfb9a1b4b5 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -250,11 +250,12 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) // get the time the packet arrived on the UART uint64_t uart_us; - if (port && _last_pps_time_us == 0) { - uart_us = port->receive_time_constraint_us(msg_length); - } else if (_last_pps_time_us != 0) { + if (_last_pps_time_us != 0 && (state.status >= AP_GPS::GPS_OK_FIX_2D)) { + // pps is only reliable when we have some sort of GPS fix uart_us = _last_pps_time_us; _last_pps_time_us = 0; + } else if (port) { + uart_us = port->receive_time_constraint_us(msg_length); } else { uart_us = AP_HAL::micros64(); } From 3f865ba102c12ec7382649ca953b347719179db4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 06:08:29 +1100 Subject: [PATCH 0383/3101] web: changed from '3DR' to 'SiK' these radios have not been sold by 3DR for a long time --- Tools/autotest/web-firmware/index.html | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/web-firmware/index.html b/Tools/autotest/web-firmware/index.html index 42ccf9fe873..2e15d88e3c0 100644 --- a/Tools/autotest/web-firmware/index.html +++ b/Tools/autotest/web-firmware/index.html @@ -89,7 +89,7 @@

Firmwares

APM Planner 2.0APM Planner 2.0 - APM Planner 2.0 tool

RadioSiK - 3DR Radio Firmware

+ alt="Radio">SiK - SiK Radio Firmware

ToolsTools - Build and development tools

Date: Wed, 23 Mar 2022 11:01:25 +1100 Subject: [PATCH 0384/3101] AP_BattMonitor: fixed battery remaining of sum battery and move to common function for update_consumed() --- libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp | 11 +++-------- libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp | 13 +++++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_Backend.h | 1 + libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp | 10 +++------- libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp | 10 +++------- libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp | 11 ++++++++++- libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp | 8 ++------ 7 files changed, 35 insertions(+), 29 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index a37257587bc..e43e400887e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -82,8 +82,8 @@ AP_BattMonitor_Analog::read() // read current if (has_current()) { // calculate time since last current read - uint32_t tnow = AP_HAL::micros(); - float dt = tnow - _state.last_time_micros; + const uint32_t tnow = AP_HAL::micros(); + const uint32_t dt_us = tnow - _state.last_time_micros; // this copes with changing the pin at runtime _state.healthy &= _curr_pin_analog_source->set_pin(_curr_pin); @@ -91,12 +91,7 @@ AP_BattMonitor_Analog::read() // read current _state.current_amps = (_curr_pin_analog_source->voltage_average() - _curr_amp_offset) * _curr_amp_per_volt; - // update total current drawn since startup - if (_state.last_time_micros != 0 && dt < 2000000.0f) { - float mah = calculate_mah(_state.current_amps, dt); - _state.consumed_mah += mah; - _state.consumed_wh += 0.001f * mah * _state.voltage; - } + update_consumed(_state, dt_us); // record time _state.last_time_micros = tnow; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index c203cc88982..e65f234a70f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -253,3 +253,16 @@ bool AP_BattMonitor_Backend::reset_remaining(float percentage) return true; } + +/* + update consumed mAh and Wh + */ +void AP_BattMonitor_Backend::update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us) +{ + // update total current drawn since startup + if (state.last_time_micros != 0 && dt_us < 2000000) { + const float mah = calculate_mah(state.current_amps, dt_us); + state.consumed_mah += mah; + state.consumed_wh += 0.001 * mah * state.voltage; + } +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 5d8d81f5aa7..d2587131c43 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -90,6 +90,7 @@ class AP_BattMonitor_Backend // checks what failsafes could be triggered void check_failsafe_types(bool &low_voltage, bool &low_capacity, bool &critical_voltage, bool &critical_capacity) const; + void update_consumed(AP_BattMonitor::BattMonitor_State &state, uint32_t dt_us); private: // resistance estimate diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index 84f780ee2a5..6ecc8fde947 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp @@ -109,15 +109,11 @@ void AP_BattMonitor_INA2XX::read(void) accumulate.count = 0; const uint32_t tnow = AP_HAL::micros(); - const float dt = tnow - _state.last_time_micros; + const uint32_t dt_us = tnow - _state.last_time_micros; // update total current drawn since startup - if (_state.last_time_micros != 0 && dt < 2000000.0) { - // .0002778 is 1/3600 (conversion to hours) - const float mah = _state.current_amps * dt * 0.0000002778; - _state.consumed_mah += mah; - _state.consumed_wh += 0.001 * mah * _state.voltage; - } + update_consumed(_state, dt_us); + _state.last_time_micros = tnow; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp index 418698436fe..3274a82da65 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_LTC2946.cpp @@ -67,15 +67,11 @@ void AP_BattMonitor_LTC2946::read(void) accumulate.count = 0; const uint32_t tnow = AP_HAL::micros(); - const float dt = tnow - _state.last_time_micros; + const uint32_t dt_us = tnow - _state.last_time_micros; // update total current drawn since startup - if (_state.last_time_micros != 0 && dt < 2000000.0) { - // .0002778 is 1/3600 (conversion to hours) - const float mah = _state.current_amps * dt * 0.0000002778; - _state.consumed_mah += mah; - _state.consumed_wh += 0.001 * mah * _state.voltage; - } + update_consumed(_state, dt_us); + _state.last_time_micros = tnow; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index cf8d3f973c4..7ce61484f89 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -73,13 +73,22 @@ AP_BattMonitor_Sum::read() current_count++; } } + const uint32_t tnow_us = AP_HAL::micros(); + const uint32_t dt_us = tnow_us - _state.last_time_micros; + if (voltage_count > 0) { _state.voltage = voltage_sum / voltage_count; - _state.last_time_micros = AP_HAL::micros(); } if (current_count > 0) { _state.current_amps = current_sum; } + + update_consumed(_state, dt_us); + _has_current = (current_count > 0); _state.healthy = (voltage_count > 0); + + if (_state.healthy) { + _state.last_time_micros = tnow_us; + } } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 81e41875149..3544c87c3f7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -154,14 +154,10 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa const uint32_t tnow = AP_HAL::micros(); if (!_has_battery_info_aux || _mppt.is_detected) { - uint32_t dt = tnow - _interim_state.last_time_micros; + const uint32_t dt_us = tnow - _interim_state.last_time_micros; // update total current drawn since startup - if (_interim_state.last_time_micros != 0 && dt < 2000000) { - float mah = calculate_mah(_interim_state.current_amps, dt); - _interim_state.consumed_mah += mah; - _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage; - } + update_consumed(_interim_state, dt_us); } // record time From 27cbe5dbbb53133b625c68f7dcc47d10f90c673a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 12:55:09 +1100 Subject: [PATCH 0385/3101] Plane: prevent rapid RTL/AUTO switching on fence breach don't switch back to RTL if already in a DO_LAND_START sequence --- ArduPlane/fence.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index ee829cabb9a..1b8f15a317a 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -65,6 +65,12 @@ void Plane::fence_check() case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS: case AC_FENCE_ACTION_RTL_AND_LAND: if (fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { + if (control_mode == &mode_auto && + mission.get_in_landing_sequence_flag() && + (g.rtl_autoland == 1 || g.rtl_autoland == 2)) { + // already landing + return; + } set_mode(mode_rtl, ModeReason::FENCE_BREACHED); } else { set_mode(mode_guided, ModeReason::FENCE_BREACHED); From 680162cef0fe6c47c78bc2666a3ed749410f6877 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 11:29:29 +1100 Subject: [PATCH 0386/3101] Plane: added a value for RTL_AUTOLAND to disable arming check set RTL_AUTOLAND=3 to get go-around but not RTL DO_LAND_START usage --- ArduPlane/AP_Arming.cpp | 2 +- ArduPlane/Parameters.cpp | 6 +++--- ArduPlane/Parameters.h | 2 +- ArduPlane/defines.h | 9 +++++++++ ArduPlane/events.cpp | 2 +- ArduPlane/mode_rtl.cpp | 4 ++-- 6 files changed, 17 insertions(+), 8 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 60d5f7f3b3f..df97ad5ec37 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -368,7 +368,7 @@ bool AP_Arming_Plane::mission_checks(bool report) { // base checks bool ret = AP_Arming::mission_checks(report); - if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland.get() == 0) { + if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { ret = false; check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index f6563e24a3e..f287fddfd89 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -716,10 +716,10 @@ const AP_Param::Info Plane::var_info[] = { // @Param: RTL_AUTOLAND // @DisplayName: RTL auto land - // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. - // @Values: 0:Disable,1:Enable - go HOME then land,2:Enable - go directly to landing sequence + // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation). + // @Values: 0:Disable,1:Fly HOME then land,2:Go directly to landing sequence, 3:OnlyForGoAround // @User: Standard - GSCALAR(rtl_autoland, "RTL_AUTOLAND", 0), + GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)), // @Param: CRASH_ACC_THRESH // @DisplayName: Crash Deceleration Threshold diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index bae6c1a3deb..9334c87d5c8 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -362,7 +362,7 @@ class Parameters { AP_Int16 sysid_my_gcs; AP_Int8 telem_delay; - AP_Int8 rtl_autoland; + AP_Enum rtl_autoland; AP_Int8 crash_accel_threshold; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 3bcfd2e8c74..8da58b7e69b 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -50,6 +50,15 @@ enum class StickMixing { VTOL_YAW = 3, }; +// values for RTL_AUTOLAND +enum class RtlAutoland { + RTL_DISABLE = 0, + RTL_THEN_DO_LAND_START = 1, + RTL_IMMEDIATE_DO_LAND_START = 2, + NO_RTL_GO_AROUND = 3, +}; + + enum ChannelMixing { MIXING_DISABLED = 0, MIXING_UPUP = 1, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 3b900f4a358..090c8364f1a 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -261,7 +261,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) #endif if (!already_landing) { // never stop a landing if we were already committed - if (g.rtl_autoland == 2 && plane.mission.is_best_land_sequence()) { + if (g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && plane.mission.is_best_land_sequence()) { // continue mission as it will reach a landing in less distance plane.mission.set_in_landing_sequence_flag(true); break; diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 42f38d560ea..7c59ac1f997 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -75,7 +75,7 @@ void ModeRTL::navigate() } #endif - if (plane.g.rtl_autoland == 1 && + if (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START && !plane.auto_state.checked_for_autoland && plane.reached_loiter_target() && labs(plane.altitude_error_cm) < 1000) { @@ -90,7 +90,7 @@ void ModeRTL::navigate() // on every loop plane.auto_state.checked_for_autoland = true; } - else if (plane.g.rtl_autoland == 2 && + else if (plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && !plane.auto_state.checked_for_autoland) { // Go directly to the landing sequence if (plane.mission.jump_to_landing_sequence()) { From 7a62b5429b2f45b5b2894e7a378250bf2b2d2987 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2022 11:20:02 +1100 Subject: [PATCH 0387/3101] autotest: add test for landing speed --- Tools/autotest/arducopter.py | 52 ++++++++++++++++++++++++++ Tools/autotest/common.py | 71 ++++++++++++++++++++++++++++-------- 2 files changed, 107 insertions(+), 16 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d04be75bb95..46439e9ede8 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -3460,6 +3460,54 @@ def fly_precision_landing_drivers(self): if ex is not None: raise ex + def Landing(self): + """Test landing the aircraft.""" + + def check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, land_speed_high_accuracy=0.1): + self.progress("Checking landing speeds (speed_high=%f speed_low=%f alt_low=%f" % + (land_speed_high, land_speed_low, land_alt_low)) + land_high_maintain = 5 + land_low_maintain = land_alt_low / land_speed_low / 2 + + takeoff_alt = (land_high_maintain * land_speed_high + land_alt_low) + 20 + # this is pretty rough, but takes *so much longer* in LOITER + self.takeoff(takeoff_alt, mode='STABILIZE', timeout=200, takeoff_throttle=2000) + # check default landing speeds: + self.change_mode('LAND') + # ensure higher-alt descent rate: + self.wait_descent_rate(land_speed_high, + minimum_duration=land_high_maintain, + accuracy=land_speed_high_accuracy) + self.wait_descent_rate(land_speed_low) + # ensure we transition to low descent rate at correct height: + self.assert_altitude(land_alt_low, relative=True) + # now make sure we maintain that descent rate: + self.wait_descent_rate(land_speed_low, minimum_duration=land_low_maintain) + self.wait_disarmed() + + # test the defaults. By default LAND_SPEED_HIGH is 0 so + # WPNAV_SPEED_DN is used + check_landing_speeds( + self.get_parameter("WPNAV_SPEED_DN") / 100, # cm/s -> m/s + self.get_parameter("LAND_SPEED") / 100, # cm/s -> m/s + self.get_parameter("LAND_ALT_LOW") / 100 # cm -> m + ) + + def test_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs): + self.set_parameters({ + "LAND_SPEED_HIGH": land_speed_high * 100, # m/s -> cm/s + "LAND_SPEED": land_speed_low * 100, # m/s -> cm/s + "LAND_ALT_LOW": land_alt_low * 100, # m -> cm + }) + check_landing_speeds(land_speed_high, land_speed_low, land_alt_low, **kwargs) + + test_landing_speeds( + 5, # descent speed high + 1, # descent speed low + 30, # transition altitude + land_speed_high_accuracy=0.5 + ) + def get_system_clock_utc(self, time_seconds): # this is a copy of ArduPilot's AP_RTC function! # separate time into ms, sec, min, hour and days but all expressed @@ -8323,6 +8371,10 @@ def tests1a(self): "Precision Loiter (Companion)", self.fly_precision_companion), # 29s + ("Landing", + "Test landing", + self.Landing), + ("PrecisionLandingSITL", "Precision Landing drivers (SITL)", self.fly_precision_landing_drivers), # 29s diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 532702dd31a..cb686b13950 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -5433,6 +5433,12 @@ def get_altitude(self, relative=False, timeout=30): return msg.relative_alt / 1000.0 # mm -> m return msg.alt / 1000.0 # mm -> m + def assert_altitude(self, alt, accuracy=1, **kwargs): + got_alt = self.get_altitude(**kwargs) + if abs(alt - got_alt) > accuracy: + raise NotAchievedException("Incorrect alt; want=%f got=%f" % + (alt, got_alt)) + def assert_rangefinder_distance_between(self, dist_min, dist_max): m = self.assert_receive_message('RANGEFINDER') @@ -5646,18 +5652,34 @@ def wait_and_maintain(self, value_name, target, current_value_getter, validator= last_value = current_value_getter() if called_function is not None: called_function(last_value, target) - if self.get_sim_time_cached() - last_print_time > 1: - if type(target) is Vector3: - self.progress("%s=(%s) (want (%s) +- %f)" % - (value_name, str(last_value), str(target), accuracy)) - else: - self.progress("%s=%0.2f (want %f +- %f)" % - (value_name, last_value, target, accuracy)) - last_print_time = self.get_sim_time_cached() if validator is not None: is_value_valid = validator(last_value, target) else: is_value_valid = math.fabs(last_value - target) <= accuracy + if self.get_sim_time_cached() - last_print_time > 1: + if is_value_valid: + want_or_got = "got" + else: + want_or_got = "want" + achieved_duration_bit = "" + if achieving_duration_start is not None: + so_far = self.get_sim_time_cached() - achieving_duration_start + achieved_duration_bit = " (maintain=%.1f/%.1f)" % (so_far, minimum_duration) + + if type(target) is Vector3: + self.progress( + "%s=(%s) (%s (%s) +- %f)%s" % + (value_name, + str(last_value), + want_or_got, + str(target), + accuracy, + achieved_duration_bit) + ) + else: + self.progress("%s=%0.2f (%s %f +- %f)%s" % + (value_name, last_value, want_or_got, target, accuracy, achieved_duration_bit)) + last_print_time = self.get_sim_time_cached() if is_value_valid: sum_of_achieved_values += last_value count_of_achieved_values += 1.0 @@ -5726,14 +5748,13 @@ def validator(value2, target2): **kwargs ) - def wait_speed_vector(self, speed_vector, accuracy=0.2, timeout=30, **kwargs): - """Wait for a given speed vector.""" - def get_speed_vector(timeout2): - msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=timeout2) - if msg: - return Vector3(msg.vx, msg.vy, msg.vz) - raise MsgRcvTimeoutException("Failed to get local speed vector") + def get_speed_vector(self, timeout=1): + '''return speed vector, NED''' + msg = self.assert_receive_message('LOCAL_POSITION_NED', timeout=timeout) + return Vector3(msg.vx, msg.vy, msg.vz) + """Wait for a given speed vector.""" + def wait_speed_vector(self, speed_vector, accuracy=0.2, timeout=30, **kwargs): def validator(value2, target2): return (math.fabs(value2.x - target2.x) <= accuracy and math.fabs(value2.y - target2.y) <= accuracy and @@ -5742,13 +5763,31 @@ def validator(value2, target2): self.wait_and_maintain( value_name="SpeedVector", target=speed_vector, - current_value_getter=lambda: get_speed_vector(timeout), + current_value_getter=lambda: self.get_speed_vector(timeout=timeout), validator=lambda value2, target2: validator(value2, target2), accuracy=accuracy, timeout=timeout, **kwargs ) + def get_descent_rate(self): + '''get descent rate - a positive number if you are going down''' + return abs(self.get_speed_vector().z) + + def wait_descent_rate(self, rate, accuracy=0.1, **kwargs): + '''wait for descent rate rate, a positive number if going down''' + def validator(value, target): + return math.fabs(value - target) <= accuracy + + self.wait_and_maintain( + value_name="DescentRate", + target=rate, + current_value_getter=lambda: self.get_descent_rate(), + validator=lambda value, target: validator(value, target), + accuracy=accuracy, + **kwargs + ) + def get_body_frame_velocity(self): gri = self.mav.recv_match(type='GPS_RAW_INT', blocking=True, timeout=1) if gri is None: From 48881eeb5517d2804f929aa3d56b68f958f73772 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Mar 2022 14:34:22 +1100 Subject: [PATCH 0388/3101] Plane: fixed yaw on the ground when rudder disarming in AUTO when a quadplane touches down in an auto throttle mode the pilot may use rudder to disarm. The check for rudder disarm was only active in modes without auto-throttle. This expands it to all modes if the throttle has hit the lower limit --- ArduPlane/fence.cpp | 3 ++- ArduPlane/quadplane.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 1b8f15a317a..d329719048f 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -67,7 +67,8 @@ void Plane::fence_check() if (fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { if (control_mode == &mode_auto && mission.get_in_landing_sequence_flag() && - (g.rtl_autoland == 1 || g.rtl_autoland == 2)) { + (g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START || + g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START)) { // already landing return; } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 80b0b58f120..0bd05402d8d 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1174,8 +1174,10 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && - !is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && - plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) { + !is_positive(plane.get_throttle_input()) && + (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && + plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && + fabsf(inertial_nav.get_velocity_z_up_cms()) < 0.5 * get_pilot_velocity_z_max_dn()) { // the user may be trying to disarm return 0; } From 549883772c2e6419a7d84af590a3e0bef803c946 Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Tue, 22 Mar 2022 14:08:10 +0530 Subject: [PATCH 0389/3101] hwdef: RM3100 reversal mask --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat | 1 + libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat | 1 + 2 files changed, 2 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat index e44d80a0dfb..dfd4ffa3975 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat @@ -102,6 +102,7 @@ define HAL_PERIPH_ENABLE_MAG SPIDEV rm3100 SPI1 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ COMPASS RM3100 SPI:rm3100 false ROTATION_PITCH_180 +define AP_RM3100_REVERSAL_MASK 7 define HAL_COMPASS_MAX_SENSORS 1 # --------------------- DPS310 --------------------------- diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat index 040ab4caac2..0aa12aaf5d3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat @@ -69,6 +69,7 @@ BARO DPS310 SPI:dps310 # Mag probe SPIDEV rm3100 SPI1 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ COMPASS RM3100 SPI:rm3100 false ROTATION_PITCH_180 +define AP_RM3100_REVERSAL_MASK 7 # PWM, WS2812 LED PB8 TIM4_CH3 TIM4 PWM(1) GPIO(50) From bf0ce199420683d010f90d2f215d4da8f4540df7 Mon Sep 17 00:00:00 2001 From: MallikarjunSE Date: Tue, 22 Mar 2022 17:41:26 +0530 Subject: [PATCH 0390/3101] hwdef: RM3100 no false rotation --- libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat index 6c399c81b66..5f3145976a0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-L431/hwdef.dat @@ -58,7 +58,7 @@ PB12 MAG_CS CS # compass SPIDEV rm3100 SPI1 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ -COMPASS RM3100 SPI:rm3100 false ROTATION_PITCH_180 +COMPASS RM3100 SPI:rm3100 false ROTATION_NONE # MS4525DO Airspeed PB6 I2C1_SCL I2C1 From 5561a5e82e81056e373125e0024cce4911508725 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Mar 2022 20:05:22 +0900 Subject: [PATCH 0391/3101] AP_HAL_ChibiOS: reversible DShot fix Co-authored-by: Andy Piper --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 40168cb385a..f80006dd5cf 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1414,7 +1414,7 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) pwm = constrain_int16(pwm, 1000, 2000); uint16_t value = MIN(2 * (pwm - 1000), 1999); - if (chan_mask & (_reversible_mask>>chan_offset)) { + if ((chan_mask & _reversible_mask) != 0) { // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed if (value < 1000) { value = 1999 - value; From 2d5d74b7a89a25faf4500d99b21c3332da98b129 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 23 Mar 2022 10:01:32 +0900 Subject: [PATCH 0392/3101] AP_BLHeli: add reboot required to some parameters --- libraries/AP_BLHeli/AP_BLHeli.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index dfe2ba9653b..eafb6f7365d 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -60,6 +60,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: Enable of BLHeli pass-thru servo protocol support to specific channels. This mask is in addition to motors enabled using SERVO_BLH_AUTO (if any) // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("MASK", 1, AP_BLHeli, channel_mask, 0), #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) @@ -68,6 +69,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: If set to 1 this auto-enables BLHeli pass-thru support for all multicopter motors // @Values: 0:Disabled,1:Enabled // @User: Standard + // @RebootRequired: True AP_GROUPINFO("AUTO", 2, AP_BLHeli, channel_auto, 0), #endif @@ -106,6 +108,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: When set to a non-zero value this overrides the output type for the output channels given by SERVO_BLH_MASK. This can be used to enable DShot on outputs that are not part of the multicopter motors group. // @Values: 0:None,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("OTYPE", 7, AP_BLHeli, output_type, 0), // @Param: PORT @@ -120,6 +123,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14. // @Range: 1 127 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("POLES", 9, AP_BLHeli, motor_poles, 14), // @Param: 3DMASK @@ -127,6 +131,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: Mask of channels which are dynamically reversible. This is used to configure ESCs in '3D' mode, allowing for the motor to spin in either direction // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("3DMASK", 10, AP_BLHeli, channel_reversible_mask, 0), #ifdef HAL_WITH_BIDIR_DSHOT @@ -135,6 +140,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: Mask of channels which support bi-directional dshot. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch. // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("BDMASK", 11, AP_BLHeli, channel_bidir_dshot_mask, 0), #endif // @Param: RVMASK @@ -142,6 +148,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Description: Mask of channels which are reversed. This is used to configure ESCs in reversed mode // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16 // @User: Advanced + // @RebootRequired: True AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0), AP_GROUPEND From 458ff45fe68a21ae6849adc99a57f95f1170422a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 22 Mar 2022 14:54:47 +0000 Subject: [PATCH 0393/3101] AP_HAL_ChibiOS: correct channel offsets for dshot commands Co-authored-by: Randy Mackay --- libraries/AP_HAL_ChibiOS/RCOutput.h | 2 +- libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 39708a11c05..a899b35bca3 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -200,7 +200,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput * The mask uses servo channel numbering */ void set_reversed_mask(uint16_t chanmask) override; - uint16_t get_reversed_mask() override { return _reversed_mask; } + uint16_t get_reversed_mask() override { return _reversed_mask << chan_offset; } /* mark escs as active for the purpose of sending dshot commands diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index 13a5578c02e..3fddedc1a4d 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp @@ -98,7 +98,7 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman DshotCommandPacket pkt; pkt.command = command; - pkt.chan = chan + chan_offset; + pkt.chan = chan - chan_offset; if (command_timeout_ms == 0) { pkt.cycle = MAX(10, repeat_count); } else { @@ -138,10 +138,10 @@ void RCOutput::update_channel_masks() { switch (_dshot_esc_type) { case DSHOT_ESC_BLHELI: if (_reversible_mask & (1U< Date: Thu, 24 Mar 2022 08:25:13 +1030 Subject: [PATCH 0394/3101] Copter: Clarify calculations in get_pilot_desired_lean_angles --- ArduCopter/mode.cpp | 43 +++++++++++++++++++++++-------------------- ArduCopter/mode.h | 2 +- 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 1b31f7b54cc..23849ed3139 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -397,38 +397,41 @@ void Copter::notify_flight_mode() { // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // returns desired angle in centi-degrees -void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const +void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const { // throttle failsafe check if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { - roll_out = 0; - pitch_out = 0; + roll_out_cd = 0.0; + pitch_out_cd = 0.0; return; } - // fetch roll and pitch inputs - roll_out = channel_roll->get_control_in(); - pitch_out = channel_pitch->get_control_in(); + // fetch roll and pitch stick positions + float thrust_angle_x_cd = - channel_pitch->get_control_in(); + float thrust_angle_y_cd = channel_roll->get_control_in(); // limit max lean angle - angle_limit = constrain_float(angle_limit, 1000.0f, angle_max); + angle_limit_cd = constrain_float(angle_limit_cd, 1000.0f, angle_max_cd); - // scale roll and pitch inputs to ANGLE_MAX parameter range - float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX; - roll_out *= scaler; - pitch_out *= scaler; + // scale roll and pitch inputs to +- angle_max + float scaler = angle_max_cd/(float)ROLL_PITCH_YAW_INPUT_MAX; + thrust_angle_x_cd *= scaler; + thrust_angle_y_cd *= scaler; - // do circular limit - float total_in = norm(pitch_out, roll_out); - if (total_in > angle_limit) { - float ratio = angle_limit / total_in; - roll_out *= ratio; - pitch_out *= ratio; + // convert square mapping to circular mapping with maximum magnitude of angle_limit + float total_in = norm(thrust_angle_x_cd, thrust_angle_y_cd); + if (total_in > angle_limit_cd) { + float ratio = angle_limit_cd / total_in; + thrust_angle_x_cd *= ratio; + thrust_angle_y_cd *= ratio; } - // do lateral tilt to euler roll conversion - roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000))); + // thrust_angle_x and thrust_angle_y represents a level body frame thrust vector in the + // direction of [thrust_angle_x, thrust_angle_y] and a magnitude + // tan(mag([thrust_angle_x, thrust_angle_y])) * 9.81 * aircraft mass. - // roll_out and pitch_out are returned + // Conversion from angular thrust vector to euler angles. + roll_out_cd = (18000/M_PI) * atanf(cosf(thrust_angle_x_cd*(M_PI/18000))*tanf(thrust_angle_y_cd*(M_PI/18000))); + pitch_out_cd = - thrust_angle_x_cd; } // transform pilot's roll or pitch input into a desired velocity diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 721db3a025a..75f23c8d0a4 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -89,7 +89,7 @@ class Mode { int32_t get_alt_above_ground_cm(void); // pilot input processing - void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const; + void get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const; Vector2f get_pilot_desired_velocity(float vel_max) const; float get_pilot_desired_yaw_rate(float yaw_in); float get_pilot_desired_throttle() const; From 7947494cab567e78b32bdcaff2969de66e903a1e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 17 Mar 2022 18:01:03 +1100 Subject: [PATCH 0395/3101] HAL_ChibiOS: prevent conflicting RC input when we have RC from both IOMCU and from rcprotocol (eg. from SERIALn_PROTOCOL=23) we need to only process one of them. This prioritises IOMCU input --- libraries/AP_HAL_ChibiOS/RCInput.cpp | 9 ++++++++- libraries/AP_HAL_ChibiOS/RCInput.h | 1 + 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index cbaaf03c7bc..a82b3fdb1b2 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -175,7 +175,13 @@ void RCInput::_timer_tick(void) } #endif - if (rcprot.new_input()) { + uint32_t now = AP_HAL::millis(); + const bool have_iocmu_rc = (_rcin_last_iomcu_ms != 0 && now - _rcin_last_iomcu_ms < 400); + if (!have_iocmu_rc) { + _rcin_last_iomcu_ms = 0; + } + + if (!have_iocmu_rc && rcprot.new_input()) { WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = AP_HAL::micros(); _num_channels = rcprot.num_channels(); @@ -208,6 +214,7 @@ void RCInput::_timer_tick(void) if (AP_BoardConfig::io_enabled() && iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { _rcin_timestamp_last_signal = last_iomcu_us; + _rcin_last_iomcu_ms = now; #ifndef HAL_NO_UARTDRIVER rc_protocol = iomcu.get_rc_protocol(); _rssi = iomcu.get_RSSI(); diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index 6a35ab05986..48eff5767bc 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -69,6 +69,7 @@ class ChibiOS::RCInput : public AP_HAL::RCInput { int16_t _rssi = -1; int16_t _rx_link_quality = -1; uint32_t _rcin_timestamp_last_signal; + uint32_t _rcin_last_iomcu_ms; bool _init; const char *last_protocol; bool pulse_input_enabled; From 53c1c235b890bba4b8eded0a0144164c70d17be8 Mon Sep 17 00:00:00 2001 From: HefnySco Date: Mon, 17 Jan 2022 16:17:03 +0200 Subject: [PATCH 0396/3101] AP_HAL:RPI read cpu by revision --- libraries/AP_HAL_Linux/Util_RPI.cpp | 80 ++++++++++------------------- libraries/AP_HAL_Linux/Util_RPI.h | 2 +- 2 files changed, 27 insertions(+), 55 deletions(-) diff --git a/libraries/AP_HAL_Linux/Util_RPI.cpp b/libraries/AP_HAL_Linux/Util_RPI.cpp index de0b3ecab54..9250f728ffe 100644 --- a/libraries/AP_HAL_Linux/Util_RPI.cpp +++ b/libraries/AP_HAL_Linux/Util_RPI.cpp @@ -27,72 +27,44 @@ using namespace Linux; UtilRPI::UtilRPI() { - _check_rpi_version(); + _check_rpi_version_by_rev(); } -int UtilRPI::_check_rpi_version() +int UtilRPI::_check_rpi_version_by_rev() { - const unsigned int MAX_SIZE_LINE = 50; - char buffer[MAX_SIZE_LINE]; - int hw; + // assume 2 if unknown + _rpi_version = 2; + const char *SOC[]= {"Broadcom BCM2835","Broadcom BCM2836","Broadcom BCM2837","Broadcom BCM2711"}; + const char *revision_file_ = "/proc/device-tree/system/linux,revision"; + uint8_t revision[4] = { 0 }; + uint32_t cpu = 0; + FILE *fd; - memset(buffer, 0, MAX_SIZE_LINE); - FILE *f = fopen("/sys/firmware/devicetree/base/model", "r"); - if (f != nullptr && fgets(buffer, MAX_SIZE_LINE, f) != nullptr) { - fclose(f); - - int ret = strncmp(buffer, "Raspberry Pi Compute Module 4", 28); - if (ret == 0) { - _rpi_version = 4; // compute module 4 e.g. Raspberry Pi Compute Module 4 Rev 1.0. - printf("%s. (intern: %d)\n", buffer, _rpi_version); - return _rpi_version; - } - - ret = strncmp(buffer, "Raspberry Pi Zero 2", 19); - if (ret == 0) { - _rpi_version = 2; // Raspberry PI Zero 2 W e.g. Raspberry Pi Zero 2 Rev 1.0. - printf("%s. (intern: %d)\n", buffer, _rpi_version); - return _rpi_version; - } - - ret = sscanf(buffer + 12, "%d", &_rpi_version); - if (ret != EOF) { - if (_rpi_version > 3) { - _rpi_version = 4; - } else if (_rpi_version > 2) { - // Preserving old behavior. - _rpi_version = 2; - } else if (_rpi_version == 0) { - // RPi 1 doesn't have a number there, so sscanf() won't have read anything. - _rpi_version = 1; + if ((fd = fopen(revision_file_, "rb")) == NULL) { + printf("Can't open '%s'\n", revision_file_); + } + else { + if (fread(revision, 1, sizeof(revision), fd) == 4) { + cpu = (revision[2] >> 4) & 0xf; + + _rpi_version = cpu; + + if (_rpi_version==0) { + _rpi_version=1; //RPI-Zero } - printf("%s. (intern: %d)\n", buffer, _rpi_version); - - return _rpi_version; + printf("SOC: %s use intern: %d \r\n",SOC[cpu], _rpi_version); } + else { + printf("Revision data too short\n"); + } + fclose(fd); } - // Attempting old method if the version couldn't be read with the new one. - hw = Util::from(hal.util)->get_hw_arm32(); - - if (hw == UTIL_HARDWARE_RPI4) { - printf("Raspberry Pi 4 with BCM2711!\n"); - _rpi_version = 4; - } else if (hw == UTIL_HARDWARE_RPI2) { - printf("Raspberry Pi 2/3 with BCM2709!\n"); - _rpi_version = 2; - } else if (hw == UTIL_HARDWARE_RPI1) { - printf("Raspberry Pi 1 with BCM2708!\n"); - _rpi_version = 1; - } else { - /* defaults to RPi version 2/3 */ - fprintf(stderr, "Could not detect RPi version, defaulting to 2/3\n"); - _rpi_version = 2; - } return _rpi_version; } + int UtilRPI::get_rpi_version() const { return _rpi_version; diff --git a/libraries/AP_HAL_Linux/Util_RPI.h b/libraries/AP_HAL_Linux/Util_RPI.h index fdd0aca365f..9735a63f8f0 100644 --- a/libraries/AP_HAL_Linux/Util_RPI.h +++ b/libraries/AP_HAL_Linux/Util_RPI.h @@ -17,7 +17,7 @@ class UtilRPI : public Util { protected: // Called in the constructor once - int _check_rpi_version(); + int _check_rpi_version_by_rev(); private: int _rpi_version = 0; From fc668b4bcafe717771f7a62e5d37a62031fcffca Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 14 Mar 2022 11:47:37 +0900 Subject: [PATCH 0397/3101] AP_Scripting: copter fast descent gets improved slowdown --- .../examples/copter-fast-descent.lua | 39 ++++++++++--------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Scripting/examples/copter-fast-descent.lua b/libraries/AP_Scripting/examples/copter-fast-descent.lua index 49752b87276..86735fdce84 100644 --- a/libraries/AP_Scripting/examples/copter-fast-descent.lua +++ b/libraries/AP_Scripting/examples/copter-fast-descent.lua @@ -136,25 +136,28 @@ function update() speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed to zero speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero else - -- determine if below slowdown point - local slowdown = false - local stopping_distance_z = 0.5 * speed_z_max:get() * speed_z_max:get() / accel_z - if (rel_pos_home_NED) then - if (-rel_pos_home_NED:z() <= alt_above_home_min:get() + stopping_distance_z) then - slowdown = true - end + -- calculate conversion between alt-above-home and alt-above-ekf-origin + local home_alt_above_origin = 0 + local home = ahrs:get_home() + local ekf_origin = ahrs:get_origin() + if home and ekf_origin then + local dist_NED = home:get_distance_NED(ekf_origin) + home_alt_above_origin = dist_NED:z() end - if (slowdown) then - -- slow down vertical and then horizontal speed - speed_z = math.max(speed_z - (accel_z * dt), math.min(speed_z_slowdown, speed_z_max:get())) -- decelerate to 0.1m/s vertically - if speed_z <= speed_z_slowdown then - speed_xy = math.max(speed_xy - (accel_xy * dt), 0) - end + -- calculate target speeds + local target_dist_to_alt_min = -target_alt_D - home_alt_above_origin - alt_above_home_min:get() -- alt target's distance to alt_min + if (target_dist_to_alt_min > 0) then + local speed_z_limit = speed_z_max:get() + speed_z_limit = math.min(speed_z_limit, math.sqrt(2.0 * target_dist_to_alt_min * accel_z)) -- limit speed so vehicle can stop at ALT_MIN + speed_z_limit = math.max(speed_z_limit, speed_z_slowdown) -- vertical speed should never be less than 0.1 m/s when above ALT_MIN + speed_z = math.min(speed_z + (accel_z * dt), speed_z_limit) + + speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to maximum else - -- normal speed - speed_xy = math.min(speed_xy + (accel_xy * dt), speed_xy_max:get()) -- accelerate horizontal speed to max - speed_z = math.min(speed_z + (accel_z * dt), speed_z_max:get()) -- accelerate to max descent rate + -- below alt min so decelerate target speeds to zero + speed_xy = math.max(speed_xy - (accel_xy * dt), 0) -- decelerate horizontal speed to zero + speed_z = math.max(speed_z - (accel_z * dt), 0) -- decelerate vertical speed to zero end end @@ -205,9 +208,9 @@ function update() -- send targets to vehicle with yaw target vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel, true, target_yaw_deg, false, 0, false) - -- advance to stage 2 when below target altitude + -- advance to stage 2 when below target altitude and target speeds are zero if (rel_pos_home_NED) then - if (-rel_pos_home_NED:z() <= alt_above_home_min:get()) then + if (-rel_pos_home_NED:z() <= alt_above_home_min:get() and (speed_xy==0) and (speed_z==0)) then stage = stage + 1 end if (update_user) then From f6166906e0575c1d28f7adf8b52ff9761253521d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 16 Mar 2022 12:18:37 +0900 Subject: [PATCH 0398/3101] Copter: guided mode takeoff failure leaves submode unchanged --- ArduCopter/mode_guided.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 501a9475d72..0f4de592a58 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -112,8 +112,6 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) { - guided_mode = SubMode::TakeOff; - // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain) int32_t alt_target_cm; bool alt_target_terrain = false; @@ -139,6 +137,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) } } + guided_mode = SubMode::TakeOff; + // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); @@ -166,9 +166,10 @@ void ModeGuided::wp_control_start() // initialise wpnav to stopping point Vector3f stopping_point; wp_nav->get_wp_stopping_point(stopping_point); - - // no need to check return status because terrain data is not used - wp_nav->set_wp_destination(stopping_point, false); + if (!wp_nav->set_wp_destination(stopping_point, false)) { + // this should never happen because terrain data is not used + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } // initialise yaw auto_yaw.set_mode_to_default(false); From f1c072bcd2127e513fbe48ed76a43012be32710b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 16 Mar 2022 12:55:13 +0900 Subject: [PATCH 0399/3101] Copter: auto mode sets submode after all possible failures are passed --- ArduCopter/mode_auto.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 088142da18a..ee381f28a07 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -24,14 +24,14 @@ bool ModeAuto::init(bool ignore_checks) { auto_RTL = false; if (mission.num_commands() > 1 || ignore_checks) { - _mode = SubMode::LOITER; - // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd"); return false; } + _mode = SubMode::LOITER; + // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check if (auto_yaw.mode() == AUTO_YAW_ROI) { @@ -251,23 +251,27 @@ bool ModeAuto::loiter_start() // auto_rtl_start - initialises RTL in AUTO flight mode void ModeAuto::rtl_start() { - _mode = SubMode::RTL; - // call regular rtl flight mode initialisation and ask it to ignore checks - copter.mode_rtl.init(true); + if (copter.mode_rtl.init(true)) { + _mode = SubMode::RTL; + } else { + // this should never happen because RTL never fails init if argument is true + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } } // auto_takeoff_start - initialises waypoint controller to implement take-off void ModeAuto::takeoff_start(const Location& dest_loc) { - _mode = SubMode::TAKEOFF; - if (!copter.current_loc.initialised()) { - // vehicle doesn't know where it is ATM. We should not - // initialise our takeoff destination without knowing this! + // this should never happen because mission commands are not executed until + // the AHRS/EKF origin is set by which time current_loc should also have been set + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return; } + _mode = SubMode::TAKEOFF; + // calculate current and target altitudes // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin int32_t alt_target_cm; @@ -453,10 +457,14 @@ void ModeAuto::circle_start() // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode void ModeAuto::nav_guided_start() { - _mode = SubMode::NAVGUIDED; - // call regular guided flight mode initialisation - copter.mode_guided.init(true); + if (!copter.mode_guided.init(true)) { + // this should never happen because guided mode never fails to init + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + _mode = SubMode::NAVGUIDED; // initialise guided start time and position as reference for limit checking copter.mode_guided.limit_init_time_and_pos(); From 8fa6432e3a49ad3b080ffeca8c6b44257e0ad75f Mon Sep 17 00:00:00 2001 From: BhumilDepani Date: Mon, 21 Mar 2022 17:06:56 +0530 Subject: [PATCH 0400/3101] AP_GPS: added comments in GPS_AUTO_test example sketch Added comments in libraries/AP_GPS/examples/GPS_AUTO_text/GPS_AUTO_test.cpp file AP_GPS: added comments to GPS_AUTO_test example sketch Added comments in libraries/AP_GPS/examples/GPS_AUTO_text/GPS_AUTO_test.cpp file AP_GPS: added comments to GPS_AUTO_test example sketch AP_GPS: add comments to GPS_AUTO_text example sketch AP_GPS: added comments in GPS_AUTO_test example sketch --- .../examples/GPS_AUTO_test/GPS_AUTO_test.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp index 14c32a82dee..06d8e48f8c7 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.cpp @@ -16,7 +16,7 @@ Test for AP_GPS_AUTO */ -#include +#include //This is a common Hardware Abstraction Layer. #include #include #include @@ -24,10 +24,10 @@ #include #include -void setup(); -void loop(); +void setup(); //This function is defined in most of the libraries. This function is called only once at boot up time. This function is called by main() function in HAL. +void loop(); //This function is defined in most of the libraries. This function is called by main function in HAL. The main work of the sketch is typically in this function only. -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); //Declare "hal" reference variable. This variable is pointing to Ap_HAL::HAL class's object. Here, AP_HAL is library and HAL is a class in that library. This reference variable can be used to get access to hardware specific functions. static AP_BoardConfig board_config; @@ -35,7 +35,7 @@ static AP_BoardConfig board_config; AP_BoardLED board_led; // create fake gcs object -GCS_Dummy _gcs; +GCS_Dummy _gcs; //gcs stands for Ground Control Station const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = { AP_GROUPEND @@ -69,7 +69,7 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) { int32_t dec_portion, frac_portion; - int32_t abs_lat_or_lon = labs(lat_or_lon); + int32_t abs_lat_or_lon = labs(lat_or_lon); //The labs() function in C++ returns the absolute value of a long or long int data. // extract decimal portion (special handling of negative numbers to ensure we round towards zero) dec_portion = abs_lat_or_lon / 10000000UL; @@ -92,7 +92,7 @@ void loop() // Update GPS state based on possible bytes received from the module. gps.update(); - // If new GPS data is received, output it's contents to the console + // If new GPS data is received, output its contents to the console // Here we rely on the time of the message in GPS class and the time of last message // saved in static variable last_msg_ms. When new message is received, the time // in GPS class will be updated. From 1521760a1713a4911956d6c6e79e296be751fd4e Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 22 Mar 2022 16:18:42 +0000 Subject: [PATCH 0401/3101] Filters: HarmonicNotch: remove statment on max number of notches harmonics from parm descripstion --- libraries/Filter/HarmonicNotchFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index b2c4e97b4bc..6dc93d5e7d8 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -60,7 +60,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: HMNCS // @DisplayName: Harmonic Notch Filter harmonics - // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A maximum of 3 harmonics can be used at any one time. + // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. // @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic // @User: Advanced // @RebootRequired: True From 79098d1d1068e0411b8f5e99fa4a2b4859c1c909 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 26 Mar 2022 09:15:40 +1100 Subject: [PATCH 0402/3101] autotest: correct diagnostic message symlink->link --- Tools/autotest/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index cb686b13950..935ac79d378 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2564,7 +2564,7 @@ def try_symlink_tlog(self): os.link(self.logfile, self.buildlog) except OSError as error: self.progress("OSError [%d]: %s" % (error.errno, error.strerror)) - self.progress("WARN: Failed to create symlink: %s => %s, " + self.progress("WARN: Failed to create link: %s => %s, " "will copy tlog manually to target location" % (self.logfile, self.buildlog)) self.copy_tlog = True From 05be2ac8c9b92a5af07426915528153b13235c59 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 26 Mar 2022 09:13:34 +1100 Subject: [PATCH 0403/3101] autotest: print mag cal progress messages when testing mag cal --- Tools/autotest/common.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 935ac79d378..c96f616a00c 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -7534,6 +7534,7 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): else: continue if m is not None: + self.progress("Mag CAL progress: %s" % str(m)) cid = m.compass_id new_pct = int(m.completion_pct) if new_pct != reached_pct[cid]: @@ -7599,6 +7600,7 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): self.progress("All Mag report failure") break if m is not None and m.get_type() == "MAG_CAL_PROGRESS": + self.progress("Mag CAL progress: %s" % str(m)) cid = m.compass_id new_pct = int(m.completion_pct) if new_pct != reached_pct[cid]: From 1a2a98892ac8a334dd98375ccd715dc0729b2458 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Mar 2022 01:10:00 +0530 Subject: [PATCH 0404/3101] modules: update pydronecan --- modules/DroneCAN/pydronecan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan index 21eab6f065a..8fb1b18ab32 160000 --- a/modules/DroneCAN/pydronecan +++ b/modules/DroneCAN/pydronecan @@ -1 +1 @@ -Subproject commit 21eab6f065a7fc61672e6324b65a95d6127bb103 +Subproject commit 8fb1b18ab3250695d85fe2943dfc68e8b45024b7 From 7770c960c915ca0630a1773aa0735eb27cd3677b Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Mar 2022 01:10:25 +0530 Subject: [PATCH 0405/3101] waf: add support for CANFD in SITL --- Tools/ardupilotwaf/boards.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 3c5080743e7..d598272322b 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -552,6 +552,7 @@ def configure_env(self, cfg, env): if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) cfg.define('UAVCAN_EXCEPTIONS', 0) + cfg.define('UAVCAN_SUPPORT_CANFD', 1) env.CXXFLAGS += [ '-Werror=float-equal' @@ -679,7 +680,7 @@ def configure_env(self, cfg, env): HAL_MISSION_ENABLED = 0, HAL_RALLY_ENABLED = 0, HAL_SCHEDULER_ENABLED = 0, - CANARD_ENABLE_TAO_OPTION = 1, + CANARD_ENABLE_CANFD = 1, ) # libcanard is written for 32bit platforms env.CXXFLAGS += [ From df95b1ee13db659e9fa5d9fbb2288783297fbe66 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Mar 2022 01:10:49 +0530 Subject: [PATCH 0406/3101] Tools: modify scripts to allow vcan iface to have CANFD supported --- Tools/scripts/CAN/can_sitl.sh | 1 + Tools/scripts/CAN/can_sitl_nodev.sh | 1 + 2 files changed, 2 insertions(+) diff --git a/Tools/scripts/CAN/can_sitl.sh b/Tools/scripts/CAN/can_sitl.sh index 00907e626d2..65aa2339a5f 100755 --- a/Tools/scripts/CAN/can_sitl.sh +++ b/Tools/scripts/CAN/can_sitl.sh @@ -32,6 +32,7 @@ done sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set up vcan0 +sudo ip link set vcan0 mtu 72 sudo modprobe slcan sudo modprobe can-gw sudo slcan_attach -f -s8 -o "$DEVPATH" diff --git a/Tools/scripts/CAN/can_sitl_nodev.sh b/Tools/scripts/CAN/can_sitl_nodev.sh index 36c2145f528..d31514c802f 100755 --- a/Tools/scripts/CAN/can_sitl_nodev.sh +++ b/Tools/scripts/CAN/can_sitl_nodev.sh @@ -19,4 +19,5 @@ done sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set up vcan0 +sudo ip link set vcan0 mtu 72 sudo modprobe can-gw From 7d2ded8de148ece66d0c5eb62efcb97fde4e8e2f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Mar 2022 01:11:17 +0530 Subject: [PATCH 0407/3101] AP_HAL: add support for CANFD in SITL --- libraries/AP_HAL/AP_HAL_Boards.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index d98c36f27d0..46ab08f925f 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -284,7 +284,9 @@ #endif #endif // HAL_HNF_MAX_FILTERS -#ifndef HAL_CANFD_SUPPORTED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL // allow SITL to have all the CANFD options +#define HAL_CANFD_SUPPORTED 8 +#elif !defined(HAL_CANFD_SUPPORTED) #define HAL_CANFD_SUPPORTED 0 #endif From bd2bf7fedcb8ff8890772be66a34794829cef9e6 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Mar 2022 01:11:40 +0530 Subject: [PATCH 0408/3101] AP_HAL_SITL: add support for CANFD in SITL --- libraries/AP_HAL_SITL/CANSocketIface.cpp | 137 ++++++++++++++++++++--- libraries/AP_HAL_SITL/CANSocketIface.h | 4 + 2 files changed, 127 insertions(+), 14 deletions(-) diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index 33e2bd104a3..02501a94863 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -71,19 +71,50 @@ static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) return sockcan_frame; } -static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame) +static canfd_frame makeSocketCanFDFrame(const AP_HAL::CANFrame& uavcan_frame) { - AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); + canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { } }; + std::copy(uavcan_frame.data, uavcan_frame.data + AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), sockcan_frame.data); + if (uavcan_frame.isExtended()) { + sockcan_frame.can_id |= CAN_EFF_FLAG; + } + if (uavcan_frame.isErrorFrame()) { + sockcan_frame.can_id |= CAN_ERR_FLAG; + } + if (uavcan_frame.isRemoteTransmissionRequest()) { + sockcan_frame.can_id |= CAN_RTR_FLAG; + } + return sockcan_frame; +} + +static AP_HAL::CANFrame makeCanFrame(const can_frame& sockcan_frame) +{ + AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); if (sockcan_frame.can_id & CAN_EFF_FLAG) { - uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF; + can_frame.id |= AP_HAL::CANFrame::FlagEFF; } if (sockcan_frame.can_id & CAN_ERR_FLAG) { - uavcan_frame.id |= AP_HAL::CANFrame::FlagERR; + can_frame.id |= AP_HAL::CANFrame::FlagERR; } if (sockcan_frame.can_id & CAN_RTR_FLAG) { - uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR; + can_frame.id |= AP_HAL::CANFrame::FlagRTR; } - return uavcan_frame; + return can_frame; +} + +static AP_HAL::CANFrame makeCanFDFrame(const canfd_frame& sockcan_frame) +{ + AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.len); + if (sockcan_frame.can_id & CAN_EFF_FLAG) { + can_frame.id |= AP_HAL::CANFrame::FlagEFF; + } + if (sockcan_frame.can_id & CAN_ERR_FLAG) { + can_frame.id |= AP_HAL::CANFrame::FlagERR; + } + if (sockcan_frame.can_id & CAN_RTR_FLAG) { + can_frame.id |= AP_HAL::CANFrame::FlagRTR; + } + return can_frame; } bool CANIface::is_initialized() const @@ -135,6 +166,10 @@ int CANIface::_openSocket(const std::string& iface_name) if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { return -1; } + // Allow CANFD + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) { + return -1; + } // Non-blocking if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) { return -1; @@ -308,7 +343,12 @@ bool CANIface::_pollRead() CanRxItem rx; rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC) bool loopback = false; - const int res = _read(rx.frame, rx.timestamp_us, loopback); + int res; + if (iterations_count % 2 == 0) { + res = _read(rx.frame, rx.timestamp_us, loopback); + } else { + res = _readfd(rx.frame, rx.timestamp_us, loopback); + } if (res == 1) { bool accept = true; if (loopback) { // We receive loopback for all CAN frames @@ -339,19 +379,27 @@ int CANIface::_write(const AP_HAL::CANFrame& frame) const return -1; } errno = 0; + int res = 0; - const can_frame sockcan_frame = makeSocketCanFrame(frame); - - const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); + if (frame.isCanFDFrame()) { + const canfd_frame sockcan_frame = makeSocketCanFDFrame(frame); + res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); + if (res > 0 && res != sizeof(sockcan_frame)) { + return -1; + } + } else { + const can_frame sockcan_frame = makeSocketCanFrame(frame); + res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); + if (res > 0 && res != sizeof(sockcan_frame)) { + return -1; + } + } if (res <= 0) { if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error return 0; } return res; } - if (res != sizeof(sockcan_frame)) { - return -1; - } return 1; } @@ -389,7 +437,48 @@ int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopb return 0; } - frame = makeUavcanFrame(sockcan_frame); + frame = makeCanFrame(sockcan_frame); + /* + * Timestamp + */ + timestamp_us = AP_HAL::native_micros64(); + return 1; +} + +int CANIface::_readfd(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const +{ + if (_fd < 0) { + return -1; + } + auto iov = iovec(); + auto sockcan_frame = canfd_frame(); + iov.iov_base = &sockcan_frame; + iov.iov_len = sizeof(sockcan_frame); + union { + uint8_t data[CMSG_SPACE(sizeof(::timeval))]; + struct cmsghdr align; + } control; + + auto msg = msghdr(); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control.data; + msg.msg_controllen = sizeof(control.data); + + const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); + if (res <= 0) { + return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; + } + /* + * Flags + */ + loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; + + if (!loopback && !_checkHWFilters(sockcan_frame)) { + return 0; + } + + frame = makeCanFDFrame(sockcan_frame); /* * Timestamp */ @@ -450,6 +539,20 @@ bool CANIface::_checkHWFilters(const can_frame& frame) const } } +bool CANIface::_checkHWFilters(const canfd_frame& frame) const +{ + if (!_hw_filters_container.empty()) { + for (auto& f : _hw_filters_container) { + if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) { + return true; + } + } + return false; + } else { + return true; + } +} + void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd) { if (!_down && (pfd.revents & POLLERR)) { @@ -463,6 +566,12 @@ void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd) } } +bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) +{ + // we are using vcan, so bitrate is irrelevant + return init(bitrate, mode); +} + bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) { char iface_name[16]; diff --git a/libraries/AP_HAL_SITL/CANSocketIface.h b/libraries/AP_HAL_SITL/CANSocketIface.h index 5a4c4310b17..a9e9ac486c9 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.h +++ b/libraries/AP_HAL_SITL/CANSocketIface.h @@ -67,6 +67,7 @@ class CANIface: public AP_HAL::CANIface { ~CANIface() { } // Initialise CAN Peripheral + bool init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) override; bool init(const uint32_t bitrate, const OperatingMode mode) override; // Put frame into Tx FIFO returns negative on error, 0 on buffer full, @@ -136,6 +137,8 @@ class CANIface: public AP_HAL::CANIface { int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; + int _readfd(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; + void _incrementNumFramesInSocketTxQueue(); void _confirmSentFrame(); @@ -143,6 +146,7 @@ class CANIface: public AP_HAL::CANIface { bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame); bool _checkHWFilters(const can_frame& frame) const; + bool _checkHWFilters(const canfd_frame& frame) const; bool _hasReadyTx(); From 88feb747625357775e2d2033812de78e0c35e135 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2022 16:38:15 +1100 Subject: [PATCH 0409/3101] AP_GPS: improve parameter documentation in UAVCAN GPS --- libraries/AP_GPS/AP_GPS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e3b4e203548..2ccdafe970d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -363,7 +363,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #if HAL_ENABLE_LIBUAVCAN_DRIVERS // @Param: _CAN_NODEID1 // @DisplayName: GPS Node ID 1 - // @Description: GPS Node id for discovered first. + // @Description: GPS Node id for first-discovered GPS. // @ReadOnly: True // @User: Advanced AP_GROUPINFO("_CAN_NODEID1", 28, AP_GPS, _node_id[0], 0), @@ -371,21 +371,21 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { #if GPS_MAX_RECEIVERS > 1 // @Param: _CAN_NODEID2 // @DisplayName: GPS Node ID 2 - // @Description: GPS Node id for discovered second. + // @Description: GPS Node id for second-discovered GPS. // @ReadOnly: True // @User: Advanced AP_GROUPINFO("_CAN_NODEID2", 29, AP_GPS, _node_id[1], 0), #endif // GPS_MAX_RECEIVERS > 1 // @Param: 1_CAN_OVRIDE // @DisplayName: First DroneCAN GPS NODE ID - // @Description: GPS Node id for first GPS. If 0 the gps will be automatically selected on first come basis. + // @Description: GPS Node id for first GPS. If 0 the gps will be automatically selected on a first-come-first-GPS basis. // @User: Advanced AP_GROUPINFO("1_CAN_OVRIDE", 30, AP_GPS, _override_node_id[0], 0), #if GPS_MAX_RECEIVERS > 1 // @Param: 2_CAN_OVRIDE // @DisplayName: Second DroneCAN GPS NODE ID - // @Description: GPS Node id for second GPS. If 0 the gps will be automatically selected on first come basis. + // @Description: GPS Node id for second GPS. If 0 the gps will be automatically selected on a second-come-second-GPS basis. // @User: Advanced AP_GROUPINFO("2_CAN_OVRIDE", 31, AP_GPS, _override_node_id[1], 0), #endif // GPS_MAX_RECEIVERS > 1 From cafad05b387066bba6130ca376fc55315bc5225d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2022 16:38:40 +1100 Subject: [PATCH 0410/3101] AP_GPS: rearrange send-text for consistency --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7d7ccfe4446..3610dfc4764 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1728,7 +1728,7 @@ AP_GPS_UBLOX::_save_cfg() _last_cfg_sent_time = AP_HAL::millis(); _num_cfg_save_tries++; GCS_SEND_TEXT(MAV_SEVERITY_INFO, - "GPS: u-blox %d saving config", + "GPS %d: u-blox saving config", state.instance + 1); } From 5f2c5be84a8e9b69671e8e7163f962bc581c4927 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 08:26:49 +1100 Subject: [PATCH 0411/3101] AP_Terrain: added terrain reference adjustment this restores the terrain adjustment functionality removed in #19946, but without the problematic approach of always using home (which can be moved in flight) and with a TERR_OFS_MAX parameter to limit the amount of adjustment --- libraries/AP_Terrain/AP_Terrain.cpp | 89 ++++++++++++++++++++++++++++- libraries/AP_Terrain/AP_Terrain.h | 23 +++++++- libraries/AP_Terrain/TerrainIO.cpp | 2 + 3 files changed, 112 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index a6146937aeb..4cc81ed3ec1 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -67,6 +67,14 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = { // @Range: 0.05 50000 // @User: Advanced AP_GROUPINFO("MARGIN", 3, AP_Terrain, margin, 0.05), + + // @Param: OFS_MAX + // @DisplayName: Maximum reference offset + // @Description: The maximum adjustment of terrain altitude based on the assumption that the vehicle is on the ground when it is armed. When the vehicle is armed the location of the vehicle is recorded, and when terrain data is available for that location a height adjustment for terrain data is calculated that aligns the terrain height at that location with the altitude recorded at arming. This height adjustment is applied to all terrain data. This parameter clamps the amount of adjustment. A value of zero disables the use of terrain height adjustment. + // @Units: m + // @Range: 0 50 + // @User: Advanced + AP_GROUPINFO("OFS_MAX", 4, AP_Terrain, offset_max, 15), AP_GROUPEND }; @@ -95,7 +103,7 @@ AP_Terrain::AP_Terrain() : This function costs about 20 microseconds on Pixhawk */ -bool AP_Terrain::height_amsl(const Location &loc, float &height) +bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) { if (!allocate()) { return false; @@ -107,6 +115,9 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height) if (loc.lat == home_loc.lat && loc.lng == home_loc.lng) { height = home_height; + if (corrected && have_reference_offset) { + height += reference_offset; + } return true; } @@ -157,6 +168,10 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height) home_loc = loc; } + if (corrected && have_reference_offset) { + height += reference_offset; + } + return true; } @@ -404,6 +419,78 @@ bool AP_Terrain::allocate(void) return true; } +/* + setup a reference location for terrain adjustment. This should + be called when the vehicle is definately on the ground +*/ +void AP_Terrain::set_reference_location(void) +{ + const auto &ahrs = AP::ahrs(); + + // check we have absolute position + nav_filter_status status; + if (!ahrs.get_filter_status(status) || + !status.flags.vert_pos || + !status.flags.horiz_pos_abs || + !status.flags.attitude) { + return; + } + + // check we have a small 3D velocity + Vector3f vel; + if (!ahrs.get_velocity_NED(vel) || + vel.length() > 3) { + return; + } + + have_reference_offset = false; + have_reference_loc = ahrs.get_location(reference_loc); + + update_reference_offset(); +} + +/* + get the offset between terrain height and reference alt at the + reference location + */ +void AP_Terrain::update_reference_offset(void) +{ + // TERR_OFS_MAX of zero means no adjustment + if (!is_positive(offset_max)) { + have_reference_offset = false; + return; + } + + // allow for change to TERRAIN_OFS_MAX while flying + if (have_reference_offset) { + reference_offset = constrain_float(reference_offset, -offset_max, offset_max); + return; + } + + if (!have_reference_loc) { + // no reference available yet + return; + } + + // calculate adjustment + float height; + if (!height_amsl(reference_loc, height)) { + return; + } + int32_t alt_cm; + if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { + return; + } + float adjustment = alt_cm*0.01 - height; + reference_offset = constrain_float(adjustment, -offset_max, offset_max); + if (fabsf(adjustment) > offset_max.get()+0.5) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f", + adjustment, reference_offset); + } + have_reference_offset = true; +} + + namespace AP { AP_Terrain *terrain() diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index ae47f726641..6444ce35c5d 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -122,7 +122,7 @@ class AP_Terrain { return false if not available */ - bool height_amsl(const Location &loc, float &height); + bool height_amsl(const Location &loc, float &height, bool corrected = true); /* find difference between home terrain height and the terrain @@ -189,6 +189,12 @@ class AP_Terrain { */ bool init_failed() const { return memory_alloc_failed; } + /* + setup a reference location for terrain adjustment. This should + be called when the vehicle is definately on the ground + */ + void set_reference_location(void); + private: // allocate the terrain subsystem data bool allocate(void); @@ -344,12 +350,18 @@ class AP_Terrain { */ void update_rally_data(void); + /* + calculate reference offset if needed + */ + void update_reference_offset(void); + // parameters AP_Int8 enable; AP_Float margin; AP_Int16 grid_spacing; // meters between grid points AP_Int16 options; // option bits + AP_Float offset_max; enum class Options { DisableDownload = (1U<<0), @@ -396,6 +408,15 @@ class AP_Terrain { float home_height; Location home_loc; + // reference position for terrain adjustment, set at arming + bool have_reference_loc; + Location reference_loc; + + // calculated reference offset + bool have_reference_offset; + float reference_offset; + + // cache the last terrain height (AMSL) of the AHRS current // location. This is used for extrapolation when terrain data is // temporarily unavailable diff --git a/libraries/AP_Terrain/TerrainIO.cpp b/libraries/AP_Terrain/TerrainIO.cpp index 65098d81d8a..950230b235d 100644 --- a/libraries/AP_Terrain/TerrainIO.cpp +++ b/libraries/AP_Terrain/TerrainIO.cpp @@ -351,6 +351,8 @@ void AP_Terrain::io_timer(void) return; } + update_reference_offset(); + switch (disk_io_state) { case DiskIoIdle: case DiskIoDoneRead: From 536b59ed42f9ecab8082671e131a619b0c1497db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 08:27:03 +1100 Subject: [PATCH 0412/3101] SITL: don't use adjusted terrain in SITL --- libraries/SITL/SIM_Aircraft.cpp | 4 ++-- libraries/SITL/SIM_Ship.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 5c5f9d5f2b4..62d23d659ef 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -111,8 +111,8 @@ float Aircraft::ground_height_difference() const if (sitl && terrain != nullptr && sitl->terrain_enable && - terrain->height_amsl(home, h1) && - terrain->height_amsl(location, h2)) { + terrain->height_amsl(home, h1, false) && + terrain->height_amsl(location, h2, false)) { h2 += local_ground_level; return h2 - h1; } diff --git a/libraries/SITL/SIM_Ship.cpp b/libraries/SITL/SIM_Ship.cpp index c5807a31df5..7baaff3ec62 100644 --- a/libraries/SITL/SIM_Ship.cpp +++ b/libraries/SITL/SIM_Ship.cpp @@ -204,7 +204,7 @@ void ShipSim::send_report(void) #if AP_TERRAIN_AVAILABLE auto terrain = AP::terrain(); float height; - if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height)) { + if (terrain != nullptr && terrain->enabled() && terrain->height_amsl(loc, height, false)) { alt_mm = height * 1000; } #endif From ed8f028359401aa4de4d8914d66cc3896a2d0549 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 08:27:19 +1100 Subject: [PATCH 0413/3101] AP_Arming: setup for terrain adjustment on arming --- libraries/AP_Arming/AP_Arming.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index dd28febee75..217fcbfa0c7 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1369,6 +1369,17 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) gcs().send_text(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled"); } +#if AP_TERRAIN_AVAILABLE + if (armed) { + // tell terrain we have just armed, so it can setup + // a reference location for terrain adjustment + auto *terrain = AP::terrain(); + if (terrain != nullptr) { + terrain->set_reference_location(); + } + } +#endif + return armed; } From f8aa4623ef3d024f7afb51b0028da43d94b74c58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 08:27:33 +1100 Subject: [PATCH 0414/3101] HAL_SITL: don't use terrain adjustment --- libraries/AP_HAL_SITL/SITL_State.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index be35e182c31..491b330a237 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -906,7 +906,7 @@ void SITL_State::set_height_agl(void) AP_Terrain *_terrain = AP_Terrain::get_singleton(); if (_terrain != nullptr && - _terrain->height_amsl(location, terrain_height_amsl)) { + _terrain->height_amsl(location, terrain_height_amsl, false)) { _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; return; } From 54f6dbf476e0c50ff32ab208e25b57c0ed71ad83 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 08:29:05 +1100 Subject: [PATCH 0415/3101] AP_Scripting: restored corrected boolean in height_amsl binding --- libraries/AP_Scripting/docs/docs.lua | 3 ++- libraries/AP_Scripting/generator/description/bindings.desc | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index bf509bcd18a..8dff6ef2f86 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1507,8 +1507,9 @@ function terrain:height_terrain_difference_home(extrapolate) end -- desc ---@param loc Location_ud +---@param corrected boolean ---@return number|nil -function terrain:height_amsl(loc) end +function terrain:height_amsl(loc, corrected) end -- desc ---@return integer diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 2e5c4fadedf..cf71840b63b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -176,7 +176,7 @@ singleton AP_Terrain alias terrain singleton AP_Terrain method enabled boolean singleton AP_Terrain enum TerrainStatusDisabled TerrainStatusUnhealthy TerrainStatusOK singleton AP_Terrain method status uint8_t -singleton AP_Terrain method height_amsl boolean Location float'Null +singleton AP_Terrain method height_amsl boolean Location float'Null boolean singleton AP_Terrain method height_terrain_difference_home boolean float'Null boolean singleton AP_Terrain method height_above_terrain boolean float'Null boolean From 62dc865321df686c26ede1a8b7152f29d928a6ac Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 12:53:17 +1100 Subject: [PATCH 0416/3101] AP_Terrain: added logging of terrain correction --- libraries/AP_Terrain/AP_Terrain.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 4cc81ed3ec1..a9864da0af9 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -69,7 +69,7 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = { AP_GROUPINFO("MARGIN", 3, AP_Terrain, margin, 0.05), // @Param: OFS_MAX - // @DisplayName: Maximum reference offset + // @DisplayName: Terrain reference offset maximum // @Description: The maximum adjustment of terrain altitude based on the assumption that the vehicle is on the ground when it is armed. When the vehicle is armed the location of the vehicle is recorded, and when terrain data is available for that location a height adjustment for terrain data is calculated that aligns the terrain height at that location with the altitude recorded at arming. This height adjustment is applied to all terrain data. This parameter clamps the amount of adjustment. A value of zero disables the use of terrain height adjustment. // @Units: m // @Range: 0 50 @@ -392,7 +392,8 @@ void AP_Terrain::log_terrain_data() terrain_height : terrain_height, current_height : current_height, pending : pending, - loaded : loaded + loaded : loaded, + reference_offset : have_reference_offset?reference_offset:0, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } From ee6a7f21ecce4ff77c721b9299a50e9384ce1a39 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 12:53:32 +1100 Subject: [PATCH 0417/3101] AP_Logger: added terrain correction logging field --- libraries/AP_Logger/LogStructure.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 810e2469714..46a90002872 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -451,6 +451,7 @@ struct PACKED log_TERRAIN { float current_height; uint16_t pending; uint16_t loaded; + float reference_offset; }; struct PACKED log_CSRV { @@ -1171,6 +1172,7 @@ struct PACKED log_VER { // @Field: CHeight: Vehicle height above terrain // @Field: Pending: Number of tile requests outstanding // @Field: Loaded: Number of tiles in memory +// @Field: ROfs: terrain reference offset for arming altitude // @LoggerMessage: TSYN // @Description: Time synchronisation response information @@ -1327,7 +1329,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????", true }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ - "TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded", "s-DU-mm--", "F-GG-00--", true }, \ + "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \ LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_CSRV_MSG, sizeof(log_CSRV), \ "CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000", true }, \ From 5df38d0298ffb986d46086d598b34b354b727236 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Mar 2022 17:29:36 +1100 Subject: [PATCH 0418/3101] AP_RCProtocol: added using_uart() method --- libraries/AP_RCProtocol/AP_RCProtocol.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index f7ba5cf2dfb..aa5a0623624 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -142,6 +142,11 @@ class AP_RCProtocol { bool invert_rx; }; + // return true if we are decoding a byte stream, instead of pulses + bool using_uart(void) const { + return _detected_with_bytes; + } + private: void check_added_uart(void); From b46b0d61a69d294bbd3fa85c4bdfa7e19d06d3f5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Mar 2022 17:30:19 +1100 Subject: [PATCH 0419/3101] HAL_ChibiOS: switch between IOMCU and RCProt rapidly keep the RCProtocol decoder going when IOMCU being used, allowing for fast failover between IOMCU RC input and uart RC input --- libraries/AP_HAL_ChibiOS/RCInput.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index a82b3fdb1b2..9e6cfeb8130 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -181,7 +181,7 @@ void RCInput::_timer_tick(void) _rcin_last_iomcu_ms = 0; } - if (!have_iocmu_rc && rcprot.new_input()) { + if (rcprot.new_input() && !have_iocmu_rc) { WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = AP_HAL::micros(); _num_channels = rcprot.num_channels(); From 456414074576f926cd58699522ab1f2c94b1b6cd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Mar 2022 17:31:20 +1100 Subject: [PATCH 0420/3101] HAL_ChibiOS: display source of RC input distinguish between IOMCU, RCInput with bytes and RCInput with pulses --- libraries/AP_HAL_ChibiOS/RCInput.cpp | 13 ++++++++++--- libraries/AP_HAL_ChibiOS/RCInput.h | 9 +++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index 9e6cfeb8130..6ea3436d9c1 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -150,6 +150,7 @@ void RCInput::_timer_tick(void) } #ifndef HAL_NO_UARTDRIVER const char *rc_protocol = nullptr; + RCSource source = last_source; #endif #ifndef HAL_BUILD_AP_PERIPH @@ -191,12 +192,13 @@ void RCInput::_timer_tick(void) _rx_link_quality = rcprot.get_rx_link_quality(); #ifndef HAL_NO_UARTDRIVER rc_protocol = rcprot.protocol_name(); + source = rcprot.using_uart() ? RCSource::RCPROT_BYTES : RCSource::RCPROT_PULSES; #endif } #endif // HAL_BUILD_AP_PERIPH #if HAL_RCINPUT_WITH_AP_RADIO - if (radio && radio->last_recv_us() != last_radio_us) { + if (radio && radio->last_recv_us() != last_radio_us && !have_iocmu_rc) { last_radio_us = radio->last_recv_us(); WITH_SEMAPHORE(rcin_mutex); _rcin_timestamp_last_signal = last_radio_us; @@ -205,6 +207,9 @@ void RCInput::_timer_tick(void) for (uint8_t i=0; i<_num_channels; i++) { _rc_values[i] = radio->read(i); } +#ifndef HAL_NO_UARTDRIVER + source = RCSource::APRADIO; +#endif } #endif @@ -218,15 +223,17 @@ void RCInput::_timer_tick(void) #ifndef HAL_NO_UARTDRIVER rc_protocol = iomcu.get_rc_protocol(); _rssi = iomcu.get_RSSI(); + source = RCSource::IOMCU; #endif } } #endif #ifndef HAL_NO_UARTDRIVER - if (rc_protocol && rc_protocol != last_protocol) { + if (rc_protocol && (rc_protocol != last_protocol || source != last_source)) { last_protocol = rc_protocol; - GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol); + last_source = source; + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s(%u)", last_protocol, unsigned(source)); } #endif diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index 48eff5767bc..08ba46d58c5 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -72,6 +72,15 @@ class ChibiOS::RCInput : public AP_HAL::RCInput { uint32_t _rcin_last_iomcu_ms; bool _init; const char *last_protocol; + + enum class RCSource { + NONE = 0, + IOMCU = 1, + RCPROT_PULSES = 2, + RCPROT_BYTES = 3, + APRADIO = 4, + } last_source; + bool pulse_input_enabled; #if HAL_RCINPUT_WITH_AP_RADIO From c32a2d542c1d539ca0260cef6e1399edf6930368 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Mar 2022 17:24:37 +1100 Subject: [PATCH 0421/3101] Plane: added release notes for 4.2.0beta4 --- ArduPlane/ReleaseNotes.txt | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 8db9c6e89f4..efb065ce2ec 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,24 @@ +Release 4.2.0beta4 28th March 2022 +---------------------------------- + +This is the 4th beta of the 4.2.0 major release. + + - added BATT_OPTIONS option to send resting voltage (corrected for internal resistance) to the ground station + - fixed a bug when a blended GPS is lost, where the wrong GPS could be used for a short time + - prevent rapid RTL/AUTO switching with DO_LAND_START and fence breach + - added RTL_AUTOLAND=3 to prevent arming check about DO_LAND_START with no RTL_AUTOLAND + - fixed yaw in AUTO mode on the ground on quadplanes when using rudder to disarm + - fixed failover between IOMCU RC input and a secondary RC input on a uart + - display source of RC input with protocol + - fixed DShot reversal bugs with IOMCU based boards + - fixed battery remaining percentage with sum battery backend + - added KakuteH7-bdshot + - added terrain reference adjustment and TERRAIN_OFS_MAX parameter + - fixed param conversion bug (impacts airspeed enable) + - changed MatekH743 to use a 32 bit timer + +Happy flying! + Release 4.2.0beta3 18th March 2022 ---------------------------------- From 7dc59115729a3d8be6ab26b07ceb83ff12ddb7a9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 27 Mar 2022 13:41:34 +1100 Subject: [PATCH 0422/3101] hwdef: changed MatekH743 to a 32 bit timer the 68ms issue is still present on this board. Switching to a 32 bit timer will fix it, but loses us the tonealarm support --- .../hwdef/MatekH743-bdshot/hwdef.dat | 9 +-------- .../AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat | 17 +++++++++-------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat index 06e4834e908..ec449a63038 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat @@ -5,7 +5,7 @@ include ../MatekH743/hwdef.dat # undefine the pins we are going to change -undef PC7 PB0 PB1 PA0 PA1 PA15 PD14 PD15 PE5 PE6 +undef PC7 PA0 PD14 PD15 PE5 PE6 PB0 PB1 PC7 USART6_RX USART6 @@ -13,7 +13,6 @@ PC7 USART6_RX USART6 PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) PA0 TIM2_CH1 TIM2 PWM(3) GPIO(52) BIDIR -PA1 TIM2_CH2 TIM2 PWM(4) GPIO(53) # Disable DMA on PWM9-12 so that the LEDs get a channel PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) NODMA @@ -21,11 +20,5 @@ PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59) NODMA PE5 TIM15_CH1 TIM15 PWM(11) GPIO(60) NODMA PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) NODMA -# Beeper -PA15 BUZZER OUTPUT GPIO(32) LOW -define HAL_BUZZER_PIN 32 -define HAL_BUZZER_ON 1 -define HAL_BUZZER_OFF 0 - DMA_PRIORITY S* TIM3* TIM2* DMA_NOSHARE SPI1* SPI4* TIM3* TIM2* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat index 6f930300db3..ffb4b9bddca 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat @@ -16,10 +16,8 @@ env OPTIMIZE -Os # bootloader takes first sector FLASH_RESERVE_START_KB 128 - # ChibiOS system timer -STM32_ST_USE_TIMER 12 -define CH_CFG_ST_RESOLUTION 16 +STM32_ST_USE_TIMER 5 # USB PA11 OTG_FS_DM OTG1 @@ -145,10 +143,10 @@ PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) # Motors PB0 TIM8_CH2N TIM8 PWM(1) GPIO(50) PB1 TIM8_CH3N TIM8 PWM(2) GPIO(51) -PA0 TIM5_CH1 TIM5 PWM(3) GPIO(52) -PA1 TIM5_CH2 TIM5 PWM(4) GPIO(53) -PA2 TIM5_CH3 TIM5 PWM(5) GPIO(54) -PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55) +PA0 TIM2_CH1 TIM2 PWM(3) GPIO(52) +PA1 TIM2_CH2 TIM2 PWM(4) GPIO(53) +PA2 TIM2_CH3 TIM2 PWM(5) GPIO(54) +PA3 TIM2_CH4 TIM2 PWM(6) GPIO(55) PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57) PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) @@ -158,7 +156,10 @@ PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED # Beeper -PA15 TIM2_CH1 TIM2 GPIO(32) ALARM +PA15 BUZZER OUTPUT GPIO(32) LOW +define HAL_BUZZER_PIN 32 +define HAL_BUZZER_ON 1 +define HAL_BUZZER_OFF 0 # microSD support PC8 SDMMC1_D0 SDMMC1 From bbc259329db272885765ded97843716ce17bdc34 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Thu, 10 Feb 2022 22:27:08 -0500 Subject: [PATCH 0423/3101] AC_AutoTune:tradheli-streamline gcs messages --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 35 +++++++--------------- 1 file changed, 10 insertions(+), 25 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 2922ed052a5..b384464618d 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -330,35 +330,20 @@ void AC_AutoTune_Heli::do_post_test_gcs_announcements() { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ff=%f", (double)tune_rff); break; case RP_UP: - if (is_equal(start_freq,stop_freq)) { - // announce results of dwell - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_rp)); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); - } - break; case RD_UP: - // announce results of dwell - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(test_phase[freq_cnt]), (double)(tune_rd)); - break; case SP_UP: - if (is_equal(start_freq,stop_freq)) { - // announce results of dwell and update - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: phase=%f angle_p=%f", (double)(test_phase[freq_cnt]), (double)(tune_sp)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: tune_accel=%f max_accel=%f", (double)(tune_accel), (double)(test_accel_max)); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); - } - break; case MAX_GAINS: if (is_equal(start_freq,stop_freq)) { - // announce results of dwell and update - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f ph=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt]), (double)(test_phase[freq_cnt])); + // announce results of dwell + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(test_freq[freq_cnt]), (double)(test_gain[freq_cnt])); + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(test_phase[freq_cnt])); + if (tune_type == RP_UP) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp)); + } else if (tune_type == RD_UP) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd)); + } else if (tune_type == SP_UP) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(tune_accel), (double)(test_accel_max)); + } } else { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep.maxgain.freq), (double)(sweep.maxgain.gain)); gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep.ph180.freq), (double)(sweep.ph180.gain)); From ba2822748350094578f316f6d562505aa3983354 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sat, 5 Mar 2022 22:16:50 -0500 Subject: [PATCH 0424/3101] AC_AutoTune: combine dwell_init methods --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 160 ++++++++++----------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 11 +- 2 files changed, 84 insertions(+), 87 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index b384464618d..a5a2b4415e0 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -156,11 +156,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine_gain function whenever test is initialized freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); - dwell_test_init(start_freq, stop_freq); + dwell_test_init(start_freq, stop_freq, RATE); } else { // initialize determine_gain function whenever test is initialized freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); - dwell_test_init(start_freq, start_freq); + dwell_test_init(start_freq, start_freq, RATE); } if (!is_zero(start_freq)) { // 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it. @@ -186,11 +186,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine gain function freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); - angle_dwell_test_init(start_freq, stop_freq); + dwell_test_init(start_freq, stop_freq, ANGLE); } else { // initialize determine gain function freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); - angle_dwell_test_init(start_freq, start_freq); + dwell_test_init(start_freq, start_freq, ANGLE); } // TODO add time limit for sweep test @@ -910,52 +910,90 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd } -void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq) +void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type) { - rotation_rate_filt.reset(0); + dwell_start_time_ms = 0.0f; + settle_time = 200; + rotation_rate_filt.set_cutoff_frequency(filt_freq); - command_filt.reset(0); command_filt.set_cutoff_frequency(filt_freq); - target_rate_filt.reset(0); target_rate_filt.set_cutoff_frequency(filt_freq); - filt_target_rate = 0.0f; - dwell_start_time_ms = 0.0f; - settle_time = 200; - if (!is_equal(start_freq, stop_freq)) { - reset_sweep_variables(); - curr_test.gain = 0.0f; - curr_test.phase = 0.0f; + + if (dwell_type == RATE) { + rotation_rate_filt.reset(0); + command_filt.reset(0); + target_rate_filt.reset(0); + rotation_rate = 0.0f; + command_out = 0.0f; + filt_target_rate = 0.0f; + } else { + switch (axis) { + case ROLL: + rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f); + command_filt.reset(motors->get_roll()); + target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f); + rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f; + command_out = motors->get_roll(); + filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; + break; + case PITCH: + rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f); + command_filt.reset(motors->get_pitch()); + target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f); + rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f; + command_out = motors->get_pitch(); + filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; + break; + case YAW: + // yaw angle will be centered on zero by removing trim heading + rotation_rate_filt.reset(0.0f); + command_filt.reset(motors->get_yaw()); + target_rate_filt.reset(0.0f); + rotation_rate = 0.0f; + command_out = motors->get_yaw(); + filt_target_rate = 0.0f; + break; + } } // filter at lower frequency to remove steady state filt_command_reading.set_cutoff_frequency(0.2f * start_frq); filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq); filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq); - filt_pit_roll_cd.set_cutoff_frequency(0.2f * start_frq); - filt_heading_error_cd.set_cutoff_frequency(0.2f * start_frq); filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq); - // save the trim output from PID controller - float ff_term = 0.0f; - float p_term = 0.0f; - switch (axis) { - case ROLL: - trim_meas_rate = ahrs_view->get_gyro().x; - ff_term = attitude_control->get_rate_roll_pid().get_ff(); - p_term = attitude_control->get_rate_roll_pid().get_p(); - break; - case PITCH: - trim_meas_rate = ahrs_view->get_gyro().y; - ff_term = attitude_control->get_rate_pitch_pid().get_ff(); - p_term = attitude_control->get_rate_pitch_pid().get_p(); - break; - case YAW: - trim_meas_rate = ahrs_view->get_gyro().z; - ff_term = attitude_control->get_rate_yaw_pid().get_ff(); - p_term = attitude_control->get_rate_yaw_pid().get_p(); - break; + if (dwell_type == RATE) { + filt_pit_roll_cd.set_cutoff_frequency(0.2f * start_frq); + filt_heading_error_cd.set_cutoff_frequency(0.2f * start_frq); + + // save the trim output from PID controller + float ff_term = 0.0f; + float p_term = 0.0f; + switch (axis) { + case ROLL: + trim_meas_rate = ahrs_view->get_gyro().x; + ff_term = attitude_control->get_rate_roll_pid().get_ff(); + p_term = attitude_control->get_rate_roll_pid().get_p(); + break; + case PITCH: + trim_meas_rate = ahrs_view->get_gyro().y; + ff_term = attitude_control->get_rate_pitch_pid().get_ff(); + p_term = attitude_control->get_rate_pitch_pid().get_p(); + break; + case YAW: + trim_meas_rate = ahrs_view->get_gyro().z; + ff_term = attitude_control->get_rate_yaw_pid().get_ff(); + p_term = attitude_control->get_rate_yaw_pid().get_p(); + break; + } + trim_pff_out = ff_term + p_term; + } + + if (!is_equal(start_freq, stop_freq)) { + reset_sweep_variables(); + curr_test.gain = 0.0f; + curr_test.phase = 0.0f; } - trim_pff_out = ff_term + p_term; } void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) @@ -1163,54 +1201,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, } } -void AC_AutoTune_Heli::angle_dwell_test_init(float start_frq, float filt_freq) -{ - rotation_rate_filt.set_cutoff_frequency(filt_freq); - command_filt.set_cutoff_frequency(filt_freq); - target_rate_filt.set_cutoff_frequency(filt_freq); - dwell_start_time_ms = 0.0f; - settle_time = 200; - switch (axis) { - case ROLL: - rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f); - command_filt.reset(motors->get_roll()); - target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f); - rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f; - command_out = motors->get_roll(); - filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; - break; - case PITCH: - rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f); - command_filt.reset(motors->get_pitch()); - target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f); - rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f; - command_out = motors->get_pitch(); - filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; - break; - case YAW: - // yaw angle will be centered on zero by removing trim heading - rotation_rate_filt.reset(0.0f); - command_filt.reset(motors->get_yaw()); - target_rate_filt.reset(0.0f); - rotation_rate = 0.0f; - command_out = motors->get_yaw(); - filt_target_rate = 0.0f; - break; - } - - // filter at lower frequency to remove steady state - filt_command_reading.set_cutoff_frequency(0.2f * start_frq); - filt_gyro_reading.set_cutoff_frequency(0.2f * start_frq); - filt_tgt_rate_reading.set_cutoff_frequency(0.2f * start_frq); - filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * start_frq); - - if (!is_equal(start_freq, stop_freq)) { - reset_sweep_variables(); - curr_test.gain = 0.0f; - curr_test.phase = 0.0f; - } -} - void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) { float gyro_reading = 0.0f; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index ce6937cac6c..9bbed21dff5 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -136,16 +136,23 @@ class AC_AutoTune_Heli : public AC_AutoTune // max gain data for rate d tuning max_gain_data max_rate_d; + // dwell type identifies whether the dwell is ran on rate or angle + enum DwellType { + RATE = 0, + ANGLE = 1, + }; + // Feedforward test used to determine Rate FF gain void rate_ff_test_init(); void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign); + // initialize dwell test or angle dwell test variables + void dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type); + // dwell test used to perform frequency dwells for rate gains - void dwell_test_init(float start_frq, float filt_freq); void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); // dwell test used to perform frequency dwells for angle gains - void angle_dwell_test_init(float start_frq, float filt_freq); void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); // generates waveform for frequency sweep excitations From ce725764d49959a7c9a9563f4ff151a55a64b53a Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sun, 6 Mar 2022 19:32:12 -0500 Subject: [PATCH 0425/3101] AC_AutoTune: combine dwell_run_test for angle and rate --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 423 ++++++++------------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 11 +- 2 files changed, 162 insertions(+), 272 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index a5a2b4415e0..d246d2812d6 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -155,11 +155,11 @@ void AC_AutoTune_Heli::test_init() } if (!is_equal(start_freq,stop_freq)) { // initialize determine_gain function whenever test is initialized - freqresp_rate.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, stop_freq, RATE); } else { // initialize determine_gain function whenever test is initialized - freqresp_rate.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); + freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); dwell_test_init(start_freq, start_freq, RATE); } if (!is_zero(start_freq)) { @@ -185,11 +185,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine gain function - freqresp_angle.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); dwell_test_init(start_freq, stop_freq, ANGLE); } else { // initialize determine gain function - freqresp_angle.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); + freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); dwell_test_init(start_freq, start_freq, ANGLE); } @@ -246,13 +246,13 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) break; case RP_UP: case RD_UP: - dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], RATE); break; case MAX_GAINS: - dwell_test_run(0, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + dwell_test_run(0, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], RATE); break; case SP_UP: - angle_dwell_test_run(start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt]); + dwell_test_run(1, start_freq, stop_freq, test_gain[freq_cnt], test_phase[freq_cnt], ANGLE); break; default: step = UPDATE_GAINS; @@ -996,131 +996,173 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq, DwellTy } } -void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) +void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type) { float gyro_reading = 0.0f; float command_reading = 0.0f; float tgt_rate_reading = 0.0f; - float tgt_attitude = 2.5f * 0.01745f; + float tgt_attitude; const uint32_t now = AP_HAL::millis(); + float target_angle_cd; float target_rate_cds; float sweep_time_ms = 23000; + float dwell_freq = start_frq; + float target_rate_mag_cds; const float att_hold_gain = 4.5f; - Vector3f attitude_cd; - float dwell_freq = start_frq; float cycle_time_ms = 0; if (!is_zero(dwell_freq)) { - cycle_time_ms = 1000.0f * 2.0f * M_PI / dwell_freq; + cycle_time_ms = 1000.0f * M_2PI / dwell_freq; + } + + if (dwell_type == RATE) { + // keep controller from requesting too high of a rate + tgt_attitude = 2.5f * 0.01745f; + target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f; + if (target_rate_mag_cds > 5000.0f) { + target_rate_mag_cds = 5000.0f; + } + } else { + tgt_attitude = 5.0f * 0.01745f; + // adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized + const float freq_co = 1.0f / attitude_control->get_input_tc(); + const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f; + tgt_attitude = constrain_float(0.08725f * (1.0f + 0.2f * added_ampl), 0.08725f, 0.5235f); } - attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor); + // body frame calculation of velocity Vector3f velocity_ned, velocity_bf; if (ahrs_view->get_velocity_NED(velocity_ned)) { velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw(); velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw(); } - // keep controller from requesting too high of a rate - float target_rate_mag_cds = dwell_freq * tgt_attitude * 5730.0f; - if (target_rate_mag_cds > 5000.0f) { - target_rate_mag_cds = 5000.0f; - } + Vector3f attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor); if (settle_time == 0) { - // give gentler start for the dwell - if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) { - target_rate_cds = -0.5f * target_rate_mag_cds * sinf(dwell_freq * (now - dwell_start_time_ms) * 0.001); + if (dwell_type == RATE) { + target_rate_cds = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, target_rate_mag_cds, start_frq, stop_frq); + filt_pit_roll_cd.apply(Vector2f(attitude_cd.x,attitude_cd.y), AP::scheduler().get_loop_period_s()); + filt_heading_error_cd.apply(wrap_180_cd(trim_attitude_cd.z - attitude_cd.z), AP::scheduler().get_loop_period_s()); } else { - if (is_equal(start_frq,stop_frq)) { - target_rate_cds = - target_rate_mag_cds * cosf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001); - } else { - target_rate_cds = waveform((now - dwell_start_time_ms - 0.5f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.5f * cycle_time_ms) * 0.001f, target_rate_mag_cds, start_frq, stop_frq); - dwell_freq = waveform_freq_rads; - } + target_angle_cd = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq); } - filt_pit_roll_cd.apply(Vector2f(attitude_cd.x,attitude_cd.y), AP::scheduler().get_loop_period_s()); - filt_heading_error_cd.apply(wrap_180_cd(trim_attitude_cd.z - attitude_cd.z), AP::scheduler().get_loop_period_s()); - Vector2f att_fdbk = Vector2f(-5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x); + const Vector2f att_fdbk { + -5730.0f * vel_hold_gain * velocity_bf.y, + 5730.0f * vel_hold_gain * velocity_bf.x + }; filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); + dwell_freq = waveform_freq_rads; } else { - target_rate_cds = 0.0f; - settle_time--; + if (dwell_type == RATE) { + target_rate_cds = 0.0f; + trim_command = command_out; + trim_attitude_cd = attitude_cd; + filt_pit_roll_cd.reset(Vector2f(attitude_cd.x,attitude_cd.y)); + filt_heading_error_cd.reset(0.0f); + } else { + target_angle_cd = 0.0f; + trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z; + trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor; + } dwell_start_time_ms = now; - trim_command = command_out; - trim_attitude_cd = attitude_cd; - filt_pit_roll_cd.reset(Vector2f(attitude_cd.x,attitude_cd.y)); - filt_heading_error_cd.reset(0.0f); filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); + settle_time--; } - // limit rate correction for position hold - Vector3f trim_rate_cds { - constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f), - constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f), - constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f) - }; - - switch (axis) { - case ROLL: - gyro_reading = ahrs_view->get_gyro().x; - command_reading = motors->get_roll(); - tgt_rate_reading = attitude_control->rate_bf_targets().x; - if (settle_time == 0) { - float ff_rate_contr = 0.0f; - if (tune_roll_rff > 0.0f) { - ff_rate_contr = 5730.0f * trim_command / tune_roll_rff; - } - trim_rate_cds.x += ff_rate_contr; - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, trim_rate_cds.y, 0.0f); - attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds.x); - } else { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (!is_zero(attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP())) { - float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_roll_pid().kP()) / (attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP()); - attitude_control->rate_bf_roll_target(trim_tgt_rate_cds); + if (dwell_type == RATE) { + // limit rate correction for position hold + Vector3f trim_rate_cds { + constrain_float(att_hold_gain * ((trim_attitude_cd.x + filt_att_fdbk_from_velxy_cd.get().x) - filt_pit_roll_cd.get().x), -15000.0f, 15000.0f), + constrain_float(att_hold_gain * ((trim_attitude_cd.y + filt_att_fdbk_from_velxy_cd.get().y) - filt_pit_roll_cd.get().y), -15000.0f, 15000.0f), + constrain_float(att_hold_gain * filt_heading_error_cd.get(), -15000.0f, 15000.0f) + }; + switch (axis) { + case ROLL: + gyro_reading = ahrs_view->get_gyro().x; + command_reading = motors->get_roll(); + tgt_rate_reading = attitude_control->rate_bf_targets().x; + if (settle_time == 0) { + float ff_rate_contr = 0.0f; + if (tune_roll_rff > 0.0f) { + ff_rate_contr = 5730.0f * trim_command / tune_roll_rff; + } + trim_rate_cds.x += ff_rate_contr; + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, trim_rate_cds.y, 0.0f); + attitude_control->rate_bf_roll_target(target_rate_cds + trim_rate_cds.x); + } else { + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_roll_pid().kP()) / (attitude_control->get_rate_roll_pid().ff() + attitude_control->get_rate_roll_pid().kP()); + attitude_control->rate_bf_roll_target(trim_tgt_rate_cds); + } } - } - break; - case PITCH: - gyro_reading = ahrs_view->get_gyro().y; - command_reading = motors->get_pitch(); - tgt_rate_reading = attitude_control->rate_bf_targets().y; - if (settle_time == 0) { - float ff_rate_contr = 0.0f; - if (tune_pitch_rff > 0.0f) { - ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff; + break; + case PITCH: + gyro_reading = ahrs_view->get_gyro().y; + command_reading = motors->get_pitch(); + tgt_rate_reading = attitude_control->rate_bf_targets().y; + if (settle_time == 0) { + float ff_rate_contr = 0.0f; + if (tune_pitch_rff > 0.0f) { + ff_rate_contr = 5730.0f * trim_command / tune_pitch_rff; + } + trim_rate_cds.y += ff_rate_contr; + attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, 0.0f, 0.0f); + attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds.y); + } else { + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_pitch_pid().kP()) / (attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP()); + attitude_control->rate_bf_pitch_target(trim_tgt_rate_cds); + } } - trim_rate_cds.y += ff_rate_contr; - attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, 0.0f, 0.0f); - attitude_control->rate_bf_pitch_target(target_rate_cds + trim_rate_cds.y); - } else { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (!is_zero(attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP())) { - float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_pitch_pid().kP()) / (attitude_control->get_rate_pitch_pid().ff() + attitude_control->get_rate_pitch_pid().kP()); - attitude_control->rate_bf_pitch_target(trim_tgt_rate_cds); + break; + case YAW: + gyro_reading = ahrs_view->get_gyro().z; + command_reading = motors->get_yaw(); + tgt_rate_reading = attitude_control->rate_bf_targets().z; + if (settle_time == 0) { + float rp_rate_contr = 0.0f; + if (tune_yaw_rp > 0.0f) { + rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp; + } + trim_rate_cds.z += rp_rate_contr; + attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, trim_rate_cds.y, 0.0f); + attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds.z); + } else { + attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); + if (!is_zero(attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP())) { + float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_yaw_pid().kP()) / (attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP()); + attitude_control->rate_bf_yaw_target(trim_tgt_rate_cds); + } } + break; } - break; - case YAW: - gyro_reading = ahrs_view->get_gyro().z; - command_reading = motors->get_yaw(); - tgt_rate_reading = attitude_control->rate_bf_targets().z; - if (settle_time == 0) { - float rp_rate_contr = 0.0f; - if (tune_yaw_rp > 0.0f) { - rp_rate_contr = 5730.0f * trim_command / tune_yaw_rp; - } - trim_rate_cds.z += rp_rate_contr; - attitude_control->input_rate_bf_roll_pitch_yaw(trim_rate_cds.x, trim_rate_cds.y, 0.0f); - attitude_control->rate_bf_yaw_target(target_rate_cds + trim_rate_cds.z); - } else { - attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f); - if (!is_zero(attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP())) { - float trim_tgt_rate_cds = 5730.0f * (trim_pff_out + trim_meas_rate * attitude_control->get_rate_yaw_pid().kP()) / (attitude_control->get_rate_yaw_pid().ff() + attitude_control->get_rate_yaw_pid().kP()); - attitude_control->rate_bf_yaw_target(trim_tgt_rate_cds); - } + } else { + const Vector2f trim_angle_cd { + constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f), + constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f) + }; + switch (axis) { + case ROLL: + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); + command_reading = motors->get_roll(); + tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; + gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f; + break; + case PITCH: + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f); + command_reading = motors->get_pitch(); + tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; + gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f; + break; + case YAW: + command_reading = motors->get_yaw(); + tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f; + gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f; + attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false); + break; } - break; } if (settle_time == 0) { @@ -1135,33 +1177,35 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, // looks at gain and phase of input rate to output rate rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()), - AP::scheduler().get_loop_period_s()); + AP::scheduler().get_loop_period_s()); filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()), - AP::scheduler().get_loop_period_s()); + AP::scheduler().get_loop_period_s()); command_out = command_filt.apply((command_reading - filt_command_reading.get()), - AP::scheduler().get_loop_period_s()); + AP::scheduler().get_loop_period_s()); - // wait for dwell to start before determining gain and phase or just start if sweep + // wait for dwell to start before determining gain and phase if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { if (freq_resp_input == 1) { - freqresp_rate.update(command_out,filt_target_rate,rotation_rate, dwell_freq); + freqresp.update(command_out,filt_target_rate,rotation_rate, dwell_freq); } else { - freqresp_rate.update(command_out,command_out,rotation_rate, dwell_freq); + freqresp.update(command_out,command_out,rotation_rate, dwell_freq); } - if (freqresp_rate.is_cycle_complete()) { + + if (freqresp.is_cycle_complete()) { if (!is_equal(start_frq,stop_frq)) { - curr_test.freq = freqresp_rate.get_freq(); - curr_test.gain = freqresp_rate.get_gain(); - curr_test.phase = freqresp_rate.get_phase(); + curr_test.freq = freqresp.get_freq(); + curr_test.gain = freqresp.get_gain(); + curr_test.phase = freqresp.get_phase(); + if (dwell_type == ANGLE) {test_accel_max = freqresp.get_accel_max();} // reset cycle_complete to allow indication of next cycle - freqresp_rate.reset_cycle_complete(); + freqresp.reset_cycle_complete(); // log sweep data Log_AutoTuneSweep(); } else { - dwell_gain = freqresp_rate.get_gain(); - dwell_phase = freqresp_rate.get_phase(); + dwell_gain = freqresp.get_gain(); + dwell_phase = freqresp.get_phase(); + if (dwell_type == ANGLE) {test_accel_max = freqresp.get_accel_max();} } - } } @@ -1174,157 +1218,9 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, sweep.progress = 2; } if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f && sweep.progress == 0) { - sweep.ph180.freq = curr_test.freq; - sweep.ph180.gain = curr_test.gain; - sweep.ph180.phase = curr_test.phase; - } - if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) { - sweep.ph270.freq = curr_test.freq; - sweep.ph270.gain = curr_test.gain; - sweep.ph270.phase = curr_test.phase; - } - if (curr_test.gain > sweep.maxgain.gain) { - sweep.maxgain.gain = curr_test.gain; - sweep.maxgain.freq = curr_test.freq; - sweep.maxgain.phase = curr_test.phase; - } - if (now - step_start_time_ms >= sweep_time_ms + 200) { - // we have passed the maximum stop time - step = UPDATE_GAINS; - } - - } else { - if (now - step_start_time_ms >= step_time_limit_ms || freqresp_rate.is_cycle_complete()) { - // we have passed the maximum stop time - step = UPDATE_GAINS; - } - } -} - -void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase) -{ - float gyro_reading = 0.0f; - float command_reading = 0.0f; - float tgt_rate_reading = 0.0f; - float tgt_attitude = 5.0f * 0.01745f; - const uint32_t now = AP_HAL::millis(); - float target_angle_cd; - float sweep_time_ms = 23000; - float dwell_freq = start_frq; - - // adjust target attitude based on input_tc so amplitude decrease with increased frequency is minimized - const float freq_co = 1.0f / attitude_control->get_input_tc(); - const float added_ampl = (safe_sqrt(powf(dwell_freq,2.0) + powf(freq_co,2.0)) / freq_co) - 1.0f; - tgt_attitude = constrain_float(0.08725f * (1.0f + 0.2f * added_ampl), 0.08725f, 0.5235f); - - float cycle_time_ms = 0; - if (!is_zero(dwell_freq)) { - cycle_time_ms = 1000.0f * 6.28f / dwell_freq; - } - - // body frame calculation of velocity - Vector3f velocity_ned, velocity_bf; - if (ahrs_view->get_velocity_NED(velocity_ned)) { - velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw(); - velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw(); - } - - if (settle_time == 0) { - // give gentler start for the dwell - if ((float)(now - dwell_start_time_ms) < 0.5f * cycle_time_ms) { - target_angle_cd = 0.5f * tgt_attitude * 5730.0f * (cosf(dwell_freq * (now - dwell_start_time_ms) * 0.001) - 1.0f); - } else { - if (is_equal(start_frq,stop_frq)) { - target_angle_cd = -tgt_attitude * 5730.0f * sinf(dwell_freq * (now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001); - } else { - target_angle_cd = -waveform((now - dwell_start_time_ms - 0.25f * cycle_time_ms) * 0.001, (sweep_time_ms - 0.25f * cycle_time_ms) * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq); - dwell_freq = waveform_freq_rads; - } - } - const Vector2f att_fdbk { - -5730.0f * vel_hold_gain * velocity_bf.y, - 5730.0f * vel_hold_gain * velocity_bf.x - }; - filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); - } else { - target_angle_cd = 0.0f; - trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z; - trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor; - settle_time--; - dwell_start_time_ms = now; - filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f)); - } - - const Vector2f trim_angle_cd { - constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f), - constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f) - }; - switch (axis) { - case ROLL: - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); - command_reading = motors->get_roll(); - tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; - gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f; - break; - case PITCH: - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f); - command_reading = motors->get_pitch(); - tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; - gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f; - break; - case YAW: - command_reading = motors->get_yaw(); - tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f; - gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f; - attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false); - break; - } - - if (settle_time == 0) { - filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s()); - filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s()); - filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s()); - } else { - filt_command_reading.reset(command_reading); - filt_gyro_reading.reset(gyro_reading); - filt_tgt_rate_reading.reset(tgt_rate_reading); - } - - // looks at gain and phase of input rate to output rate - rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()), - AP::scheduler().get_loop_period_s()); - filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()), - AP::scheduler().get_loop_period_s()); - command_out = command_filt.apply((command_reading - filt_command_reading.get()), - AP::scheduler().get_loop_period_s()); - - // wait for dwell to start before determining gain and phase - if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) { - freqresp_angle.update(command_out, filt_target_rate, rotation_rate, dwell_freq); - if (freqresp_angle.is_cycle_complete()) { - if (!is_equal(start_frq,stop_frq)) { - curr_test.freq = freqresp_angle.get_freq(); - curr_test.gain = freqresp_angle.get_gain(); - curr_test.phase = freqresp_angle.get_phase(); - test_accel_max = freqresp_angle.get_accel_max(); - // reset cycle_complete to allow indication of next cycle - freqresp_angle.reset_cycle_complete(); - // log sweep data - Log_AutoTuneSweep(); - } else { - dwell_gain = freqresp_angle.get_gain(); - dwell_phase = freqresp_angle.get_phase(); - test_accel_max = freqresp_angle.get_accel_max(); - } - } - } - - // set sweep data if a frequency sweep is being conducted - if (!is_equal(start_frq,stop_frq)) { - if (curr_test.phase <= 160.0f && curr_test.phase >= 150.0f) { sweep.ph180 = curr_test; } - if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f) { + if (curr_test.phase <= 250.0f && curr_test.phase >= 240.0f && sweep.progress == 1) { sweep.ph270 = curr_test; } if (curr_test.gain > sweep.maxgain.gain) { @@ -1335,13 +1231,12 @@ void AC_AutoTune_Heli::angle_dwell_test_run(float start_frq, float stop_frq, flo step = UPDATE_GAINS; } } else { - if (now - step_start_time_ms >= step_time_limit_ms || freqresp_angle.is_cycle_complete()) { + if (now - step_start_time_ms >= step_time_limit_ms || freqresp.is_cycle_complete()) { // we have passed the maximum stop time step = UPDATE_GAINS; } } } - // init_test - initialises the test float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax) { diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 9bbed21dff5..022c52b8f1c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -150,10 +150,7 @@ class AC_AutoTune_Heli : public AC_AutoTune void dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type); // dwell test used to perform frequency dwells for rate gains - void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); - - // dwell test used to perform frequency dwells for angle gains - void angle_dwell_test_run(float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase); + void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type); // generates waveform for frequency sweep excitations float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax); @@ -286,8 +283,6 @@ class AC_AutoTune_Heli : public AC_AutoTune AP_Float max_resp_gain; // maximum response gain AP_Float vel_hold_gain; // gain for velocity hold - // freqresp object for the rate frequency response tests - AC_AutoTune_FreqResp freqresp_rate; - // freqresp object for the angle frequency response tests - AC_AutoTune_FreqResp freqresp_angle; + // freqresp object for the frequency response tests + AC_AutoTune_FreqResp freqresp; }; From 26297069d3e78c459849341050319fa469ca5e43 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Mon, 14 Mar 2022 22:53:57 -0400 Subject: [PATCH 0426/3101] Copter: make systemid use new chirp math function --- ArduCopter/mode.h | 4 +++- ArduCopter/mode_systemid.cpp | 44 ++++-------------------------------- 2 files changed, 7 insertions(+), 41 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 75f23c8d0a4..8f41a3ff581 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1,6 +1,7 @@ #pragma once #include "Copter.h" +#include class Parameters; class ParametersG2; @@ -1481,6 +1482,8 @@ class ModeSystemId : public Mode { static const struct AP_Param::GroupInfo var_info[]; + Chirp chirp_input; + protected: const char *name() const override { return "SYSTEMID"; } @@ -1489,7 +1492,6 @@ class ModeSystemId : public Mode { private: void log_data() const; - float waveform(float time); enum class AxisType { NONE = 0, // none diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 63ac09a1e1f..0ce41da15ac 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -95,6 +95,8 @@ bool ModeSystemId::init(bool ignore_checks) systemid_state = SystemIDModeState::SYSTEMID_STATE_TESTING; log_subsample = 0; + chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq); + gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis); copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); @@ -172,7 +174,8 @@ void ModeSystemId::run() } waveform_time += G_Dt; - waveform_sample = waveform(waveform_time - SYSTEM_ID_DELAY); + waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude); + waveform_freq_rads = chirp_input.get_frequency_rads(); switch (systemid_state) { case SystemIDModeState::SYSTEMID_STATE_STOPPED: @@ -291,43 +294,4 @@ void ModeSystemId::log_data() const copter.Log_Write_Attitude(); } -// init_test - initialises the test -float ModeSystemId::waveform(float time) -{ - float wMin = 2 * M_PI * frequency_start; - float wMax = 2 * M_PI * frequency_stop; - - float window; - float output; - - float B = logf(wMax / wMin); - - if (time <= 0.0f) { - window = 0.0f; - } else if (time <= time_fade_in) { - window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in); - } else if (time <= time_record - time_fade_out) { - window = 1.0; - } else if (time <= time_record) { - window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI); - } else { - window = 0.0; - } - - if (time <= 0.0f) { - waveform_freq_rads = wMin; - output = 0.0f; - } else if (time <= time_const_freq) { - waveform_freq_rads = wMin; - output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq); - } else if (time <= time_record) { - waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq)); - output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1)); - } else { - waveform_freq_rads = wMax; - output = 0.0f; - } - return output; -} - #endif From bd0df72863437136b66f96a082bb03080d9421b0 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Mon, 7 Mar 2022 00:23:41 -0500 Subject: [PATCH 0427/3101] AC_AutoTune: use chirp function in AP_Math for frequency sweeps --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 61 +++++----------------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 12 +++-- 2 files changed, 20 insertions(+), 53 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index d246d2812d6..f718c09b640 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -156,11 +156,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine_gain function whenever test is initialized freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::RATE); - dwell_test_init(start_freq, stop_freq, RATE); + dwell_test_init(start_freq, stop_freq, stop_freq, RATE); } else { // initialize determine_gain function whenever test is initialized freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::RATE); - dwell_test_init(start_freq, start_freq, RATE); + dwell_test_init(start_freq, stop_freq, start_freq, RATE); } if (!is_zero(start_freq)) { // 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it. @@ -186,11 +186,11 @@ void AC_AutoTune_Heli::test_init() if (!is_equal(start_freq,stop_freq)) { // initialize determine gain function freqresp.init(AC_AutoTune_FreqResp::InputType::SWEEP, AC_AutoTune_FreqResp::ResponseType::ANGLE); - dwell_test_init(start_freq, stop_freq, ANGLE); + dwell_test_init(start_freq, stop_freq, stop_freq, ANGLE); } else { // initialize determine gain function freqresp.init(AC_AutoTune_FreqResp::InputType::DWELL, AC_AutoTune_FreqResp::ResponseType::ANGLE); - dwell_test_init(start_freq, start_freq, ANGLE); + dwell_test_init(start_freq, stop_freq, start_freq, ANGLE); } // TODO add time limit for sweep test @@ -910,7 +910,7 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd } -void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type) +void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float filt_freq, DwellType dwell_type) { dwell_start_time_ms = 0.0f; settle_time = 200; @@ -989,11 +989,14 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float filt_freq, DwellTy trim_pff_out = ff_term + p_term; } - if (!is_equal(start_freq, stop_freq)) { + if (!is_equal(start_frq, stop_frq)) { reset_sweep_variables(); curr_test.gain = 0.0f; curr_test.phase = 0.0f; } + + chirp_input.init(sweep_time_ms * 0.001f, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * sweep_time_ms, 0.0f); + } void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type) @@ -1005,7 +1008,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, const uint32_t now = AP_HAL::millis(); float target_angle_cd; float target_rate_cds; - float sweep_time_ms = 23000; float dwell_freq = start_frq; float target_rate_mag_cds; const float att_hold_gain = 4.5f; @@ -1040,18 +1042,18 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, Vector3f attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor); if (settle_time == 0) { if (dwell_type == RATE) { - target_rate_cds = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, target_rate_mag_cds, start_frq, stop_frq); + target_rate_cds = -chirp_input.update((now - dwell_start_time_ms) * 0.001, target_rate_mag_cds); filt_pit_roll_cd.apply(Vector2f(attitude_cd.x,attitude_cd.y), AP::scheduler().get_loop_period_s()); filt_heading_error_cd.apply(wrap_180_cd(trim_attitude_cd.z - attitude_cd.z), AP::scheduler().get_loop_period_s()); } else { - target_angle_cd = -waveform((now - dwell_start_time_ms) * 0.001, sweep_time_ms * 0.001f, tgt_attitude * 5730.0f, start_frq, stop_frq); + target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, tgt_attitude * 5730.0f); } const Vector2f att_fdbk { -5730.0f * vel_hold_gain * velocity_bf.y, 5730.0f * vel_hold_gain * velocity_bf.x }; filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s()); - dwell_freq = waveform_freq_rads; + dwell_freq = chirp_input.get_frequency_rads(); } else { if (dwell_type == RATE) { target_rate_cds = 0.0f; @@ -1237,45 +1239,6 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, } } } -// init_test - initialises the test -float AC_AutoTune_Heli::waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax) -{ - float time_fade_in = 0.0f; // Time to reach maximum amplitude of chirp - float time_fade_out = 0.1 * time_record; // Time to reach zero amplitude after chirp finishes - float time_const_freq = 0.0f; - - float window; - float output; - - float B = logf(wMax / wMin); - - if (time <= 0.0f) { - window = 0.0f; - } else if (time <= time_fade_in) { - window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in); - } else if (time <= time_record - time_fade_out) { - window = 1.0; - } else if (time <= time_record) { - window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI); - } else { - window = 0.0; - } - - if (time <= 0.0f) { - waveform_freq_rads = wMin; - output = 0.0f; - } else if (time <= time_const_freq) { - waveform_freq_rads = wMin; - output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq); - } else if (time <= time_record) { - waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq)); - output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1)); - } else { - waveform_freq_rads = wMax; - output = 0.0f; - } - return output; -} // update gains for the rate p up tune type void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 022c52b8f1c..72a18f16876 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -19,6 +19,7 @@ #pragma once #include "AC_AutoTune.h" +#include class AC_AutoTune_Heli : public AC_AutoTune { @@ -147,14 +148,11 @@ class AC_AutoTune_Heli : public AC_AutoTune void rate_ff_test_run(float max_angle_cds, float target_rate_cds, float dir_sign); // initialize dwell test or angle dwell test variables - void dwell_test_init(float start_frq, float filt_freq, DwellType dwell_type); + void dwell_test_init(float start_frq, float stop_frq, float filt_freq, DwellType dwell_type); // dwell test used to perform frequency dwells for rate gains void dwell_test_run(uint8_t freq_resp_input, float start_frq, float stop_frq, float &dwell_gain, float &dwell_phase, DwellType dwell_type); - // generates waveform for frequency sweep excitations - float waveform(float time, float time_record, float waveform_magnitude, float wMin, float wMax); - // updating_rate_ff_up - adjust FF to ensure the target is reached // FF is adjusted until rate requested is acheived void updating_rate_ff_up(float &tune_ff, float rate_target, float meas_rate, float meas_command); @@ -275,6 +273,10 @@ class AC_AutoTune_Heli : public AC_AutoTune }; sweep_data sweep; + // fix the frequency sweep time to 23 seconds + const float sweep_time_ms = 23000; + + // parameters AP_Int8 axis_bitmask; // axes to be tuned AP_Int8 seq_bitmask; // tuning sequence bitmask @@ -285,4 +287,6 @@ class AC_AutoTune_Heli : public AC_AutoTune // freqresp object for the frequency response tests AC_AutoTune_FreqResp freqresp; + + Chirp chirp_input; }; From f1a6865caa21bb90e50c01ee6d0f4ab9bdcc53fa Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sat, 19 Mar 2022 23:49:46 -0400 Subject: [PATCH 0428/3101] AC_Autotune: clean up variable init for dwell --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 42 ++++------------------ 1 file changed, 6 insertions(+), 36 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index f718c09b640..5267e662694 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -919,42 +919,12 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float fi command_filt.set_cutoff_frequency(filt_freq); target_rate_filt.set_cutoff_frequency(filt_freq); - if (dwell_type == RATE) { - rotation_rate_filt.reset(0); - command_filt.reset(0); - target_rate_filt.reset(0); - rotation_rate = 0.0f; - command_out = 0.0f; - filt_target_rate = 0.0f; - } else { - switch (axis) { - case ROLL: - rotation_rate_filt.reset(((float)ahrs_view->roll_sensor) / 5730.0f); - command_filt.reset(motors->get_roll()); - target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f); - rotation_rate = ((float)ahrs_view->roll_sensor) / 5730.0f; - command_out = motors->get_roll(); - filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; - break; - case PITCH: - rotation_rate_filt.reset(((float)ahrs_view->pitch_sensor) / 5730.0f); - command_filt.reset(motors->get_pitch()); - target_rate_filt.reset(((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f); - rotation_rate = ((float)ahrs_view->pitch_sensor) / 5730.0f; - command_out = motors->get_pitch(); - filt_target_rate = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; - break; - case YAW: - // yaw angle will be centered on zero by removing trim heading - rotation_rate_filt.reset(0.0f); - command_filt.reset(motors->get_yaw()); - target_rate_filt.reset(0.0f); - rotation_rate = 0.0f; - command_out = motors->get_yaw(); - filt_target_rate = 0.0f; - break; - } - } + rotation_rate_filt.reset(0); + command_filt.reset(0); + target_rate_filt.reset(0); + rotation_rate = 0.0f; + command_out = 0.0f; + filt_target_rate = 0.0f; // filter at lower frequency to remove steady state filt_command_reading.set_cutoff_frequency(0.2f * start_frq); From 7e09f68f2600ef56a142f2400f314ed25271d6e4 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Wed, 9 Mar 2022 00:00:49 -0500 Subject: [PATCH 0429/3101] AP_Math: add chirp function to support frequency sweeps in systemid and autotune --- libraries/AP_Math/chirp.cpp | 87 +++++++++++++++++++++++++++++++++++++ libraries/AP_Math/chirp.h | 53 ++++++++++++++++++++++ 2 files changed, 140 insertions(+) create mode 100644 libraries/AP_Math/chirp.cpp create mode 100644 libraries/AP_Math/chirp.h diff --git a/libraries/AP_Math/chirp.cpp b/libraries/AP_Math/chirp.cpp new file mode 100644 index 00000000000..b01250bca6b --- /dev/null +++ b/libraries/AP_Math/chirp.cpp @@ -0,0 +1,87 @@ +/* + * chirp.cpp + * + * Copyright (C) Leonard Hall 2020 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +/* +* This object generates a chirp signal based on the input variables. A chirp is +* a sine wave starting from the minimum frequency and ending at the maximum frequency. +* The progression in frequency is not linear but designed to increase exponentially. +* The chirp can be designed to dwell at the minimum frequency for a specified time +* before sweeping through the frequencies and also can fade in the magnitude at the +* beginning and fade out the magnitude at the end. This object can also generate +* constant frequency sine waves by setting the minimum and maximum frequency to the +* same value. +*/ +#include +#include "chirp.h" + +// constructor +Chirp::Chirp() {} + +// initializes the chirp object +void Chirp::init(float time_record, float frequency_start_hz, float frequency_stop_hz, float time_fade_in, float time_fade_out, float time_const_freq) +{ + // pass in variables to class + record = time_record; + wMin = M_2PI * frequency_start_hz; + wMax = M_2PI * frequency_stop_hz; + fade_in = time_fade_in; + fade_out = time_fade_out; + const_freq = time_const_freq; + + B = logf(wMax / wMin); + +} + +// determine chirp signal output at the specified time and amplitude +float Chirp::update(float time, float waveform_magnitude) +{ + magnitude = waveform_magnitude; + if (time <= 0.0f) { + window = 0.0f; + } else if (time <= fade_in) { + window = 0.5 - 0.5 * cosf(M_PI * time / fade_in); + } else if (time <= record - fade_out) { + window = 1.0; + } else if (time <= record) { + window = 0.5 - 0.5 * cosf(M_PI * (time - (record - fade_out)) / fade_out + M_PI); + } else { + window = 0.0; + } + + if (time <= 0.0f) { + waveform_freq_rads = wMin; + output = 0.0f; + } else if (time <= const_freq) { + waveform_freq_rads = wMin; + output = window * magnitude * sinf(wMin * time - wMin * const_freq); + } else if (time <= record) { + // handles constant frequency dwells and chirps + if (is_equal(wMin, wMax)) { + waveform_freq_rads = wMin; + output = window * magnitude * sinf(wMin * time); + } else { + waveform_freq_rads = wMin * expf(B * (time - const_freq) / (record - const_freq)); + output = window * magnitude * sinf((wMin * (record - const_freq) / B) * (expf(B * (time - const_freq) / (record - const_freq)) - 1)); + } + } else { + waveform_freq_rads = wMax; + output = 0.0f; + } + return output; +} diff --git a/libraries/AP_Math/chirp.h b/libraries/AP_Math/chirp.h new file mode 100644 index 00000000000..23487e21e56 --- /dev/null +++ b/libraries/AP_Math/chirp.h @@ -0,0 +1,53 @@ +#pragma once + +class Chirp { + +public: + + // constructor + Chirp(); + + // initializes the chirp object + void init(float time_record, float frequency_start_hz, float frequency_stop_hz, float time_fade_in, float time_fade_out, float time_const_freq); + + // determine chirp signal output at the specified time and amplitude + float update(float time, float waveform_magnitude); + + // accessor for the current waveform frequency + float get_frequency_rads() {return waveform_freq_rads; } + +private: + // Total chirp length in seconds + float record; + + // Chirp oscillation amplitude + float magnitude; + + // Chirp start frequency in rad/s + float wMin; + + // Chirp end frequency in rad/s + float wMax; + + // Amplitude fade in time in seconds + float fade_in; + + // Amplitude fade out time in seconds + float fade_out; + + // Time that chirp will remain at the min frequency before increasing to max frequency + float const_freq; + + // frequency ratio + float B; + + // current waveform frequency in rad/s + float waveform_freq_rads; + + // current amplitude of chirp + float window; + + // output of chirp signal at the requested time + float output; + +}; From f186f612f9a7a6c084a329f671b12043bf844fc1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:17:34 +1100 Subject: [PATCH 0430/3101] AP_HAL_ChibiOS: enable Solo Gimbal only on CubeGreen --- libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat index a594650bedd..12804f9f7d6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat @@ -12,4 +12,6 @@ env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param' define HAL_OREO_LED_ENABLED 1 +define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED + AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat index b5bd97b349b..696118b716d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat @@ -52,4 +52,6 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES define HAL_OREO_LED_ENABLED 1 define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5 +define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED + AUTOBUILD_TARGETS Copter From f3bf6e56c8c7f7bd865c9d0288fbe81978effa70 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 Mar 2022 18:17:35 +1100 Subject: [PATCH 0431/3101] AP_Mount: enable Solo Gimbal only on CubeGreen --- libraries/AP_Mount/AP_Mount.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index aa617040d3c..da4cb5de80a 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -27,7 +27,7 @@ #endif #ifndef HAL_SOLO_GIMBAL_ENABLED -#define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED && BOARD_FLASH_SIZE > 1024 +#define HAL_SOLO_GIMBAL_ENABLED 0 #endif #if HAL_MOUNT_ENABLED From a30f33a6742fd72fc8f1dcaa1279fa7e0634aeb4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2022 12:29:45 +1100 Subject: [PATCH 0432/3101] hwdef: enable both OreoLED and Solo gimbal on CubeOrange for users who have replaced their CubeSolo with a CubeOrange --- libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index aedc8dd0990..a3ba4e1bab2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat @@ -333,3 +333,6 @@ ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin DMA_NOSHARE SPI1* SPI4* USART6* +# for users who have replaced their CubeSolo with a CubeOrange: +define HAL_OREO_LED_ENABLED 1 +define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED From 29c2c40c410a780182584f48b0e92c65554042e2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 23 Mar 2022 12:30:57 +1100 Subject: [PATCH 0433/3101] hwdef: enable both OreoLED and Solo gimbal on fmuv3 for users running fmuv3 on their Solo --- libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat index fec01b85833..183f6c4a976 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat @@ -468,3 +468,7 @@ DMA_PRIORITY USART6* SPI* # is "ROMFS ROMFS-filename source-filename". Paths are relative to the # ardupilot root. ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_lowpolh.bin + +# for users running fmuv3 on their Solo: +define HAL_OREO_LED_ENABLED (BOARD_FLASH_SIZE > 1024) +define HAL_SOLO_GIMBAL_ENABLED (HAL_MOUNT_ENABLED && BOARD_FLASH_SIZE > 1024) From 4513546845f350a6d0bb3a95524850687dc1a1b9 Mon Sep 17 00:00:00 2001 From: xianglunkai <1322099375@qq.com> Date: Mon, 28 Mar 2022 22:43:46 +0800 Subject: [PATCH 0434/3101] vector3:return w1 not zero vector --- libraries/AP_Math/vector3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index e13ab69a27f..2bbf57790b5 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -509,7 +509,7 @@ Vector3 Vector3::point_on_line_closest_to_other_point(const Vector3 &w1 const T line_vec_len = line_vec.length(); // protection against divide by zero if(::is_zero(line_vec_len)) { - return {0.0f, 0.0f, 0.0f}; + return w1; } const T scale = 1/line_vec_len; From abb0bf34d2417945b721a63f490f49f8d0e785d2 Mon Sep 17 00:00:00 2001 From: xianglunkai <1322099375@qq.com> Date: Mon, 28 Mar 2022 22:44:21 +0800 Subject: [PATCH 0435/3101] test_3d_lines:fix test result for test_3d_lines --- libraries/AP_Math/tests/test_3d_lines.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Math/tests/test_3d_lines.cpp b/libraries/AP_Math/tests/test_3d_lines.cpp index da8c1cfbcb5..0d41064f98d 100644 --- a/libraries/AP_Math/tests/test_3d_lines.cpp +++ b/libraries/AP_Math/tests/test_3d_lines.cpp @@ -26,7 +26,7 @@ TEST(Lines3dTests, ClosestDistBetweenLinePoint) // check protection agains null length const Vector3f intersection_null = Vector3f::point_on_line_closest_to_other_point(Vector3f{1.0f, 1.0f, 1.0f}, Vector3f{1.0f, 1.0f, 1.0f}, Vector3f{0.0f, 5.0f, 5.0f}); - EXPECT_VECTOR3F_EQ((Vector3f{0.0f, 0.0f, 0.0f}), intersection_null); + EXPECT_VECTOR3F_EQ((Vector3f{1.0f, 1.0f, 1.0f}), intersection_null); } TEST(Lines3dTests, SegmentToSegmentCloestPoint) From 4fa0e275fd1b255b6c37c9d511ab784bd02cae4b Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Mar 2022 08:09:20 +0000 Subject: [PATCH 0436/3101] AP_HAL_ChibiOS: fix ESCs constantly arming on rover with dshot commands make sure debug will compile take into account active channels when configuring bdshot add channel mask debug output correct set bdshot telemetry position at startup make sure all channels in a bdshot group are pulled high to prevent spurious pulses --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 26 +++++++++++++----- libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp | 29 ++++++++++++-------- libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp | 15 ++++++++-- 3 files changed, 49 insertions(+), 21 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index f80006dd5cf..3ad81dc9e2f 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -201,6 +201,16 @@ void RCOutput::rcout_thread() // process any pending RC output requests timer_tick(time_out_us); +#if RCOU_DSHOT_TIMING_DEBUG + static bool output_masks = true; + if (AP_HAL::millis() > 5000 && output_masks) { + output_masks = false; + hal.console->printf("bdmask 0x%x, en_mask 0x%lx, 3dmask 0x%x:\n", _bdshot.mask, en_mask, _reversible_mask); + for (auto &group : pwm_group_list) { + hal.console->printf(" timer %u: ch_mask 0x%x, en_mask 0x%x\n", group.timer_id, group.ch_mask, group.en_mask); + } + } +#endif } } @@ -1338,8 +1348,9 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) // assume that we won't be able to get the input capture lock group.bdshot.enabled = false; + uint16_t active_channels = group.ch_mask & group.en_mask; // now grab the input capture lock if we are able, we can only enable bi-dir on a group basis - if (((_bdshot.mask & group.ch_mask) == group.ch_mask) && group.has_ic()) { + if (((_bdshot.mask & active_channels) == active_channels) && group.has_ic()) { if (group.has_shared_ic_up_dma()) { // no locking required group.bdshot.enabled = true; @@ -1398,19 +1409,20 @@ void RCOutput::dshot_send(pwm_group &group, uint32_t time_out_us) #endif _bdshot.erpm[chan] = erpm; #endif - uint16_t pwm = period[chan]; - if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) { - // safety is on, overwride pwm - pwm = 0; + // safety is on, don't output anything + continue; } - const uint16_t chan_mask = (1U<is_locked(), "DMA handle is already locked"); @@ -48,9 +50,10 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha group.dshot_waiter = rcout_thread_ctx; bool bdshot_telem = false; #ifdef HAL_WITH_BIDIR_DSHOT + uint16_t active_channels = group.ch_mask & group.en_mask; // no need to get the input capture lock group.bdshot.enabled = false; - if ((_bdshot.mask & group.ch_mask) == group.ch_mask) { + if ((_bdshot.mask & active_channels) == active_channels) { bdshot_telem = true; // it's not clear why this is required, but without it we get no output if (group.pwm_started) { @@ -68,9 +71,13 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha const uint16_t packet = create_dshot_packet(command, true, bdshot_telem); for (uint8_t i = 0; i < 4; i++) { - if (group.chan[i] == chan || (chan == RCOutput::ALL_CHANNELS && group.is_chan_enabled(i))) { + if (!group.is_chan_enabled(i)) { + continue; + } + + if (group.chan[i] == chan || chan == RCOutput::ALL_CHANNELS) { fill_DMA_buffer_dshot(group.dma_buffer + i, 4, packet, group.bit_width_mul); - } else if (group.is_chan_enabled(i)) { + } else { fill_DMA_buffer_dshot(group.dma_buffer + i, 4, zero_packet, group.bit_width_mul); } } @@ -78,7 +85,9 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha chEvtGetAndClearEvents(group.dshot_event_mask); // start sending the pulses out send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH); +#ifdef HAL_GPIO_LINE_GPIO81 TOGGLE_PIN_DEBUG(81); +#endif return true; } From 6511a65b67ea1d961e788691404e954f8885b204 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Mar 2022 10:45:43 +0000 Subject: [PATCH 0437/3101] SRV_Channel: don't count disabled channels in mask and setup functions disable channels that are not in use --- libraries/SRV_Channel/SRV_Channel.h | 2 +- libraries/SRV_Channel/SRV_Channel_aux.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 2affe9eb197..a4e775dc2de 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -183,7 +183,7 @@ class SRV_Channel { // check if a function is valid for indexing into functions static bool valid_function(Aux_servo_function_t fn) { - return fn >= 0 && fn < k_nr_aux_servo_functions; + return fn > k_none && fn < k_nr_aux_servo_functions; } bool valid_function(void) const { return valid_function(function); diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 0c94f9f9ce8..8df4e1ee59e 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -208,6 +208,8 @@ void SRV_Channels::enable_aux_servos() // see if it is a valid function if (c.valid_function()) { hal.rcout->enable_ch(c.ch_num); + } else { + hal.rcout->disable_ch(c.ch_num); } // output some servo functions before we fiddle with the From 63229d7eca1311a94ade6bb358281a213715af2f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 25 Mar 2022 17:52:53 +0000 Subject: [PATCH 0438/3101] AP_HAL_SITL: correct disable channel maths unilaterally write rcoutput to appease the sitl gods --- libraries/AP_HAL_SITL/RCOutput.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index b1df8706a14..616d6a27c0a 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -35,7 +35,7 @@ void RCOutput::enable_ch(uint8_t ch) if (!(_enable_mask & (1U << ch))) { Debug("enable_ch(%u)\n", ch); } - _enable_mask |= 1U << ch; + _enable_mask |= (1U << ch); } void RCOutput::disable_ch(uint8_t ch) @@ -43,13 +43,14 @@ void RCOutput::disable_ch(uint8_t ch) if (_enable_mask & (1U << ch)) { Debug("disable_ch(%u)\n", ch); } - _enable_mask &= ~1U << ch; + _enable_mask &= ~(1U << ch); } void RCOutput::write(uint8_t ch, uint16_t period_us) { _sitlState->output_ready = true; - if (ch < SITL_NUM_CHANNELS && (_enable_mask & (1U<pwm_output[ch]; } return 0; From 913afe7b4e980b49e43d32c07381d39b536ee8c3 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 26 Mar 2022 17:15:21 -0500 Subject: [PATCH 0439/3101] Blimp: fix duplicate param and remove params/values not used --- Blimp/Parameters.cpp | 48 +++++++------------------------------------- Blimp/Parameters.h | 9 ++------- 2 files changed, 9 insertions(+), 48 deletions(-) diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 884e9771baa..5410f404acc 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -102,7 +102,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled. - // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land (4.0+ Only) + // @Values: 0:Disabled/NoAction,5:Land // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), @@ -116,7 +116,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land + // @Values: 0:Disabled,3:Enabled always Land // @User: Standard GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), @@ -141,7 +141,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate + // @Values: 0:LAND,1:MANUAL,2:VELOCITY,3:LOITER // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1), @@ -189,7 +189,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: INITIAL_MODE // @DisplayName: Initial flight mode - // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. + // @Description: This selects the mode to start in on boot. // @CopyValuesFrom: FLTMODE1 // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", (uint8_t)Mode::Number::MANUAL), @@ -201,21 +201,6 @@ const AP_Param::Info Blimp::var_info[] = { // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - // @Param: TUNE - // @DisplayName: Channel 6 Tuning - // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob - // @User: Standard - // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude - GSCALAR(radio_tuning, "TUNE", 0), - - // @Param: FRAME_TYPE - // @DisplayName: Frame Type (+, X, V, etc) - // @Description: Controls motor mixing for multiblimps. Not used for Tri or Traditional Heliblimps. - // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed - // @User: Standard - // @RebootRequired: True - GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT), - // @Group: ARMING_ // @Path: ../libraries/AP_Arming/AP_Arming.cpp GOBJECT(arming, "ARMING_", AP_Arming_Blimp), @@ -231,7 +216,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: FS_EKF_ACTION // @DisplayName: EKF Failsafe Action // @Description: Controls the action that will be taken when an EKF failsafe is invoked - // @Values: 1:Land, 2:AltHold, 3:Land even in Stabilize + // @Values: 1:Land, 3:Land even in MANUAL // @User: Advanced GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT), @@ -574,13 +559,6 @@ const AP_Param::Info Blimp::var_info[] = { // @Units: Hz // @User: Advanced - // @Param: VELYAW_FLTE - // @DisplayName: Velocity (yaw) input filter - // @Description: Velocity (yaw) input filter. This filter (in Hz) is applied to the input for D term - // @Range: 0 100 - // @Units: Hz - // @User: Advanced - // @Param: VELYAW_FF // @DisplayName: Velocity (yaw) feed forward gain // @Description: Velocity (yaw) feed forward gain. Converts the difference between desired velocity to a target acceleration @@ -822,18 +800,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), #endif - // @Param: TUNE_MIN - // @DisplayName: Tuning minimum - // @Description: Minimum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to - // @User: Standard - AP_GROUPINFO("TUNE_MIN", 31, ParametersG2, tuning_min, 0), - - // @Param: TUNE_MAX - // @DisplayName: Tuning maximum - // @Description: Maximum value that the parameter currently being tuned with the transmitter's channel 6 knob will be set to - // @User: Standard - AP_GROUPINFO("TUNE_MAX", 32, ParametersG2, tuning_max, 0), - // @Param: FS_VIBE_ENABLE // @DisplayName: Vibration Failsafe enable // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations @@ -844,8 +810,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FS_OPTIONS // @DisplayName: Failsafe options bitmask // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options. - // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe - // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe, 5:Release Gripper + // @Values: 0:Disabled, 16:Continue if in pilot controlled modes on GCS failsafe + // @Bitmask: 4:Continue if in pilot controlled modes on GCS failsafe // @User: Advanced AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Blimp::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL), diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index 70718b39e58..7db8cae8dbd 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -148,7 +148,7 @@ class Parameters // 140: Sensor parameters // k_param_compass, - k_param_frame_type, + k_param_frame_type, //unused k_param_ahrs, // AHRS group // 159 // @@ -170,7 +170,7 @@ class Parameters // k_param_failsafe_throttle = 170, k_param_failsafe_throttle_value, - k_param_radio_tuning, + k_param_radio_tuning, // unused k_param_rc_speed = 192, k_param_failsafe_gcs, k_param_rcmap, // 199 @@ -235,8 +235,6 @@ class Parameters // Misc // AP_Int32 log_bitmask; - AP_Int8 radio_tuning; - AP_Int8 frame_type; AP_Int8 disarm_delay; AP_Int8 fs_ekf_action; @@ -313,9 +311,6 @@ class ParametersG2 AP_Scripting scripting; #endif // AP_SCRIPTING_ENABLED - AP_Float tuning_min; - AP_Float tuning_max; - // vibration failsafe enable/disable AP_Int8 fs_vibe_enabled; From eb996f28cc4671f8291dd668cc03978f5403750f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 27 Mar 2022 19:07:19 +0100 Subject: [PATCH 0440/3101] AP_HAL_ChibiOS: spro H7 extreme updates. cannot currently use both IMUs on spro H7 extreme due to CPU load --- libraries/AP_HAL_ChibiOS/QSPIDevice.cpp | 1 - libraries/AP_HAL_ChibiOS/QSPIDevice.h | 1 + .../hwdef/SPRacingH7/defaults.parm | 3 +++ .../hwdef/SPRacingH7/hwdef-bl.dat | 5 +++- .../AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat | 23 ++++++++++++------- 5 files changed, 23 insertions(+), 10 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp index acfa563ebfa..4a7b7e68114 100644 --- a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp @@ -27,7 +27,6 @@ #include "Util.h" #include "Scheduler.h" #include -#include "hwdef/common/stm32_util.h" #if HAL_USE_WSPI == TRUE && defined(HAL_QSPI_DEVICE_LIST) diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.h b/libraries/AP_HAL_ChibiOS/QSPIDevice.h index 5c04845a57f..f6049a79dba 100644 --- a/libraries/AP_HAL_ChibiOS/QSPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.h @@ -29,6 +29,7 @@ #if !defined(HAL_BOOTLOADER_BUILD) #include "Semaphores.h" #endif +#include "hwdef/common/stm32_util.h" #include "Scheduler.h" #include "Device.h" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/defaults.parm index d521a5f59d2..3f24e7fb259 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/defaults.parm @@ -5,3 +5,6 @@ NTF_LED_TYPES 257 # setup SERIAL2 for RCIN SERIAL2_BAUD 115 SERIAL2_OPTIONS 8 + +# currently using both IMUs is too CPU intensive +INS_ENABLE_MASK 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat index 887fc131470..d1e7543e1a8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat @@ -35,7 +35,7 @@ PA12 OTG_FS_DP OTG1 PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD -PE3 LED_BOOTLOADER OUTPUT LOW # Blue LED +PE3 LED_BOOTLOADER OUTPUT LOW # Red LED define HAL_LED_ON 0 # QuadSPI Flash @@ -57,3 +57,6 @@ DEFAULTGPIO OUTPUT LOW PULLDOWN PB12 ICM20602_2_CS CS PA15 ICM20602_1_CS CS PE11 AT7456E_CS CS + +## pull up VTX power +PB01 VTX_PWR OUTPUT HIGH GPIO(81) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat index 4d654f00696..bad4197e5ea 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat @@ -68,28 +68,33 @@ PE14 SPI4_MOSI SPI4 PB8 I2C1_SCL I2C1 PULLUP PB9 I2C1_SDA I2C1 PULLUP +# Internal current sensor PC1 BATT_VOLTAGE_SENS ADC1 SCALE(1) PC0 BATT_CURRENT_SENS ADC1 SCALE(1) +# External current sensor +PC5 BATT_CURRENT_SENS2 ADC1 SCALE(1) # analog pin 8 + +# VTX Power control - should be high at startup to ensure power +PB01 VTX_PWR OUTPUT HIGH GPIO(81) +define RELAY2_PIN_DEFAULT 81 PC4 RSSI_IN ADC1 define BOARD_RSSI_ANA_PIN 0 # define default battery setup -define HAL_BATT_VOLT_PIN 13 -define HAL_BATT_CURR_PIN 12 +define HAL_BATT_VOLT_PIN 11 +define HAL_BATT_CURR_PIN 10 define HAL_BATT_VOLT_SCALE 10.9 define HAL_BATT_CURR_SCALE 28.5 define HAL_BATT_MONITOR_DEFAULT 4 -PC5 RSSI_ADC ADC1 - -# USART1 +# USART1 (RCIN) PB15 USART1_RX USART1 PB14 USART1_TX USART1 +define HAL_SERIAL1_PROTOCOL SerialProtocol_RCIN -# USART2 (RCIN) +# USART2 (SmartPort) PD5 USART2_TX USART2 -define HAL_SERIAL2_PROTOCOL SerialProtocol_RCIN # USART3 PD9 USART3_RX USART3 @@ -150,7 +155,8 @@ PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59) # M10 # NeoPixel LED strip PA8 TIM1_CH1 TIM1 PWM(11) GPIO(60) -PE3 LED0 OUTPUT LOW GPIO(90) # Blue LED + # Red LED +PE3 LED0 OUTPUT LOW GPIO(90) # spi devices SPIDEV icm20602_1 SPI3 DEVID1 ICM20602_1_CS MODE3 2*MHZ 10*MHZ @@ -162,6 +168,7 @@ DMA_PRIORITY SPI3* SPI2* TIM5* define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE define ALLOW_ARM_NO_COMPASS define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 # two IMUs IMU Invensense SPI:icm20602_1 ROTATION_YAW_90 From 2e524964328489b8ebe4068a4502009e12634673 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 27 Mar 2022 21:42:00 +0100 Subject: [PATCH 0441/3101] bootloaders: update spracing h7 extreme bootloader --- Tools/bootloaders/SPRacingH7_bl.bin | Bin 26472 -> 26464 bytes Tools/bootloaders/SPRacingH7_bl.hex | 2391 +++++++++++++-------------- 2 files changed, 1195 insertions(+), 1196 deletions(-) diff --git a/Tools/bootloaders/SPRacingH7_bl.bin b/Tools/bootloaders/SPRacingH7_bl.bin index 9e6d4c5f0c2dcb6205eb7e0fecdf362b2f8a1e40..07d603851818fdbbb560c4d732e61f48167e83a2 100644 GIT binary patch delta 2095 zcmYk7e^6A{702&+`xYL^!am8$E{yJ57IZfYVG)Qc6j^>gV`joc=8j2-Qgh0 z!k^VYvWOa-W?nEe397m7oh%Uwp))FQEzz4D+?^`+828J5C(k^b0L}6ccT^QO737uf zpjr`GZcLTB4}10ONX!O9_eDZT43S7WLWqD!(R&Z`#ni;!T@3qGN*dF|8k3H<62UQ| zQbxjRs#1Q#ieioMHfzH6B0ChD8#i&8jE!TI<7_0hAt`o=n6*!ATln>JiPb9QeU>KF z!8>w?@D?Pg7K!pRwhhy z$_(?ZUJHlW&DDmKW_CEJ}`eLZ+ex)$Ue553y;ro=8t})j>A0d{1Pm&CY z27M-4NZrqJ%DD(niRIhLJU6k;e|{d7k0amv32}^Qs7QI5N=0uEsz)N(UMqDy`(x_E z?82HsXkm3Jm9f{ZIDBNRkqxBS)_$@K=_4i{-^lvN^KP@N=-__W@nDwSDe;Pr&7|0@ z?iM2Owb~J$s$An8f|pm$E=yGF)1n?#BLM|7=9QOOf!?T|3E%fOvR(Rm*ugI7vvu3U zem{4n46D%^zU3zwxBbzT=k-RhElf<&ozNcvi5u4X8kxzEA~ru%kD#i3kY$a*%&oZC z&ke7vQU$|)PWkpPoN`r8N}UGXetgaCxQR(70Y2SZ6MvppzLZ00V;sCM`_?6MaDYup z4Vsyi=t{R%)=8k;WCf-wUDWFftpBI@6lK`|0jI~C08Ng6b`diPmfI;yq1D(5CU<+lv29jwfH zBf93t;b`j%Z(Dd|r^l@tng}NJOw93*ll2pGG0&*nO&zaQdE6UmE>*e7D^o$OsV9$) zSH<84du%ik7z^8}FzlOEdLx0+aIq`kC+peSbbIWJVGG7R%%;+FHNF2g=@1jmRj`@u zG9QE$Hf63eei@2pa4jU5d(%%~#mTFYz^frO)sIH?uUSb(HXirRjLYyfvu6GRuFC_N z{{S$s=QjKi7G#4Z58wq>XH7F43>DHDx4j}9*dMy>E~@yKUr!&U;Xo(rwVF3Ih1xo7 z6>TjR`i8qH?G2Y|Sn+A5H_{7f&>s$L4Z%Pm{mPw--t>X@oImR8ly6#3aF8IkWHE@# z$1^gJO$47D%Ep)1z(l(ds#vbwrpaCMbm+a>bYcnzY^=j>#ti%Hop4KDwuiX5cJ$nh zxMavL;=m*qKVHm1K0D+n=0cm;CC5fsX5TqV;h!vL^R(KHQxC|R!W@-pcRLY&f*(08 z+OCn;ht*V|k5CvQ%GDs84+Vx;b8$pHj1FJ0mM!Jkr?B@r@&}{~_rE|jG!$4``tV)+ zCgiIQtQytRF5gCKMfGoNa*Gb@>fh1;ud$L6D;#4TC3(8TxK`BvWwc~=-k5+Km%Sw> zRoV~18-AYt-ESSL3XD#r$&=Sghd<1L2E*? z5d)$|=20(28;!CSw@DmQiR2>1h!xQxfNW>YZAP&Ri#%>6QuG51==C1!Ys*m==Mdo? z_FkJ2?z5RTvktak&c&!9{YW{^)H6eSEwr;e?VrTeqGzm delta 2043 zcmX|?4NP0t702&+euihjV23zh81XZPfdd{Ul+}TS4+9SfWBFhRp^(Mlqccs@C26aK zEE%Adl}d{ibe6P;CMDCfhE^?cUs*~LhqT(dbQ=ZMMh)5`g&@_iMae@;W<#*OW17y= z@1A@A=iGCz@7KHYIV^k*r*ffrz?P%;wlHClFzjJMIdNcME^DAH4hGk(PTThrCR|c~ zLy$BWL=RX{gc`1^0ppRG5tU^;@)2V!iis?pU`!yyNc#eMqiUn?FNb_8B@MOFriA`hCOF1a%B7H6 zQYo)sakL5E$K6z3#-B&$Yo@NSiR+Z5AIGAb5;QB!qT9Cp;ju&UH7ey(Ocm;3RPGes zg#`67vz%LVy-BW5WJ}CR4o;ybZUp{yma3uN@0KADttZWV!|YL zCG0KeT0NM?amt5sL1HOq zONge<4LzY<@YP5v?*GGyv^UU7IGIZRw_V?voT zf$=zFcv@p4^`i_X22j5tE%9iWU&>~iq!=@&91e?;!#j=T8!UA%g<1B0W>|(O1-xdd zNOC{RDd)qy#IpZ2Q{ZMfOVbxf`CItDk1@xXR?HHl#@337}CegJnhJL!!NikuHvp2U9SXk?PVgbOyFEnj`g{H zOyKKuW4xqtUGEe;ymIcbDCxZGNsp*uzd|$Sl>sbFHmT=Bk9;lIoZJXM#?fS(p&{h+ zaYIg8jrP!8AIq5cMMhpqHt9P;%p5rh17Q%ki#l%$nvF^Ny-(B=r0UjYS!=X#Y#%;v zJQJ%9gnXQG?<%MKQBFvi1;bI=W;Y#TmdSum_teH*;FUQ!m^#714cWUfk%QwnBQ|O0 zS0f|cUEL^xavKZH)rLr~FKPX!>0d}xUm=@;1LmK0Z&#Smf6+uMsg>e~*iu=XB>g$S zdSHLA$K#CuV!#=9YwYn-2p(_Txn;b3|EWN3Pr^B)t9O6CYlEb-yQ&svo|j)RwsxY^ zdMh&SjZmb#659T7eZ#O@H8L59>zQ2S3t8smVpOBby}R>`>S6aL$ttPb45!SoDWYT> z5Bt5LY)QE2T~K<%{!5_}m*2-S@r`tQ^sD3u`;XyNdcL+hWdDCq7iull@H94Bx?lyT zEcK?_!N}0lLBZ0S-bc&LUJv_E2h~#Y_}YSCafS`9VrRw`SisE8pTVqrI`b=lG(3>? zHUwltb^$;i)>>1IM}kGtoZDU*@*fV)yNfIT?Ms$6Ng@A!JYls!GoH7m8Mg&tut>V& z&Znr#wWvj3B&J0E%G$?)PTrG?pmESeIBR3V$MUj`cBm1xb`w;g&7PypTN&<5_UO`? zIpnuuhus26_?rCy+>!nEAg6ItPZ!ZH2MdZhdQ`T2e=7%@@rZ+r1OMRI49mFaD2H2^ zvt?HO9EBd2HAQ(UjjNprKcJUvJ(h1xg151GYgqjuxo_c~l8TDoQ0*KsLL8(cEzs5n z{ehKFKcH*=l)0H0b$-1E8RkE=@o?#WR57HD^LFyU7zW_Q~(2 z$b>BxCg@Y>Lq>Rs^ElXQ$^4AeK5Bg%*_XkDlf)sSi#SW1$GO(LT751Pf;p6jY$lOU zq!Ak85$RQABU#LJAhbj+QAm^%Ho{1#iAHR0GwENX1-))%V&pvq>h&jltu0Sop2q|a zdfQC2`?uRH26YL|cPnYcX`+H6zk`PMI&kB@_P=WC^C{<3kv@eTbs);U9Z8_6d^6k~ lNy+f+GpW{p(!)|pjU7B=z9)5dCgK*pHH=*BIL Date: Mon, 28 Mar 2022 16:00:55 +1100 Subject: [PATCH 0442/3101] AP_Param: fixed param class conversion code param class conversion was unconditionally overwriting the parameter from the old parameter. This meant if the user has set a value in an old firmware they could not change it in a new firmware. I hit this with ARSPD_TYPE. I had previously set this to 0 in a previous use of the board, and found that it kept resetting to 0 on the new firmware when I tried to enable airspeed --- libraries/AP_Param/AP_Param.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index ab351c50c83..f1eecc4fdf6 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1951,6 +1951,10 @@ void AP_Param::convert_class(uint16_t param_key, void *object_pointer, } AP_Param *ap2 = (AP_Param *)(group_info[i].offset + (uint8_t *)object_pointer); + if (ap2->configured_in_storage()) { + // user has already set a value, or previous conversion was done + continue; + } memcpy(ap2, ap, sizeof(old_value)); // and save ap2->save(); From 479b02e5b090c7d117db92b06ddc2ef454027f44 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 28 Mar 2022 11:37:39 -0500 Subject: [PATCH 0443/3101] HWDEF: add SLCAN OTG port, correct defaults.param errors --- .../AP_HAL_ChibiOS/hwdef/QioTekZealotH743/defaults.parm | 8 ++++---- libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/defaults.parm index fb5477cba0b..65b893da7cb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/defaults.parm @@ -2,10 +2,10 @@ BRD_HEAT_TARG 45 # setup serial2 port defaults for ESP8266 -define HAL_SERIAL2_PROTOCOL SerialProtocol_MAVLink2 -define HAL_SERIAL2_BAUD 921600 +SERIAL2_BAUD 921600 # setup the parameter for the ADC power module +BATT_MONITOR 4 BATT_VOLT_PIN 16 BATT_CURR_PIN 17 BATT_VOLT_MULT 20.000 @@ -16,5 +16,5 @@ BATT2_VOLT_MULT 20.000 BATT2_AMP_PERVLT 17.000 # setup the parameter for the two Relays GPIO others for reserve -define RELAY1_PIN_DEFAULT 1 -define RELAY2_PIN_DEFAULT 2 \ No newline at end of file +RELAY_PIN 1 +RELAY_PIN2 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat index bed152a23be..d2470c5a1c4 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat @@ -20,7 +20,7 @@ STM32_ST_USE_TIMER 2 FLASH_RESERVE_START_KB 128 # order of UARTs (and USB) -SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART7 +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART7 OTG2 # now we define the pins that USB is connected on PA11 OTG_FS_DM OTG1 From d9f819085c53bb3c163d5604c5a5ca4e991d0309 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 22 Mar 2022 17:24:47 +1100 Subject: [PATCH 0444/3101] AP_Math: add specialisation for sq(float) avoids conversion to double --- libraries/AP_Math/AP_Math.h | 4 ++++ libraries/AP_Math/tests/test_math.cpp | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 5b71dea1f96..fa51036ae6f 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -207,6 +207,10 @@ ftype sq(const T val) ftype v = static_cast(val); return v*v; } +static inline constexpr float sq(const float val) +{ + return val*val; +} /* * Variadic template for calculating the square norm of a vector of any diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index 775f21b7ae7..e5a04112683 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -277,6 +277,12 @@ TEST(MathTest, Square) AP_Float t_sqfloat; t_sqfloat = sq(2); EXPECT_EQ(4.f, t_sqfloat); + + EXPECT_FLOAT_EQ(sq(2.3), 5.289999999999999); // uses template sq + EXPECT_FLOAT_EQ(sq(2.3f), 5.29); // uses sq(float v) + EXPECT_EQ(sq(4294967295), 18446744065119617025U); // uses template sq + EXPECT_FLOAT_EQ(sq(4294967295.0), 1.8446744e+19); // uses template sq + EXPECT_FLOAT_EQ(sq(pow(2,25)), pow(2,50)); } TEST(MathTest, Norm) From 910406e82da77f105fc9a9f569d1b3c1753f0322 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 24 Mar 2022 08:30:07 -0600 Subject: [PATCH 0445/3101] Plane: handle AIRBRAKE RC option --- ArduPlane/RC_Channel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index ec183484c72..fe790e681f1 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -268,6 +268,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::FLAP: case AUX_FUNC::FBWA_TAILDRAGGER: + case AUX_FUNC::AIRBRAKE: break; // input labels, nothing to do #if HAL_QUADPLANE_ENABLED From fff82bed755d7230d4b9210c751c74c2472e3594 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 8 Mar 2022 18:03:41 +0000 Subject: [PATCH 0446/3101] AP_HAL: update prescaler tests --- libraries/AP_HAL/tests/test_prescaler.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL/tests/test_prescaler.cpp b/libraries/AP_HAL/tests/test_prescaler.cpp index 1afaa9f1bff..6e9bfb60707 100644 --- a/libraries/AP_HAL/tests/test_prescaler.cpp +++ b/libraries/AP_HAL/tests/test_prescaler.cpp @@ -11,24 +11,24 @@ class PrescalerParameterizedTestFixture :public ::testing::TestWithParam Date: Tue, 8 Mar 2022 18:03:55 +0000 Subject: [PATCH 0447/3101] AP_HAL_ChibiOS: use narrower bitwidths for dshot and LEDs to allow more accurate prescaler calculation --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 41 +++++++++++++++++++-------- 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 3ad81dc9e2f..5d328f957c2 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -63,6 +63,18 @@ static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13); static const eventmask_t EVT_PWM_SEND_NEXT = EVENT_MASK(14); static const eventmask_t EVT_LED_SEND = EVENT_MASK(15); +static const uint32_t DSHOT_BIT_WIDTH_TICKS = 8; +static const uint32_t DSHOT_BIT_0_TICKS = 3; +static const uint32_t DSHOT_BIT_1_TICKS = 6; + +// See WS2812B spec for expected pulse widths +static const uint32_t NEOP_BIT_WIDTH_TICKS = 11; +static const uint32_t NEOP_BIT_0_TICKS = 4; +static const uint32_t NEOP_BIT_1_TICKS = 9; +// neopixel does not use pulse widths at all +static const uint32_t PROFI_BIT_0_TICKS = 4; +static const uint32_t PROFI_BIT_1_TICKS = 9; + // #pragma GCC optimize("Og") /* @@ -892,7 +904,6 @@ void RCOutput::set_group_mode(pwm_group &group) } const uint32_t rate = protocol_bitrate(group.current_mode); - const uint32_t bit_period = 20; // configure timer driver for DMAR at requested rate const uint8_t pad_end_bits = 8; @@ -903,7 +914,7 @@ void RCOutput::set_group_mode(pwm_group &group) // calculate min time between pulses taking into account the DMAR parallelism const uint32_t pulse_time_us = 1000000UL * bit_length / rate; - if (!setup_group_DMA(group, rate, bit_period, active_high, buffer_length, pulse_time_us, false)) { + if (!setup_group_DMA(group, rate, NEOP_BIT_WIDTH_TICKS, active_high, buffer_length, pulse_time_us, false)) { group.current_mode = MODE_PWM_NONE; break; } @@ -912,13 +923,12 @@ void RCOutput::set_group_mode(pwm_group &group) case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: { const uint32_t rate = protocol_bitrate(group.current_mode); - const uint32_t bit_period = 20; bool active_high = is_bidir_dshot_enabled() ? false : true; // calculate min time between pulses const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / rate; // configure timer driver for DMAR at requested rate - if (!setup_group_DMA(group, rate, bit_period, active_high, + if (!setup_group_DMA(group, rate, DSHOT_BIT_WIDTH_TICKS, active_high, MAX(DSHOT_BUFFER_LENGTH, GCR_TELEMETRY_BUFFER_LEN), pulse_send_time_us, true)) { group.current_mode = MODE_PWM_NORMAL; break; @@ -1303,8 +1313,8 @@ uint16_t RCOutput::create_dshot_packet(const uint16_t value, bool telem_request, */ void RCOutput::fill_DMA_buffer_dshot(uint32_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul) { - const uint32_t DSHOT_MOTOR_BIT_0 = 7 * clockmul; - const uint32_t DSHOT_MOTOR_BIT_1 = 14 * clockmul; + const uint32_t DSHOT_MOTOR_BIT_0 = DSHOT_BIT_0_TICKS * clockmul; + const uint32_t DSHOT_MOTOR_BIT_1 = DSHOT_BIT_1_TICKS * clockmul; uint16_t i = 0; for (; i < dshot_pre; i++) { buffer[i * stride] = 0; @@ -2193,8 +2203,8 @@ void RCOutput::_set_neopixel_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, const uint8_t stride = 4; uint32_t *buf = grp->dma_buffer + (led * neopixel_bit_length + pad_start_bits) * stride + idx; uint32_t bits = (green<<16) | (red<<8) | blue; - const uint32_t BIT_0 = 7 * grp->bit_width_mul; - const uint32_t BIT_1 = 14 * grp->bit_width_mul; + const uint32_t BIT_0 = NEOP_BIT_0_TICKS * grp->bit_width_mul; + const uint32_t BIT_1 = NEOP_BIT_1_TICKS * grp->bit_width_mul; for (uint16_t b=0; b < 24; b++) { buf[b * stride] = (bits & 0x800000) ? BIT_1 : BIT_0; bits <<= 1; @@ -2212,7 +2222,7 @@ void RCOutput::_set_profiled_rgb_data(pwm_group *grp, uint8_t idx, uint8_t led, const uint8_t stride = 4; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; uint32_t bits = 0x1000000 | (blue<<16) | (red<<8) | green; - const uint32_t BIT_1 = 14 * grp->bit_width_mul; + const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul; for (uint16_t b=0; b < bit_length; b++) { buf[b * stride] = (bits & 0x1000000) ? 0 : BIT_1; bits <<= 1; @@ -2229,7 +2239,7 @@ void RCOutput::_set_profiled_blank_frame(pwm_group *grp, uint8_t idx, uint8_t le const uint8_t bit_length = 25; const uint8_t stride = 4; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; - const uint32_t BIT_1 = 14 * grp->bit_width_mul; + const uint32_t BIT_1 = PROFI_BIT_1_TICKS * grp->bit_width_mul; for (uint16_t b=0; b < bit_length; b++) { buf[b * stride] = BIT_1; } @@ -2244,7 +2254,7 @@ void RCOutput::_set_profiled_clock(pwm_group *grp, uint8_t idx, uint8_t led) const uint8_t bit_length = 25; const uint8_t stride = 4; uint32_t *buf = grp->dma_buffer + (led * bit_length + pad_start_bits) * stride + idx; - const uint32_t BIT_1 = 7 * grp->bit_width_mul; + const uint32_t BIT_1 = PROFI_BIT_0_TICKS * grp->bit_width_mul; for (uint16_t b=0; b < bit_length; b++) { buf[b * stride] = BIT_1; } @@ -2382,7 +2392,14 @@ void RCOutput::timer_info(ExpandingString &str) str.printf("TIMERV1\n"); for (auto &group : pwm_group_list) { - const uint32_t target_freq = &group == serial_group ? 19200 * 10 : protocol_bitrate(group.current_mode) * 20; + uint32_t target_freq; + if (&group == serial_group) { + target_freq = 19200 * 10; + } else if (is_dshot_protocol(group.current_mode)) { + target_freq = protocol_bitrate(group.current_mode) * DSHOT_BIT_WIDTH_TICKS; + } else { + target_freq = protocol_bitrate(group.current_mode) * NEOP_BIT_WIDTH_TICKS; + } const uint32_t prescaler = calculate_bitrate_prescaler(group.pwm_drv->clock, target_freq, is_dshot_protocol(group.current_mode)); str.printf("TIM%-2u CLK=%4uMhz MODE=%5s FREQ=%8u TGT=%8u\n", group.timer_id, unsigned(group.pwm_drv->clock / 1000000), get_output_mode_string(group.current_mode), From 8fcefb59b1dd4ad0630227f529bda40d274de09c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Apr 2021 16:43:30 +0900 Subject: [PATCH 0448/3101] AP_Math: add Vector2f::dot --- libraries/AP_Math/vector2.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 35a61d56870..152da159895 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -89,6 +89,11 @@ struct Vector2 // dot product T operator *(const Vector2 &v) const; + // dot product (same as above but a more easily understood name) + T dot(const Vector2 &v) const { + return *this * v; + } + // cross product T operator %(const Vector2 &v) const; From abc7bd446ac4481b3d9de88334063f171cb8c9aa Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 Apr 2021 20:53:38 +0900 Subject: [PATCH 0449/3101] AR_PosControl: rover position controller --- libraries/APM_Control/AR_PosControl.cpp | 389 ++++++++++++++++++++++++ libraries/APM_Control/AR_PosControl.h | 116 +++++++ 2 files changed, 505 insertions(+) create mode 100644 libraries/APM_Control/AR_PosControl.cpp create mode 100644 libraries/APM_Control/AR_PosControl.h diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp new file mode 100644 index 00000000000..ac868d47776 --- /dev/null +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -0,0 +1,389 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AR_PosControl.h" + +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define AR_POSCON_TIMEOUT_MS 100 // timeout after 0.1 sec +#define AR_POSCON_POS_P 0.2f // default position P gain +#define AR_POSCON_VEL_P 1.0f // default velocity P gain +#define AR_POSCON_VEL_I 0.2f // default velocity I gain +#define AR_POSCON_VEL_D 0.0f // default velocity D gain +#define AR_POSCON_VEL_FF 0.0f // default velocity FF gain +#define AR_POSCON_VEL_IMAX 1.0f // default velocity IMAX +#define AR_POSCON_VEL_FILT 5.0f // default velocity filter +#define AR_POSCON_VEL_FILT_D 5.0f // default velocity D term filter +#define AR_POSCON_DT 0.02f // default dt for PID controllers + +const AP_Param::GroupInfo AR_PosControl::var_info[] = { + + // @Param: _POS_P + // @DisplayName: Position controller P gain + // @Description: Position controller P gain. Converts the distance to the target location into a desired speed which is then passed to the loiter latitude rate controller + // @Range: 0.500 2.000 + // @User: Standard + AP_SUBGROUPINFO(_p_pos, "_POS_", 1, AR_PosControl, AC_P_2D), + + // @Param: _VEL_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration + // @Range: 0.1 6.0 + // @Increment: 0.1 + // @User: Advanced + + // @Param: _VEL_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration + // @Range: 0.02 1.00 + // @Increment: 0.01 + // @User: Advanced + + // @Param: _VEL_D + // @DisplayName: Velocity (horizontal) D gain + // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity + // @Range: 0.00 1.00 + // @Increment: 0.001 + // @User: Advanced + + // @Param: _VEL_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output + // @Range: 0 4500 + // @Increment: 10 + // @Units: cm/s/s + // @User: Advanced + + // @Param: _VEL_FLTE + // @DisplayName: Velocity (horizontal) input filter + // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + + // @Param: _VEL_FLTD + // @DisplayName: Velocity (horizontal) input filter + // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term + // @Range: 0 100 + // @Units: Hz + // @User: Advanced + + // @Param: _VEL_FF + // @DisplayName: Velocity (horizontal) feed forward gain + // @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration + // @Range: 0 6 + // @Increment: 0.01 + // @User: Advanced + AP_SUBGROUPINFO(_pid_vel, "_VEL_", 2, AR_PosControl, AC_PID_2D), + + AP_GROUPEND +}; + +AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) : + _atc(atc), + _p_pos(AR_POSCON_POS_P, AR_POSCON_DT), + _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D, AR_POSCON_DT) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// update navigation +void AR_PosControl::update(float dt) +{ + // exit immediately if no current location, destination or disarmed + Vector2f curr_pos_NE; + Vector3f curr_vel_NED; + if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || + !AP::ahrs().get_velocity_NED(curr_vel_NED)) { + _desired_speed = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; + _desired_turn_rate_rads = 0.0f; + return; + } + + // check for ekf xy position reset + handle_ekf_xy_reset(); + + // if no recent calls reset velocity controller + if (!is_active()) { + _pid_vel.reset_I(); + _pid_vel.reset_filter(); + } + _last_update_ms = AP_HAL::millis(); + + // update P, PID object's dt + _p_pos.set_dt(dt); + _pid_vel.set_dt(dt); + + // calculate position error and convert to desired velocity + Vector2f des_vel_NE; + if (_pos_target_valid) { + Vector2p pos_target = _pos_target; + des_vel_NE = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); + } + + // calculation velocity error + if (_vel_target_valid) { + // add target velocity to desired velocity from position error + des_vel_NE += _vel_target; + } + + // limit velocity to maximum speed + des_vel_NE.limit_length(get_speed_max()); + + // Limit the velocity to prevent fence violations + bool backing_up = false; + AC_Avoid *avoid = AP::ac_avoid(); + if (avoid != nullptr) { + Vector3f vel_3d_cms{des_vel_NE.x * 100.0f, des_vel_NE.y * 100.0f, 0.0f}; + const float accel_max_cmss = MIN(_accel_max, _lat_accel_max) * 100.0; + avoid->adjust_velocity(vel_3d_cms, backing_up, _p_pos.kP(), accel_max_cmss, _p_pos.kP(), accel_max_cmss, dt); + des_vel_NE.x = vel_3d_cms.x * 0.01; + des_vel_NE.y = vel_3d_cms.y * 0.01; + } + + // calculate desired acceleration + // To-Do: fixup _limit_vel used below + Vector2f des_accel_NE = _pid_vel.update_all(des_vel_NE, curr_vel_NED.xy(), _limit_vel); + if (_accel_target_valid) { + des_accel_NE += _accel_target; + } + + // convert desired acceleration to desired forward-back speed, desired lateral speed and desired turn rate + + // rotate acceleration into body frame using current heading + const Vector2f des_accel_FR = AP::ahrs().earth_to_body2D(des_accel_NE); + + // calculate minimum turn speed which is the max speed the vehicle could turn through the corner + // given the vehicle's turn radius and half its max lateral acceleration + // todo: remove MAX of zero when safe_sqrt fixed + float turn_speed_min = MAX(safe_sqrt(_atc.get_turn_lat_accel_max() * 0.5 * _turn_radius), 0); + + // rotate target velocity from earth-frame to body frame + const Vector2f des_vel_FR = AP::ahrs().earth_to_body2D(des_vel_NE); + + // desired speed is normally the forward component (only) of the target velocity + // but we do not let it fall below the minimum turn speed unless the vehicle is slowing down + const float abs_des_speed_min = MIN(des_vel_NE.length(), turn_speed_min); + float des_speed; + if (_reversed != backing_up) { + // if reversed or backing up desired speed will be negative + des_speed = MIN(-abs_des_speed_min, des_vel_FR.x); + } else { + des_speed = MAX(abs_des_speed_min, des_vel_FR.x); + } + _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); + + // calculate turn rate from desired lateral acceleration + _desired_lat_accel = des_accel_FR.y; + _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); +} + +// true if update has been called recently +bool AR_PosControl::is_active() const +{ + return ((AP_HAL::millis() - _last_update_ms) < AR_POSCON_TIMEOUT_MS); +} + +// set limits +void AR_PosControl::set_limits(float speed_max, float accel_max, float lat_accel_max, float jerk_max) +{ + _speed_max = MAX(speed_max, 0); + _accel_max = MAX(accel_max, 0); + _lat_accel_max = MAX(lat_accel_max, 0); + _jerk_max = MAX(jerk_max, 0); + + // set position P controller limits + _p_pos.set_limits(_speed_max, MIN(_accel_max, _lat_accel_max), _jerk_max); +} + +// setter to allow vehicle code to provide turn related param values to this library (should be updated regularly) +void AR_PosControl::set_turn_params(float turn_radius, bool pivot_possible) +{ + if (pivot_possible) { + _turn_radius = 0; + } else { + _turn_radius = turn_radius; + } +} + +// initialise the position controller to the current position, velocity, acceleration and attitude +// this should be called before the input shaping methods are used +bool AR_PosControl::init() +{ + // get current position and velocity from AHRS + Vector2f pos_NE; + Vector3f vel_NED; + if (!AP::ahrs().get_relative_position_NE_origin(pos_NE) || !AP::ahrs().get_velocity_NED(vel_NED)) { + return false; + } + + // set target position to current position + _pos_target.x = pos_NE.x; + _pos_target.y = pos_NE.y; + + // set target velocity and acceleration + _vel_target = vel_NED.xy(); + _accel_target = AP::ahrs().get_accel_ef_blended().xy(); + + // clear reversed setting + _reversed = false; + + // initialise ekf xy reset handler + init_ekf_xy_reset(); + + return true; +} + +// methods to adjust position, velocity and acceleration targets smoothly using input shaping +// pos should be the target position as an offset from the EKF origin (in meters) +// dt should be the update rate in seconds +void AR_PosControl::input_pos_target(const Vector2p &pos, float dt) +{ + // adjust target position, velocity and acceleration forward by dt + update_pos_vel_accel_xy(_pos_target, _vel_target, _accel_target, dt, Vector2f(), Vector2f(), Vector2f()); + + // call shape_pos_vel_accel_xy to pull target towards final destination + Vector2f vel; + Vector2f accel; + const float accel_max = MIN(_accel_max, _lat_accel_max); + shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_target, _accel_target, + _speed_max, accel_max, _jerk_max, dt, false); + + // set flags so update will consume target position, velocity and acceleration + _pos_target_valid = true; + _vel_target_valid = true; + _accel_target_valid = true; +} + +// set position, velocity and acceleration targets +void AR_PosControl::set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel) +{ + _pos_target = pos; + _vel_target = vel; + _accel_target = accel; + _pos_target_valid = true; + _vel_target_valid = true; + _accel_target_valid = true; +} + +// returns desired velocity vector (i.e. feed forward) in cm/s in lat and lon direction +Vector2f AR_PosControl::get_desired_velocity() const +{ + if (_vel_target_valid) { + return _vel_target; + } + return Vector2f(); +} + +// return desired acceleration vector in m/s in lat and lon direction +Vector2f AR_PosControl::get_desired_accel() const +{ + if (_accel_target_valid) { + return _accel_target; + } + return Vector2f(); +} + +/// get position error as a vector from the current position to the target position +Vector2p AR_PosControl::get_pos_error() const +{ + // return zero error is not active or no position estimate + Vector2f curr_pos_NE; + if (!is_active() ||!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + return Vector2p{}; + } + + // get current position + return (_pos_target - curr_pos_NE.topostype()); +} + +// write PSC logs +void AR_PosControl::write_log() +{ + // exit immediately if not active + if (!is_active()) { + return; + } + + // exit immediately if no position or velocity estimate + Vector3f curr_pos_NED; + Vector3f curr_vel_NED; + if (!AP::ahrs().get_relative_position_NED_origin(curr_pos_NED) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { + return; + } + + // get acceleration + const Vector3f curr_accel_NED;// = AP::ahrs().get_accel_ef_blended; + + // convert position, velocity and accel targets to required format + Vector2f pos_target_2d_cm = get_pos_target().tofloat() * 100.0; + Vector2f vel_target_2d_cm = get_desired_velocity() * 100.0; + Vector2f accel_target_2d_cm = get_desired_accel() * 100.0; + + AP::logger().Write_PSCN(pos_target_2d_cm.x, // position target + curr_pos_NED.x * 100.0, // position + 0.0, // desired velocity + vel_target_2d_cm.x, // target velocity + curr_vel_NED.x * 100.0, // velocity + 0.0, // desired accel + accel_target_2d_cm.x, // target accel + curr_accel_NED.x); // accel + AP::logger().Write_PSCE(pos_target_2d_cm.y, // position target + curr_pos_NED.y * 100.0, // position + 0.0, // desired velocity + vel_target_2d_cm.y, // target velocity + curr_vel_NED.y * 100.0, // velocity + 0.0, // desired accel + accel_target_2d_cm.y, // target accel + curr_accel_NED.y); // accel +} + +/// initialise ekf xy position reset check +void AR_PosControl::init_ekf_xy_reset() +{ + Vector2f pos_shift; + _ekf_xy_reset_ms = AP::ahrs().getLastPosNorthEastReset(pos_shift); +} + +/// handle_ekf_xy_reset - check for ekf position reset and adjust loiter or brake target position +void AR_PosControl::handle_ekf_xy_reset() +{ + // check for position shift + Vector2f pos_shift; + uint32_t reset_ms = AP::ahrs().getLastPosNorthEastReset(pos_shift); + if (reset_ms != _ekf_xy_reset_ms) { + Vector2f pos_NE; + if (!AP::ahrs().get_relative_position_NE_origin(pos_NE)) { + return; + } + _pos_target = (pos_NE + _p_pos.get_error()).topostype(); + + Vector3f vel_NED; + if (!AP::ahrs().get_velocity_NED(vel_NED)) { + return; + } + _vel_target = vel_NED.xy() + _pid_vel.get_error(); + + _ekf_xy_reset_ms = reset_ms; + } +} diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h new file mode 100644 index 00000000000..ed0d65853fe --- /dev/null +++ b/libraries/APM_Control/AR_PosControl.h @@ -0,0 +1,116 @@ +#pragma once + +#include +#include +#include // P library (2-axis) +#include // PID library (2-axis) + +class AR_PosControl { +public: + + // constructor + AR_PosControl(AR_AttitudeControl& atc); + + // update navigation + void update(float dt); + + // true if update has been called recently + bool is_active() const; + + // set speed, acceleration and jerk limits + void set_limits(float speed_max, float accel_max, float lat_accel_max, float jerk_max); + + // setter to allow vehicle code to provide turn related param values to this library (should be updated regularly) + void set_turn_params(float turn_radius, bool pivot_possible); + + // set reversed + void set_reversed(bool reversed) { _reversed = reversed; } + + // get limits + float get_speed_max() const { return _speed_max; } + float get_accel_max() const { return _accel_max; } + float get_lat_accel_max() const { return _lat_accel_max; } + float get_jerk_max() const { return _jerk_max; } + + // initialise the position controller to the current position, velocity, acceleration and attitude + // this should be called before the input shaping methods are used + // return true on success, false if targets cannot be initialised + bool init(); + + // adjust position, velocity and acceleration targets smoothly using input shaping + // pos should be the target position as an offset from the EKF origin (in meters) + // dt should be the update rate in seconds + // init should be called once before starting to use these methods + void input_pos_target(const Vector2p &pos, float dt); + + // set position, velocity and acceleration targets. These should be from an externally created path and are not "input shaped" + void set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel); + + // get outputs for forward-back speed (in m/s), lateral speed (in m/s) and turn rate (in rad/sec) + float get_desired_speed() const { return _desired_speed; } + float get_desired_turn_rate_rads() const { return _desired_turn_rate_rads; } + float get_desired_lat_accel() const { return _desired_lat_accel; } + + // get position target + const Vector2p& get_pos_target() const { return _pos_target; } + + // returns desired velocity vector (i.e. feed forward) in m/s in lat and lon direction + Vector2f get_desired_velocity() const; + + // return desired acceleration vector in m/s in lat and lon direction + Vector2f get_desired_accel() const; + + /// get position error as a vector from the current position to the target position + Vector2p get_pos_error() const; + + // get pid controllers + AC_P_2D& get_pos_p() { return _p_pos; } + AC_PID_2D& get_vel_pid() { return _pid_vel; } + + // write PSC logs + void write_log(); + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // initialise and check for ekf position resets + void init_ekf_xy_reset(); + void handle_ekf_xy_reset(); + + // references + AR_AttitudeControl &_atc; // rover attitude control library + + // parameters + AC_P_2D _p_pos; // position P controller to convert distance error to desired velocity + AC_PID_2D _pid_vel; // velocity PID controller to convert velocity error to desired acceleration + + // limits + float _speed_max; // maximum forward speed in m/s + float _accel_max; // maximum forward/back acceleration in m/s/s + float _lat_accel_max; // lateral acceleration maximum in m/s/s + float _jerk_max; // maximum jerk in m/s/s/s (used for both forward and lateral input shaping) + float _turn_radius; // vehicle turn radius in meters + Vector2f _limit_vel; // To-Do: explain what this is + + // position and velocity targets + Vector2p _pos_target; // position target as an offset (in meters) from the EKF origin + Vector2f _vel_target; // velocity target in m/s in NE frame + Vector2f _accel_target; // accel target in m/s/s in NE frame + bool _pos_target_valid; // true if _pos_target is valid + bool _vel_target_valid; // true if _vel_target is valid + bool _accel_target_valid; // true if _accel_target is valid + + // variables for navigation + uint32_t _last_update_ms; // system time of last call to update + bool _reversed; // true if vehicle should move in reverse towards target + + // main outputs + float _desired_speed; // desired forward_back speed in m/s + float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise) + float _desired_lat_accel; // desired lateral acceleration (for reporting only) + + // ekf reset handling + uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset +}; From 9d629f5ecdc59a5b0351231e4922479ba270575e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 19 Apr 2021 20:53:57 +0900 Subject: [PATCH 0450/3101] AR_WPNav: use position controller and s-curves --- libraries/AR_WPNav/AR_WPNav.cpp | 329 ++++++++++++++++++++------------ libraries/AR_WPNav/AR_WPNav.h | 48 +++-- 2 files changed, 237 insertions(+), 140 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 1e550ca023d..5795eafc3fc 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -16,13 +16,17 @@ #include #include #include "AR_WPNav.h" +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_RADIUS_DEFAULT 2.0f -#define AR_WPNAV_OVERSHOOT_DEFAULT 2.0f #define AR_WPNAV_PIVOT_ANGLE_DEFAULT 60 #define AR_WPNAV_PIVOT_ANGLE_ACCURACY 5 // vehicle will pivot to within this many degrees of destination #define AR_WPNAV_PIVOT_RATE_DEFAULT 90 @@ -47,14 +51,7 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { // @User: Standard AP_GROUPINFO("RADIUS", 2, AR_WPNav, _radius, AR_WPNAV_RADIUS_DEFAULT), - // @Param: OVERSHOOT - // @DisplayName: Waypoint overshoot maximum - // @Description: Waypoint overshoot maximum in meters. The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next. - // @Units: m - // @Range: 0 10 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("OVERSHOOT", 3, AR_WPNav, _overshoot, AR_WPNAV_OVERSHOOT_DEFAULT), + // 3 was OVERSHOOT // @Param: PIVOT_ANGLE // @DisplayName: Waypoint Pivot Angle @@ -95,13 +92,56 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { AP_GROUPEND }; -AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AP_Navigation& nav_controller) : +AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : _atc(atc), - _nav_controller(nav_controller) + _pos_control(pos_control) { AP_Param::setup_object_defaults(this, var_info); } +// initialise waypoint controller +// speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed +// accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration +// lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration +// jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration +void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float jerk_max) +{ + // default max speed and accel + if (!is_positive(speed_max)) { + speed_max = _speed_max; + } + if (!is_positive(accel_max)) { + accel_max = _atc.get_accel_max(); + } + if (!is_positive(lat_accel_max)) { + lat_accel_max = _atc.get_turn_lat_accel_max(); + } + if (!is_positive(jerk_max)) { + jerk_max = _atc.get_accel_max(); + } + _scurve_jerk = jerk_max; + + // initialise position controller + _pos_control.set_limits(speed_max, accel_max, lat_accel_max); + + _scurve_prev_leg.init(); + _scurve_this_leg.init(); + _scurve_next_leg.init(); + _track_scalar_dt = 1.0f; + + // init some flags + _reached_destination = false; + _fast_waypoint = false; + + // initialise origin and destination to stopping point + Location stopping_loc; + if (get_stopping_location(stopping_loc)) { + _origin = _destination = stopping_loc; + } else { + // handle not current location + } +} + // update navigation void AR_WPNav::update(float dt) { @@ -165,12 +205,7 @@ void AR_WPNav::update(float dt) } } - // check if vehicle has reached the destination - const bool near_wp = _distance_to_destination <= _radius; - const bool past_wp = !_oa_active && current_loc.past_interval_finish_line(_origin, _destination); - if (!_reached_destination && (near_wp || past_wp)) { - _reached_destination = true; - } + advance_wp_target_along_track(current_loc, dt); // handle stopping vehicle if avoidance has failed if (stop_vehicle) { @@ -181,57 +216,85 @@ void AR_WPNav::update(float dt) return; } - // calculate the required turn of the wheels - update_steering(current_loc, speed); - - // calculate desired speed - update_desired_speed(dt); + // update_steering_and_speed + update_steering_and_speed(current_loc, dt); } -// set desired location -bool AR_WPNav::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) +// set desired location and (optionally) next_destination +// next_destination should be provided if known to allow smooth cornering +bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination) { - // set origin to last destination if waypoint controller active - if (is_active() && _orig_and_dest_valid && _reached_destination) { - _origin = _destination; - } else { - // otherwise use reasonable stopping point - if (!get_stopping_location(_origin)) { - return false; - } + // re-initialise if previous destination has been interrupted + if (!is_active() || !_reached_destination) { + init(0,0,0,0); } + // shift this leg to previous leg + _scurve_prev_leg = _scurve_this_leg; + // initialise some variables - _oa_origin = _origin; + _oa_origin = _origin = _destination; _oa_destination = _destination = destination; _orig_and_dest_valid = true; _reached_destination = false; - update_distance_and_bearing_to_destination(); // determine if we should pivot immediately + update_distance_and_bearing_to_destination(); update_pivot_active_flag(); - // set final desired speed and whether vehicle should pivot - _desired_speed_final = 0.0f; - if (!is_equal(next_leg_bearing_cd, AR_WPNAV_HEADING_UNKNOWN)) { - const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination); - const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd); - if (fabsf(turn_angle_cd) < 10.0f) { - // if turning less than 0.1 degrees vehicle can continue at full speed - // we use 0.1 degrees instead of zero to avoid divide by zero in calcs below - _desired_speed_final = _desired_speed; - } else if (use_pivot_steering_at_next_WP(turn_angle_cd)) { - // pivoting so we will stop - _desired_speed_final = 0.0f; - } else { - // calculate maximum speed that keeps overshoot within bounds - const float radius_m = fabsf(_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f)); - _desired_speed_final = MIN(_desired_speed, safe_sqrt(_atc.get_turn_lat_accel_max() * radius_m)); - // ensure speed does not fall below minimum - apply_speed_min(_desired_speed_final); + // convert origin and destination to offset from EKF origin + Vector2f origin_NE; + Vector2f destination_NE; + if (!_origin.get_vector_xy_from_origin_NE(origin_NE) || + !_destination.get_vector_xy_from_origin_NE(destination_NE)) { + return false; + } + origin_NE *= 0.01f; + destination_NE *= 0.01f; + + // calculate track to destination + if (_fast_waypoint && !_scurve_next_leg.finished()) { + // skip recalculating this leg by simply shifting next leg + _scurve_this_leg = _scurve_next_leg; + } else { + _scurve_this_leg.calculate_track(Vector3f{origin_NE.x, origin_NE.y, 0.0f}, // origin + Vector3f{destination_NE.x, destination_NE.y, 0.0f}, // destination + _pos_control.get_speed_max(), + _pos_control.get_speed_max(), // speed up (not used) + _pos_control.get_speed_max(), // speed down (not used) + MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), + _pos_control.get_accel_max(), // vertical accel (not used) + 1.0, // jerk time + _scurve_jerk); + } + + // handle next destination + if (next_destination.initialised()) { + // convert next_destination to offset from EKF origin + Vector2f next_destination_NE; + if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) { + return false; } + next_destination_NE *= 0.01f; + _scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f}, + Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f}, + _pos_control.get_speed_max(), + _pos_control.get_speed_max(), // speed up (not used) + _pos_control.get_speed_max(), // speed down (not used) + _pos_control.get_accel_max(), + _pos_control.get_accel_max(), // vertical accel (not used) + 1.0, // jerk time + _scurve_jerk); + + // next destination provided so fast waypoint + _fast_waypoint = true; + } else { + _scurve_next_leg.init(); + _fast_waypoint = false; } + update_distance_and_bearing_to_destination(); + return true; } @@ -246,7 +309,7 @@ bool AR_WPNav::set_desired_location_to_stopping_location() } // set desired location as offset from the EKF origin, return true on success -bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd) +bool AR_WPNav::set_desired_location_NED(const Vector3f& destination) { // initialise destination to ekf origin Location destination_ned; @@ -256,7 +319,22 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_ // apply offset destination_ned.offset(destination.x, destination.y); - return set_desired_location(destination_ned, next_leg_bearing_cd); + return set_desired_location(destination_ned); +} + +bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) +{ + // initialise destination to ekf origin + Location dest_loc, next_dest_loc; + if (!AP::ahrs().get_origin(dest_loc)) { + return false; + } + next_dest_loc = dest_loc; + + // apply offsets + dest_loc.offset(destination.x, destination.y); + next_dest_loc.offset(next_destination.x, next_destination.y); + return set_desired_location(dest_loc, next_dest_loc); } // calculate vehicle stopping point using current location, velocity and maximum acceleration @@ -345,6 +423,67 @@ bool AR_WPNav::is_active() const return ((AP_HAL::millis() - _last_update_ms) < AR_WPNAV_TIMEOUT_MS); } +// move target location along track from origin to destination +void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float dt) +{ + // exit immediately if no current location, destination or disarmed + Vector2f curr_pos_NE; + Vector3f curr_vel_NED; + if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { + return; + } + + // exit immediately if we can't convert waypoint origin to offset from ekf origin + Vector2f origin_NE; + if (!_origin.get_vector_xy_from_origin_NE(origin_NE)) { + return; + } + // convert from cm to meters + origin_NE *= 0.01f; + + // use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of vehicle + Vector2f curr_target_vel = _pos_control.get_desired_velocity(); + float track_scaler_dt = 1.0f; + if (is_positive(curr_target_vel.length())) { + Vector2f track_direction = curr_target_vel.normalized(); + const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction); + float track_velocity = curr_vel_NED.xy().dot(track_direction); + // set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation. + track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); + } + // change s-curve time speed with a time constant of maximum acceleration / maximum jerk + float track_scaler_tc = 1.0f; + if (is_positive(_scurve_jerk)) { + track_scaler_tc = _pos_control.get_accel_max() / _scurve_jerk; + } + _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); + + // target position, velocity and acceleration from straight line or spline calculators + Vector3f target_pos_3d_ftype{origin_NE.x, origin_NE.y, 0.0f}; + Vector3f target_vel, target_accel; + + // update target position, velocity and acceleration + const float wp_radius = MAX(_radius, _turn_radius); + bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); + + // pass new target to the position controller + Vector2p target_pos_ptype{target_pos_3d_ftype.x, target_pos_3d_ftype.y}; + _pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy()); + + // check if we've reached the waypoint + if (!_reached_destination && s_finished) { + // "fast" waypoints are complete once the intermediate point reaches the destination + if (_fast_waypoint) { + _reached_destination = true; + } else { + // regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line" + const bool near_wp = current_loc.get_distance(_destination) <= _radius; + const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination); + _reached_destination = near_wp || past_wp; + } + } +} + // update distance from vehicle's current position to destination void AR_WPNav::update_distance_and_bearing_to_destination() { @@ -372,90 +511,36 @@ void AR_WPNav::update_distance_and_bearing_to_destination() } } -// calculate steering output to drive along line from origin to destination waypoint -// relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated -void AR_WPNav::update_steering(const Location& current_loc, float current_speed) +// calculate steering and speed to drive along line from origin to destination waypoint +void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { - // calculate desired turn rate and update desired heading + // handle pivot turns if (_pivot_active) { _cross_track_error = calc_crosstrack_error(current_loc); _desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;; _desired_lat_accel = 0.0f; _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate)); - // update flag so that it can be cleared - update_pivot_active_flag(); - } else { - // run L1 controller - _nav_controller.set_reverse(_reversed); - _nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius); - - // retrieve lateral acceleration, heading back towards line and crosstrack error - _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_atc.get_turn_lat_accel_max(), _atc.get_turn_lat_accel_max()); - _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd()); - if (_reversed) { - _desired_lat_accel *= -1.0f; - _desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000); - } - _cross_track_error = _nav_controller.crosstrack_error(); - _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed); - } -} - -// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint -// relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members -// have been updated: _oa_wp_bearing_cd, _cross_track_error, _oa_distance_to_destination -void AR_WPNav::update_desired_speed(float dt) -{ - // reduce speed to zero during pivot turns - if (_pivot_active) { // decelerate to zero _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); - return; - } - - // accelerate desired speed towards max - float des_speed_lim = _atc.get_desired_speed_accel_limited(_reversed ? -_desired_speed : _desired_speed, dt); - - // reduce speed to limit overshoot from line between origin and destination - // calculate number of degrees vehicle must turn to face waypoint - float ahrs_yaw_sensor = AP::ahrs().yaw_sensor; - const float heading_cd = is_negative(des_speed_lim) ? wrap_180_cd(ahrs_yaw_sensor + 18000) : ahrs_yaw_sensor; - const float wp_yaw_diff_cd = wrap_180_cd(_oa_wp_bearing_cd - heading_cd); - const float turn_angle_rad = fabsf(radians(wp_yaw_diff_cd * 0.01f)); - - // calculate distance from vehicle to line + wp_overshoot - const float line_yaw_diff = wrap_180_cd(_oa_origin.get_bearing_to(_oa_destination) - heading_cd); - const float dist_from_line = fabsf(_cross_track_error); - const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error); - const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line; - - // calculate radius of circle that touches vehicle's current position and heading and target position and heading - float radius_m = 999.0f; - const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad)); - if (!is_zero(radius_calc_denom)) { - radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom; - } - - // calculate and limit speed to allow vehicle to stay on circle - // ensure limit does not force speed below minimum - float overshoot_speed_max = safe_sqrt(_atc.get_turn_lat_accel_max() * MAX(_turn_radius, radius_m)); - apply_speed_min(overshoot_speed_max); - des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max); - // limit speed based on distance to waypoint and max acceleration/deceleration - if (is_positive(_oa_distance_to_destination) && is_positive(_atc.get_decel_max())) { - const float dist_speed_max = safe_sqrt(2.0f * _oa_distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final)); - des_speed_lim = constrain_float(des_speed_lim, -dist_speed_max, dist_speed_max); + // update flag so that it can be cleared + update_pivot_active_flag(); + return; } - _desired_speed_limited = des_speed_lim; + _pos_control.set_reversed(_reversed); + _pos_control.update(dt); + _desired_speed_limited = _pos_control.get_desired_speed(); + _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); + _desired_lat_accel = _pos_control.get_desired_lat_accel(); + _cross_track_error = _pos_control.get_crosstrack_error(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible) { - _turn_radius = turn_radius; + _turn_radius = pivot_possible ? 0.0 : turn_radius; _pivot_possible = pivot_possible; } @@ -493,5 +578,5 @@ float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc); // calculate distance to target track, for reporting - return veh_from_origin % dest_from_origin; + return fabsf(veh_from_origin % dest_from_origin); } diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 4e64fadc55d..a872033130e 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -1,8 +1,9 @@ #pragma once #include +#include #include -#include +#include #include const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown @@ -11,7 +12,14 @@ class AR_WPNav { public: // constructor - AR_WPNav(AR_AttitudeControl& atc, AP_Navigation& nav_controller); + AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control); + + // initialise waypoint controller + // speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed + // accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration + // lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration + // jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration + void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max); // update navigation void update(float dt); @@ -36,15 +44,16 @@ class AR_WPNav { // get desired lateral acceleration (for reporting purposes only because will be zero during pivot turns) float get_lat_accel() const { return _desired_lat_accel; } - // set desired location - // next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn) - bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED; + // set desired location and (optionally) next_destination + // next_destination should be provided if known to allow smooth cornering + bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; // set desired location to a reasonable stopping point, return true on success bool set_desired_location_to_stopping_location() WARN_IF_UNUSED; // set desired location as offset from the EKF origin, return true on success - bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED; + bool set_desired_location_NED(const Vector3f& destination) WARN_IF_UNUSED; + bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED; // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck bool reached_destination() const { return _reached_destination; } @@ -89,17 +98,14 @@ class AR_WPNav { // true if update has been called recently bool is_active() const; + // move target location along track from origin to destination + void advance_wp_target_along_track(const Location ¤t_loc, float dt); + // update distance and bearing from vehicle's current position to destination void update_distance_and_bearing_to_destination(); - // calculate steering output to drive along line from origin to destination waypoint - // relies on update_distance_and_bearing_to_destination being called first - void update_steering(const Location& current_loc, float current_speed); - - // calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint - // relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members - // have been updated: _wp_bearing_cd, _cross_track_error, _distance_to_destination - void update_desired_speed(float dt); + // calculate steering and speed to drive along line from origin to destination waypoint + void update_steering_and_speed(const Location ¤t_loc, float dt); // returns true if vehicle should pivot turn at next waypoint bool use_pivot_steering_at_next_WP(float yaw_error_cd) const; @@ -120,14 +126,21 @@ class AR_WPNav { AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached - AP_Float _overshoot; // maximum horizontal overshoot in meters AP_Int16 _pivot_angle; // angle error that leads to pivot turn AP_Int16 _pivot_rate; // desired turn rate during pivot turns in deg/sec AP_Float _pivot_delay; // waiting time after pivot turn // references AR_AttitudeControl& _atc; // rover attitude control library - AP_Navigation& _nav_controller; // navigation controller (aka L1 controller) + AR_PosControl &_pos_control; // rover position control library + + // scurve + SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory + SCurve _scurve_this_leg; // current scurve trajectory + SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory + float _scurve_jerk; // scurve jerk max in m/s/s/s + bool _fast_waypoint; // true if vehicle will stop at the next waypoint + float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle // variables held in vehicle code (for now) float _turn_radius; // vehicle turn radius in meters @@ -141,11 +154,10 @@ class AR_WPNav { Location _destination; // destination Location when in Guided_WP bool _orig_and_dest_valid; // true if the origin and destination have been set bool _reversed; // execute the mission by backing up - float _desired_speed_final; // desired speed in m/s when we reach the destination // main outputs from navigation library float _desired_speed; // desired speed in m/s - float _desired_speed_limited; // desired speed (above) but accel/decel limited and reduced to keep vehicle within _overshoot of line + float _desired_speed_limited; // desired speed (above) but accel/decel limited float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise) float _desired_lat_accel; // desired lateral acceleration (for reporting only) float _desired_heading_cd; // desired heading (back towards line between origin and destination) From 95c69811cbc24574962bc5006cdaba866efcefbb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 19 Nov 2021 12:38:12 +0900 Subject: [PATCH 0451/3101] Rover: integrate position controller --- Rover/Parameters.cpp | 15 +++++-------- Rover/Parameters.h | 5 ++++- Rover/Rover.cpp | 6 +++++ Rover/Rover.h | 4 +--- Rover/mode.cpp | 4 ++-- Rover/mode.h | 8 +++---- Rover/mode_auto.cpp | 48 +++++++++++++++++++++++----------------- Rover/mode_guided.cpp | 5 ++--- Rover/mode_smart_rtl.cpp | 29 +++++++++++++++++------- 9 files changed, 74 insertions(+), 50 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 96dad7a978b..d4a4f61b64f 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -282,10 +282,6 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp GOBJECT(serial_manager, "SERIAL", AP_SerialManager), - // @Group: NAVL1_ - // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp - GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), - // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), @@ -660,6 +656,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(ais, "AIS_", 50, ParametersG2, AP_AIS), #endif + // @Group: PSC + // @Path: ../libraries/APM_Control/AR_PosControl.cpp + AP_SUBGROUPINFO(pos_control, "PSC", 51, ParametersG2, AR_PosControl), + AP_GROUPEND }; @@ -709,7 +709,8 @@ ParametersG2::ParametersG2(void) avoid(), follow(), windvane(), - wp_nav(attitude_control, rover.L1_controller), + pos_control(attitude_control), + wp_nav(attitude_control, pos_control), sailboat() { AP_Param::setup_object_defaults(this, var_info); @@ -744,7 +745,6 @@ const AP_Param::ConversionInfo conversion_table[] = { { Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" }, { Parameters::k_param_pivot_turn_angle_old, 0, AP_PARAM_INT16, "WP_PIVOT_ANGLE" }, { Parameters::k_param_waypoint_radius_old, 0, AP_PARAM_FLOAT, "WP_RADIUS" }, - { Parameters::k_param_waypoint_overshoot_old, 0, AP_PARAM_FLOAT, "WP_OVERSHOOT" }, { Parameters::k_param_g2, 20, AP_PARAM_INT16, "WP_PIVOT_RATE" }, { Parameters::k_param_g2, 32, AP_PARAM_FLOAT, "SAIL_ANGLE_MIN" }, @@ -792,9 +792,6 @@ void Rover::load_parameters(void) SRV_Channels::upgrade_parameters(); hal.console->printf("load_all took %uus\n", unsigned(micros() - before)); - // set a more reasonable default NAVL1_PERIOD for rovers - L1_controller.set_default_period(NAVL1_PERIOD); - // convert CH7_OPTION to RC7_OPTION for Rover-3.4 to 3.5 upgrade const AP_Param::ConversionInfo ch7_option_info = { Parameters::k_param_ch7_option, 0, AP_PARAM_INT8, "RC7_OPTION" }; AP_Int8 ch7_opt_old; diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 653dc5a5d06..6f3ade2e177 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -206,7 +206,7 @@ class Parameters { k_param_ins, k_param_compass, k_param_rcmap, - k_param_L1_controller, + k_param_L1_controller, // unused k_param_steerController_old, // unused k_param_barometer, k_param_notify, @@ -411,6 +411,9 @@ class ParametersG2 { // Automatic Identification System - for tracking sea-going vehicles AP_AIS ais; #endif + + // position controller + AR_PosControl pos_control; }; extern const AP_Param::Info var_info[]; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index b4f552083f2..afec3bc1a04 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -371,6 +371,11 @@ void Rover::update_logging1(void) if (should_log(MASK_LOG_NTUN)) { Log_Write_Nav_Tuning(); + if (g2.pos_control.is_active()) { + g2.pos_control.write_log(); + logger.Write_PID(LOG_PIDN_MSG, g2.pos_control.get_vel_pid().get_pid_info_x()); + logger.Write_PID(LOG_PIDE_MSG, g2.pos_control.get_vel_pid().get_pid_info_y()); + } } #if HAL_PROXIMITY_ENABLED @@ -433,6 +438,7 @@ void Rover::one_second_loop(void) // send latest param values to wp_nav g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering()); + g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering()); } void Rover::update_current_mode(void) diff --git a/Rover/Rover.h b/Rover/Rover.h index 2cab6b2651d..c919ad332a5 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -34,7 +34,6 @@ #include // ArduPilot Mega Magnetometer Library #include // Compass declination library #include // Inertial Sensor (uncalibated IMU) Library -#include #include // ArduPilot Mega Vector/Matrix math Library #include // Mission command library #include // Camera/Antenna mount @@ -52,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -156,8 +156,6 @@ class Rover : public AP_Vehicle { // Arming/Disarming management class AP_Arming_Rover arming; - AP_L1_Control L1_controller{ahrs, nullptr}; - #if AP_OPTICALFLOW_ENABLED OpticalFlow optflow; #endif diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 415bbb2c288..00a8d894c56 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -246,9 +246,9 @@ float Mode::get_desired_lat_accel() const } // set desired location -bool Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) +bool Mode::set_desired_location(const Location &destination, Location next_destination ) { - if (!g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) { + if (!g2.wp_nav.set_desired_location(destination, next_destination)) { return false; } diff --git a/Rover/mode.h b/Rover/mode.h index 67bea9e8dbb..c44b15bf283 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -112,8 +112,8 @@ class Mode virtual bool get_desired_location(Location& destination) const WARN_IF_UNUSED { return false; } // set desired location (used in Guided, Auto) - // next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn) - virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED; + // set next_destination (if known). If not provided vehicle stops at destination + virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck virtual bool reached_destination() const { return true; } @@ -262,7 +262,7 @@ class ModeAuto : public Mode // get or set desired location bool get_desired_location(Location& destination) const override WARN_IF_UNUSED; - bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED; + bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED; bool reached_destination() const override; // set desired speed in m/s @@ -413,7 +413,7 @@ class ModeGuided : public Mode // get or set desired location bool get_desired_location(Location& destination) const override WARN_IF_UNUSED; - bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED; + bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED; // set desired heading and speed void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed); diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 3aeb363c6cd..9afbb234b45 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -61,17 +61,18 @@ void ModeAuto::update() switch (_submode) { case Auto_WP: { - if (!g2.wp_nav.reached_destination()) { + // check if we've reached the destination + if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { // update navigation controller navigate_to_waypoint(); } else { // we have reached the destination so stay here if (rover.is_boat()) { if (!start_loiter()) { - stop_vehicle(); + start_stop(); } } else { - stop_vehicle(); + start_stop(); } // update distance to destination _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination()); @@ -187,10 +188,10 @@ bool ModeAuto::get_desired_location(Location& destination) const } // set desired location to drive to -bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) +bool ModeAuto::set_desired_location(const Location &destination, Location next_destination) { // call parent - if (!Mode::set_desired_location(destination, next_leg_bearing_cd)) { + if (!Mode::set_desired_location(destination, next_destination)) { return false; } @@ -602,29 +603,36 @@ void ModeAuto::do_RTL(void) bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination) { - // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot) - // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point - float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN; - if (!always_stop_at_destination && loiter_duration == 0) { - next_leg_bearing_cd = mission.get_next_ground_course_cd(AR_WPNAV_HEADING_UNKNOWN); - } - // retrieve and sanitize target location Location cmdloc = cmd.content.location; cmdloc.sanitize(rover.current_loc); - if (!set_desired_location(cmdloc, next_leg_bearing_cd)) { - return false; + + // delayed stored in p1 in seconds + loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1; + loiter_start_time = 0; + if (loiter_duration > 0) { + always_stop_at_destination = true; + } + + // do not add next wp if there are no more navigation commands + AP_Mission::Mission_Command next_cmd; + if (always_stop_at_destination || !mission.get_next_nav_cmd(cmd.index+1, next_cmd)) { + // single destination + if (!set_desired_location(cmdloc)) { + return false; + } + } else { + // retrieve and sanitize next destination location + Location next_cmdloc = next_cmd.content.location; + next_cmdloc.sanitize(cmdloc); + if (!set_desired_location(cmdloc, next_cmdloc)) { + return false; + } } // just starting so we haven't previously reached the waypoint previously_reached_wp = false; - // this will be used to remember the time in millis after we reach or pass the WP. - loiter_start_time = 0; - - // this is the delay, stored in seconds, checked such that commanded delays < 0 delay 0 seconds - loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1; - return true; } diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 83c45599e9f..524d0f9d596 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -232,10 +232,9 @@ bool ModeGuided::get_desired_location(Location& destination) const } // set desired location -bool ModeGuided::set_desired_location(const struct Location& destination, - float next_leg_bearing_cd) +bool ModeGuided::set_desired_location(const Location &destination, Location next_destination) { - if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) { + if (g2.wp_nav.set_desired_location(destination, next_destination)) { // handle guided specific initialisation and logging _guided_mode = ModeGuided::Guided_WP; diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp index a28984e452b..f1e76697ac1 100644 --- a/Rover/mode_smart_rtl.cpp +++ b/Rover/mode_smart_rtl.cpp @@ -49,20 +49,33 @@ void ModeSmartRTL::update() case SmartRTL_PathFollow: // load point if required if (_load_point) { - Vector3f next_point; - if (!g2.smart_rtl.pop_point(next_point)) { + Vector3f dest_NED; + if (!g2.smart_rtl.pop_point(dest_NED)) { // if not more points, we have reached home gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); smart_rtl_state = SmartRTL_StopAtHome; break; + } else { + // peek at the next point. this can fail if the IO task currently has the path semaphore + Vector3f next_dest_NED; + if (g2.smart_rtl.peek_point(next_dest_NED)) { + if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) { + // this should never happen because the EKF origin should already be set + gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); + smart_rtl_state = SmartRTL_Failure; + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + } else { + // no next point so add only immediate point + if (!g2.wp_nav.set_desired_location_NED(dest_NED)) { + // this should never happen because the EKF origin should already be set + gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); + smart_rtl_state = SmartRTL_Failure; + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + } } _load_point = false; - // set target destination to new point - if (!g2.wp_nav.set_desired_location_NED(next_point)) { - // this failure should never happen but we add it just in case - gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); - smart_rtl_state = SmartRTL_Failure; - } } // update navigation controller navigate_to_waypoint(); From 315c5fb7d74becf54cbaf333e659ca648a4a4b68 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Nov 2021 13:02:20 +0900 Subject: [PATCH 0452/3101] Rover: GCS_PID_MASK supports Velocity North and East --- Rover/GCS_Mavlink.cpp | 34 ++++++++++++++++++++++++++++++++++ Rover/Parameters.cpp | 4 ++-- 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 35790deb80e..ff2a1e6fc44 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -288,6 +288,40 @@ void GCS_MAVLINK_Rover::send_pid_tuning() return; } } + + // Position Controller Velocity North PID + if (g.gcs_pid_mask & 64) { + pid_info = &g2.pos_control.get_vel_pid().get_pid_info_x(); + mavlink_msg_pid_tuning_send(chan, 10, + pid_info->target, + pid_info->actual, + pid_info->FF, + pid_info->P, + pid_info->I, + pid_info->D, + pid_info->slew_rate, + pid_info->Dmod); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } + + // Position Controller Velocity East PID + if (g.gcs_pid_mask & 128) { + pid_info = &g2.pos_control.get_vel_pid().get_pid_info_y(); + mavlink_msg_pid_tuning_send(chan, 11, + pid_info->target, + pid_info->actual, + pid_info->FF, + pid_info->P, + pid_info->I, + pid_info->D, + pid_info->slew_rate, + pid_info->Dmod); + if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { + return; + } + } } void Rover::send_wheel_encoder_distance(const mavlink_channel_t chan) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index d4a4f61b64f..b67981597ee 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -63,8 +63,8 @@ const AP_Param::Info Rover::var_info[] = { // @DisplayName: GCS PID tuning mask // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for // @User: Advanced - // @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel,32:Sailboat Heel - // @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel,5:Sailboat Heel + // @Values: 0:None,1:Steering,2:Throttle,4:Pitch,8:Left Wheel,16:Right Wheel,32:Sailboat Heel,64:Velocity North,128:Velocity East + // @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel,5:Sailboat Heel,6:Velocity North,7:Velocity East GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), // @Param: AUTO_TRIGGER_PIN From 4c6bccc8d5031fce6ff003443487cfde649109d9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Nov 2021 19:27:35 +0900 Subject: [PATCH 0453/3101] Rover: incorporate mission change detector --- Rover/mode.h | 4 ++++ Rover/mode_auto.cpp | 16 ++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/Rover/mode.h b/Rover/mode.h index c44b15bf283..94a45cfdb92 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include "defines.h" @@ -383,6 +384,9 @@ class ModeAuto : public Mode float arg2; // 2nd argument provided by mission command } nav_scripting; #endif + + // Mission change detector + AP_Mission_ChangeDetector mis_change_detector; }; diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 9afbb234b45..67ee4828084 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -53,8 +53,24 @@ void ModeAuto::update() // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); waiting_to_start = false; + + // initialise mission change check + IGNORE_RETURN(mis_change_detector.check_for_mission_change()); } } else { + // check for mission changes + if (mis_change_detector.check_for_mission_change()) { + // if mission is running restart the current command if it is a waypoint command + if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::Auto_WP)) { + if (mission.restart_current_nav_cmd()) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); + } else { + // failed to restart mission for some reason + gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command"); + } + } + } + mission.update(); } From 7225d747771443921a29e909352f47bf1ea3c94e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Nov 2021 15:55:21 +0900 Subject: [PATCH 0454/3101] AR_PivotTurn: pivot turn controller also reduces default pivot speed to 60deg/sec (was 90deg/sec) add would_active and allow activation to be forced --- libraries/AR_WPNav/AR_PivotTurn.cpp | 151 ++++++++++++++++++++++++++++ libraries/AR_WPNav/AR_PivotTurn.h | 70 +++++++++++++ 2 files changed, 221 insertions(+) create mode 100644 libraries/AR_WPNav/AR_PivotTurn.cpp create mode 100644 libraries/AR_WPNav/AR_PivotTurn.h diff --git a/libraries/AR_WPNav/AR_PivotTurn.cpp b/libraries/AR_WPNav/AR_PivotTurn.cpp new file mode 100644 index 00000000000..5c1965b63ca --- /dev/null +++ b/libraries/AR_WPNav/AR_PivotTurn.cpp @@ -0,0 +1,151 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include +#include "AR_PivotTurn.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + +extern const AP_HAL::HAL& hal; + +#define AR_PIVOT_TIMEOUT_MS 100 // pivot controller timesout and reset target if not called within this many milliseconds +#define AR_PIVOT_ANGLE_DEFAULT 60 // default PIVOT_ANGLE parameter value +#define AR_PIVOT_ANGLE_ACCURACY 5 // vehicle will pivot to within this many degrees of destination +#define AR_PIVOT_RATE_DEFAULT 60 // default PIVOT_RATE parameter value +#define AR_PIVOT_DELAY_DEFAULT 0 // default PIVOT_DELAY parameter value + +const AP_Param::GroupInfo AR_PivotTurn::var_info[] = { + + // @Param: ANGLE + // @DisplayName: Pivot Angle + // @Description: Pivot when the difference between the vehicle's heading and its target heading is more than this many degrees. Set to zero to disable pivot turns. This parameter should be greater than 5 degrees for pivot turns to work. + // @Units: deg + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ANGLE", 1, AR_PivotTurn, _angle, AR_PIVOT_ANGLE_DEFAULT), + + // @Param: RATE + // @DisplayName: Pivot Turn Rate + // @Description: Turn rate during pivot turns + // @Units: deg/s + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("RATE", 2, AR_PivotTurn, _rate_max, AR_PIVOT_RATE_DEFAULT), + + // @Param: DELAY + // @DisplayName: Pivot Delay + // @Description: Vehicle waits this many seconds after completing a pivot turn before proceeding + // @Units: s + // @Range: 0 60 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("DELAY", 3, AR_PivotTurn, _delay, AR_PIVOT_DELAY_DEFAULT), + + AP_GROUPEND +}; + +AR_PivotTurn::AR_PivotTurn(AR_AttitudeControl& atc) : + _atc(atc) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// enable or disable pivot controller +void AR_PivotTurn::enable(bool enable_pivot) +{ + _enabled = enable_pivot; + if (!_enabled) { + _active = false; + } +} + +// true if update has been called recently +bool AR_PivotTurn::active() const +{ + return _enabled && _active; +} + +// checks if pivot turns should be activated or deactivated +// force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error +void AR_PivotTurn::check_activation(float desired_heading_deg, bool force_active) +{ + // check cases where we clearly cannot use pivot steering + if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) { + _active = false; + return; + } + + // calc yaw error in degrees + const float yaw_error = fabsf(wrap_180(desired_heading_deg - (AP::ahrs().yaw_sensor * 0.01f))); + + // if error is larger than _pivot_angle start pivot steering + if (yaw_error > _angle || force_active) { + _active = true; + _delay_start_ms = 0; + return; + } + + uint32_t now_ms = AP_HAL::millis(); + + // if within 5 degrees of the target heading, set start time of pivot steering + if (_active && (yaw_error < AR_PIVOT_ANGLE_ACCURACY) && (_delay_start_ms == 0)) { + _delay_start_ms = now_ms; + } + + // exit pivot steering after the time set by pivot_delay has elapsed + if ((_delay_start_ms > 0) && + (now_ms - _delay_start_ms) >= get_delay_duration_ms()) { + _active = false; + _delay_start_ms = 0; + } +} + +// check if pivot turn would be activated given an expected change in yaw in degrees +// note this does not actually active the pivot turn. To activate use the check_activation method +bool AR_PivotTurn::would_activate(float yaw_change_deg) const +{ + // check cases where we clearly cannot use pivot steering + if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) { + return false; + } + + // return true if yaw change is larger than _pivot_angle + return fabsf(wrap_180(yaw_change_deg)) > _angle; +} + +// get turn rate (in rad/sec) +// desired heading should be the heading towards the next waypoint in degrees +// dt should be the time since the last call in seconds +float AR_PivotTurn::get_turn_rate_rads(float desired_heading_deg, float dt) +{ + // handle pivot turns + const float desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(desired_heading_deg), radians(_rate_max)); + + // update flag so that it can be cleared + check_activation(desired_heading_deg); + + return desired_turn_rate_rads; +} + +// return post-turn delay duration in milliseconds +uint32_t AR_PivotTurn::get_delay_duration_ms() const +{ + return constrain_float(_delay.get(), 0.0f, 60.0f) * 1000; +} diff --git a/libraries/AR_WPNav/AR_PivotTurn.h b/libraries/AR_WPNav/AR_PivotTurn.h new file mode 100644 index 00000000000..035a4d28eeb --- /dev/null +++ b/libraries/AR_WPNav/AR_PivotTurn.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include + +/* + * Pivot Turn controller for skid-steering rovers and boats + * + * How-to-Use: + * 1. call "enable(true)" once for skid-steering vehicles to enable this controller + * 2. before vehicle starts towards a waypoint call "check_activation" and provide the earth-frame heading to the waypoint + * this may change the controller's internal state to "active" + * 3. on each main loop iteration call "active()" to see if this controller thinks it is controllering the vehicle + * 4. call "get_turn_rate_rads()" to retrieve the desired turn rate towards the next waypoint + * 5. pass above turn rate into the rate controller, set speed controller's speed to zero + * 6. this controller's "active" state will change to false once it has completed the pivot turn + */ + +class AR_PivotTurn { +public: + + // constructor + AR_PivotTurn(AR_AttitudeControl& atc); + + // enable or disable pivot controller + void enable(bool enable_pivot); + + // true if this controller is controlling vehicle + bool active() const; + + // checks if pivot turns should be activated or deactivated + // force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error + void check_activation(float desired_heading_deg, bool force_active = false); + + // check if pivot turn would be activated given an expected change in yaw in degrees + // note this does not actually active the pivot turn. To activate use the check_activation method + bool would_activate(float yaw_change_deg) const WARN_IF_UNUSED; + + // forcibly deactivate this controller + void deactivate() { _active = false; }; + + // get turn rate (in rad/sec) + // desired heading should be the heading towards the next waypoint in degrees + // dt should be the time since the last call in seconds + float get_turn_rate_rads(float desired_heading_deg, float dt); + + // accessors for parameter values + float get_rate_max() const { return _rate_max; } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // return post-turn delay duration in milliseconds + uint32_t get_delay_duration_ms() const; + + // parameters + AP_Int16 _angle; // minimum angle error (in degrees) that leads to pivot turn + AP_Int16 _rate_max; // maximum turn rate (in degrees) during pivot turn + AP_Float _delay; // waiting time (in seconds) after pivot turn completes + + // references + AR_AttitudeControl& _atc; // rover attitude control library + + // local variables + bool _enabled; // true if vehicle can pivot + bool _active; // true if vehicle is currently pivoting + uint32_t _delay_start_ms; // system time when post-turn delay started +}; From a185e78271fbf5cacc8d9f255eb678f3db42740a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Nov 2021 15:55:56 +0900 Subject: [PATCH 0455/3101] AR_WPNav: integrate AR_PivotTurn class moves pivot turn feature to separate class also does not advance along scurve path while pivotin --- libraries/AR_WPNav/AR_WPNav.cpp | 117 ++++++++------------------------ libraries/AR_WPNav/AR_WPNav.h | 18 +---- 2 files changed, 30 insertions(+), 105 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 5795eafc3fc..0b5ce3c54cd 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -27,9 +27,6 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_RADIUS_DEFAULT 2.0f -#define AR_WPNAV_PIVOT_ANGLE_DEFAULT 60 -#define AR_WPNAV_PIVOT_ANGLE_ACCURACY 5 // vehicle will pivot to within this many degrees of destination -#define AR_WPNAV_PIVOT_RATE_DEFAULT 90 const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -53,23 +50,8 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { // 3 was OVERSHOOT - // @Param: PIVOT_ANGLE - // @DisplayName: Waypoint Pivot Angle - // @Description: Pivot when the difference between the vehicle's heading and its target heading is more than this many degrees. Set to zero to disable pivot turns. Note: This parameter should be greater than 10 degrees for pivot turns to work. - // @Units: deg - // @Range: 0 360 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("PIVOT_ANGLE", 4, AR_WPNav, _pivot_angle, AR_WPNAV_PIVOT_ANGLE_DEFAULT), - - // @Param: PIVOT_RATE - // @DisplayName: Waypoint Pivot Turn Rate - // @Description: Turn rate during pivot turns - // @Units: deg/s - // @Range: 0 360 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("PIVOT_RATE", 5, AR_WPNav, _pivot_rate, AR_WPNAV_PIVOT_RATE_DEFAULT), + // 4 was PIVOT_ANGLE + // 5 was PIVOT_RATE // @Param: SPEED_MIN // @DisplayName: Waypoint speed minimum @@ -80,21 +62,19 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { // @User: Standard AP_GROUPINFO("SPEED_MIN", 6, AR_WPNav, _speed_min, 0), - // @Param: PIVOT_DELAY - // @DisplayName: Delay after pivot turn - // @Description: Waiting time after pivot turn - // @Units: s - // @Range: 0 60 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("PIVOT_DELAY", 7, AR_WPNav, _pivot_delay, 0), + // 7 was PIVOT_DELAY + + // @Group: PIVOT_ + // @Path: AR_PivotTurn.cpp + AP_SUBGROUPINFO(_pivot, "PIVOT_", 8, AR_WPNav, AR_PivotTurn), AP_GROUPEND }; AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : _atc(atc), - _pos_control(pos_control) + _pos_control(pos_control), + _pivot(atc) { AP_Param::setup_object_defaults(this, var_info); } @@ -133,6 +113,9 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float _reached_destination = false; _fast_waypoint = false; + // ensure pivot turns are deactivated + _pivot.deactivate(); + // initialise origin and destination to stopping point Location stopping_loc; if (get_stopping_location(stopping_loc)) { @@ -194,7 +177,7 @@ void AR_WPNav::update(float dt) // if object avoidance is active check if vehicle should pivot towards destination if (_oa_active) { - update_pivot_active_flag(); + _pivot.check_activation(_oa_wp_bearing_cd * 0.01); } // check if vehicle is in recovering state after switching off OA @@ -205,7 +188,10 @@ void AR_WPNav::update(float dt) } } - advance_wp_target_along_track(current_loc, dt); + // advance target along path unless vehicle is pivoting + if (!_pivot.active()) { + advance_wp_target_along_track(current_loc, dt); + } // handle stopping vehicle if avoidance has failed if (stop_vehicle) { @@ -238,9 +224,14 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _orig_and_dest_valid = true; _reached_destination = false; - // determine if we should pivot immediately update_distance_and_bearing_to_destination(); - update_pivot_active_flag(); + + // check if vehicle should pivot if vehicle stopped at previous waypoint + // or journey to previous waypoint was interrupted or navigation has just started + if (!_fast_waypoint) { + _pivot.deactivate(); + _pivot.check_activation(_oa_wp_bearing_cd * 0.01); + } // convert origin and destination to offset from EKF origin Vector2f origin_NE; @@ -366,57 +357,6 @@ bool AR_WPNav::get_stopping_location(Location& stopping_loc) return true; } -// returns true if vehicle should pivot turn at next waypoint -bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const -{ - // check cases where we clearly cannot use pivot steering - if (!_pivot_possible || _pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY) { - return false; - } - - // if error is larger than _pivot_angle then use pivot steering at next WP - if (fabsf(yaw_error_cd) * 0.01f > _pivot_angle) { - return true; - } - - return false; -} - -// updates _pivot_active flag based on heading error to destination -// relies on update_distance_and_bearing_to_destination having been called first -// to update _oa_wp_bearing and _reversed variables -void AR_WPNav::update_pivot_active_flag() -{ - // check cases where we clearly cannot use pivot steering - if (!_pivot_possible || (_pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY)) { - _pivot_active = false; - return; - } - - // calc yaw error - const float yaw_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd; - const float yaw_error = fabsf(wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor)) * 0.01f; - - // if error is larger than _pivot_angle start pivot steering - if (yaw_error > _pivot_angle) { - _pivot_active = true; - return; - } - - uint32_t now = AP_HAL::millis(); - - // if within 5 degrees of the target heading, set start time of pivot steering - if (_pivot_active && yaw_error < AR_WPNAV_PIVOT_ANGLE_ACCURACY && _pivot_start_ms == 0) { - _pivot_start_ms = now; - } - - // exit pivot steering after the time set by pivot_delay has elapsed - if (_pivot_start_ms > 0 && now - _pivot_start_ms >= constrain_float(_pivot_delay.get(), 0.0f, 60.0f) * 1000.0f) { - _pivot_active = false; - _pivot_start_ms = 0; - } -} - // true if update has been called recently bool AR_WPNav::is_active() const { @@ -515,17 +455,14 @@ void AR_WPNav::update_distance_and_bearing_to_destination() void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { // handle pivot turns - if (_pivot_active) { + if (_pivot.active()) { _cross_track_error = calc_crosstrack_error(current_loc); _desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;; + _desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt); _desired_lat_accel = 0.0f; - _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate)); // decelerate to zero _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); - - // update flag so that it can be cleared - update_pivot_active_flag(); return; } @@ -541,7 +478,7 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible) { _turn_radius = pivot_possible ? 0.0 : turn_radius; - _pivot_possible = pivot_possible; + _pivot.enable(pivot_possible); } // adjust speed to ensure it does not fall below value held in SPEED_MIN diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index a872033130e..c06408da1cf 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -5,6 +5,7 @@ #include #include #include +#include "AR_PivotTurn.h" const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown @@ -84,7 +85,7 @@ class AR_WPNav { // accessors for parameter values float get_default_speed() const { return _speed_max; } float get_radius() const { return _radius; } - float get_pivot_rate() const { return _pivot_rate; } + float get_pivot_rate() const { return _pivot.get_rate_max(); } // calculate stopping location using current position and attitude controller provided maximum deceleration // returns true on success, false on failure @@ -107,14 +108,6 @@ class AR_WPNav { // calculate steering and speed to drive along line from origin to destination waypoint void update_steering_and_speed(const Location ¤t_loc, float dt); - // returns true if vehicle should pivot turn at next waypoint - bool use_pivot_steering_at_next_WP(float yaw_error_cd) const; - - // updates _pivot_active flag based on heading error to destination - // relies on update_distance_and_bearing_to_destination having been called first - // to update _oa_wp_bearing and _reversed variables - void update_pivot_active_flag(); - // adjust speed to ensure it does not fall below value held in SPEED_MIN // desired_speed should always be positive (or zero) void apply_speed_min(float &desired_speed) const; @@ -126,9 +119,7 @@ class AR_WPNav { AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached - AP_Int16 _pivot_angle; // angle error that leads to pivot turn - AP_Int16 _pivot_rate; // desired turn rate during pivot turns in deg/sec - AP_Float _pivot_delay; // waiting time after pivot turn + AR_PivotTurn _pivot; // pivot turn controller // references AR_AttitudeControl& _atc; // rover attitude control library @@ -144,12 +135,9 @@ class AR_WPNav { // variables held in vehicle code (for now) float _turn_radius; // vehicle turn radius in meters - bool _pivot_possible; // true if vehicle can pivot - bool _pivot_active; // true if vehicle is currently pivoting // variables for navigation uint32_t _last_update_ms; // system time of last call to update - uint32_t _pivot_start_ms; // system time when pivot turn started Location _origin; // origin Location (vehicle will travel from the origin to the destination) Location _destination; // destination Location when in Guided_WP bool _orig_and_dest_valid; // true if the origin and destination have been set From c66f6188ea1869c3ab8e0345ac8b360516fe2314 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 14 Dec 2021 12:44:21 +0900 Subject: [PATCH 0456/3101] Rover: param conversion for WP_PIVOT params parameters moved from WPNav to AR_PivotTurn --- Rover/Parameters.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index b67981597ee..8df6413fc9e 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -743,10 +743,10 @@ const AP_Param::ConversionInfo conversion_table[] = { { Parameters::k_param_throttle_min_old, 0, AP_PARAM_INT8, "MOT_THR_MIN" }, { Parameters::k_param_throttle_max_old, 0, AP_PARAM_INT8, "MOT_THR_MAX" }, { Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" }, - { Parameters::k_param_pivot_turn_angle_old, 0, AP_PARAM_INT16, "WP_PIVOT_ANGLE" }, { Parameters::k_param_waypoint_radius_old, 0, AP_PARAM_FLOAT, "WP_RADIUS" }, - { Parameters::k_param_g2, 20, AP_PARAM_INT16, "WP_PIVOT_RATE" }, - + { Parameters::k_param_g2, 299, AP_PARAM_INT16, "WP_PIVOT_ANGLE" }, + { Parameters::k_param_g2, 363, AP_PARAM_INT16, "WP_PIVOT_RATE" }, + { Parameters::k_param_g2, 491, AP_PARAM_FLOAT, "WP_PIVOT_DELAY" }, { Parameters::k_param_g2, 32, AP_PARAM_FLOAT, "SAIL_ANGLE_MIN" }, { Parameters::k_param_g2, 33, AP_PARAM_FLOAT, "SAIL_ANGLE_MAX" }, { Parameters::k_param_g2, 34, AP_PARAM_FLOAT, "SAIL_ANGLE_IDEAL" }, From 2a1013e8965e2a1e877b546837c062380be7ca63 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 13:35:23 +0900 Subject: [PATCH 0457/3101] AR_WPNav: set desired lat accel and crosstrack to zero when disarmed --- libraries/AR_WPNav/AR_WPNav.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 0b5ce3c54cd..368baa5371c 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -133,7 +133,9 @@ void AR_WPNav::update(float dt) float speed; if (!hal.util->get_soft_armed() || !_orig_and_dest_valid || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) { _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; _desired_turn_rate_rads = 0.0f; + _cross_track_error = 0; return; } From 6515e71fc1882c35d64cf84d14972488376b1b2c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 13:35:34 +0900 Subject: [PATCH 0458/3101] AR_WPNav: move OA path planning handling into a separate class --- libraries/AR_WPNav/AR_WPNav.cpp | 87 ++--------- libraries/AR_WPNav/AR_WPNav.h | 30 ++-- libraries/AR_WPNav/AR_WPNav_OA.cpp | 228 +++++++++++++++++++++++++++++ libraries/AR_WPNav/AR_WPNav_OA.h | 43 ++++++ 4 files changed, 296 insertions(+), 92 deletions(-) create mode 100644 libraries/AR_WPNav/AR_WPNav_OA.cpp create mode 100644 libraries/AR_WPNav/AR_WPNav_OA.h diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 368baa5371c..e6f21d5fd35 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -145,65 +145,13 @@ void AR_WPNav::update(float dt) } _last_update_ms = AP_HAL::millis(); - // run path planning around obstacles - bool stop_vehicle = false; - - // true if OA has been recently active; - bool _oa_was_active = _oa_active; - - AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); - if (oa != nullptr) { - AP_OAPathPlanner::OAPathPlannerUsed path_planner_used; - const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin, _destination, _oa_origin, _oa_destination, path_planner_used); - switch (oa_retstate) { - case AP_OAPathPlanner::OA_NOT_REQUIRED: - _oa_active = false; - break; - case AP_OAPathPlanner::OA_PROCESSING: - case AP_OAPathPlanner::OA_ERROR: - // during processing or in case of error, slow vehicle to a stop - stop_vehicle = true; - _oa_active = false; - break; - case AP_OAPathPlanner::OA_SUCCESS: - _oa_active = true; - break; - } - } - if (!_oa_active) { - _oa_origin = _origin; - _oa_destination = _destination; - } - update_distance_and_bearing_to_destination(); - // if object avoidance is active check if vehicle should pivot towards destination - if (_oa_active) { - _pivot.check_activation(_oa_wp_bearing_cd * 0.01); - } - - // check if vehicle is in recovering state after switching off OA - if (!_oa_active && _oa_was_active) { - if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) { - //reset wp origin to vehicles current location - _origin = current_loc; - } - } - // advance target along path unless vehicle is pivoting if (!_pivot.active()) { advance_wp_target_along_track(current_loc, dt); } - // handle stopping vehicle if avoidance has failed - if (stop_vehicle) { - // decelerate to speed to zero and set turn rate to zero - _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); - _desired_lat_accel = 0.0f; - _desired_turn_rate_rads = 0.0f; - return; - } - // update_steering_and_speed update_steering_and_speed(current_loc, dt); } @@ -221,8 +169,8 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _scurve_prev_leg = _scurve_this_leg; // initialise some variables - _oa_origin = _origin = _destination; - _oa_destination = _destination = destination; + _origin = _destination; + _destination = destination; _orig_and_dest_valid = true; _reached_destination = false; @@ -232,7 +180,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location // or journey to previous waypoint was interrupted or navigation has just started if (!_fast_waypoint) { _pivot.deactivate(); - _pivot.check_activation(_oa_wp_bearing_cd * 0.01); + _pivot.check_activation(oa_wp_bearing_cd() * 0.01); } // convert origin and destination to offset from EKF origin @@ -434,23 +382,10 @@ void AR_WPNav::update_distance_and_bearing_to_destination() if (!_orig_and_dest_valid || !AP::ahrs().get_location(current_loc)) { _distance_to_destination = 0.0f; _wp_bearing_cd = 0.0f; - - // update OA adjusted values - _oa_distance_to_destination = 0.0f; - _oa_wp_bearing_cd = 0.0f; return; } _distance_to_destination = current_loc.get_distance(_destination); _wp_bearing_cd = current_loc.get_bearing_to(_destination); - - // update OA adjusted values - if (_oa_active) { - _oa_distance_to_destination = current_loc.get_distance(_oa_destination); - _oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination); - } else { - _oa_distance_to_destination = _distance_to_destination; - _oa_wp_bearing_cd = _wp_bearing_cd; - } } // calculate steering and speed to drive along line from origin to destination waypoint @@ -459,7 +394,7 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) // handle pivot turns if (_pivot.active()) { _cross_track_error = calc_crosstrack_error(current_loc); - _desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;; + _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); _desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt); _desired_lat_accel = 0.0f; @@ -495,26 +430,30 @@ void AR_WPNav::apply_speed_min(float &desired_speed) const desired_speed = MAX(desired_speed, _speed_min); } -// calculate the crosstrack error (does not rely on L1 controller) +// calculate the crosstrack error float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const { if (!_orig_and_dest_valid) { return 0.0f; } + // get object avoidance adjusted origin and destination + const Location &orig = get_oa_origin(); + const Location &dest = get_oa_destination(); + // calculate the NE position of destination relative to origin - Vector2f dest_from_origin = _oa_origin.get_distance_NE(_oa_destination); + Vector2f dest_from_origin = orig.get_distance_NE(dest); - // return distance to origin if length of track is very small + // return distance to destination if length of track is very small if (dest_from_origin.length() < 1.0e-6f) { - return current_loc.get_distance_NE(_oa_destination).length(); + return current_loc.get_distance_NE(dest).length(); } // convert to a vector indicating direction only dest_from_origin.normalize(); // calculate the NE position of the vehicle relative to origin - const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc); + const Vector2f veh_from_origin = orig.get_distance_NE(current_loc); // calculate distance to target track, for reporting return fabsf(veh_from_origin % dest_from_origin); diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index c06408da1cf..91e4eeef3bb 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -7,8 +7,6 @@ #include #include "AR_PivotTurn.h" -const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown - class AR_WPNav { public: @@ -23,7 +21,7 @@ class AR_WPNav { void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max); // update navigation - void update(float dt); + virtual void update(float dt); // return desired speed float get_desired_speed() const { return _desired_speed; } @@ -47,7 +45,7 @@ class AR_WPNav { // set desired location and (optionally) next_destination // next_destination should be provided if known to allow smooth cornering - bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; + virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; // set desired location to a reasonable stopping point, return true on success bool set_desired_location_to_stopping_location() WARN_IF_UNUSED; @@ -57,7 +55,7 @@ class AR_WPNav { bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED; // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck - bool reached_destination() const { return _reached_destination; } + virtual bool reached_destination() const { return _reached_destination; } // return distance (in meters) to destination float get_distance_to_destination() const { return _distance_to_destination; } @@ -66,18 +64,21 @@ class AR_WPNav { bool is_destination_valid() const { return _orig_and_dest_valid; } // get current destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) - const Location &get_destination() { return _destination; } - - // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) - const Location &get_oa_destination() { return _oa_destination; } + const Location &get_destination() const { return _destination; } // return heading (in centi-degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) float wp_bearing_cd() const { return _wp_bearing_cd; } float nav_bearing_cd() const { return _desired_heading_cd; } float crosstrack_error() const { return _cross_track_error; } + // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + virtual const Location &get_oa_origin() const { return _origin; } + + // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + virtual const Location &get_oa_destination() const { return get_destination(); } + // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) - float oa_wp_bearing_cd() const { return _oa_wp_bearing_cd; } + virtual float oa_wp_bearing_cd() const { return wp_bearing_cd(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) void set_turn_params(float turn_radius, bool pivot_possible); @@ -94,7 +95,7 @@ class AR_WPNav { // parameter var table static const struct AP_Param::GroupInfo var_info[]; -private: +protected: // true if update has been called recently bool is_active() const; @@ -155,11 +156,4 @@ class AR_WPNav { // variables for reporting float _distance_to_destination; // distance from vehicle to final destination in meters bool _reached_destination; // true once the vehicle has reached the destination - - // object avoidance variables - bool _oa_active; // true if we should use alternative destination to avoid obstacles - Location _oa_origin; // intermediate origin during avoidance - Location _oa_destination; // intermediate destination during avoidance - float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters - float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees }; diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp new file mode 100644 index 00000000000..68d51e3c514 --- /dev/null +++ b/libraries/AR_WPNav/AR_WPNav_OA.cpp @@ -0,0 +1,228 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#include +#include "AR_WPNav_OA.h" +#include + +extern const AP_HAL::HAL& hal; + +// update navigation +void AR_WPNav_OA::update(float dt) +{ + // exit immediately if no current location, origin or destination + Location current_loc; + float speed; + if (!hal.util->get_soft_armed() || !is_destination_valid() || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) { + _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; + _desired_turn_rate_rads = 0.0f; + _oa_active = false; + return; + } + + // run path planning around obstacles + bool stop_vehicle = false; + + // backup _origin and _destination when not doing oa + if (!_oa_active) { + _origin_oabak = _origin; + _destination_oabak = _destination; + } + + AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); + if (oa != nullptr) { + Location oa_origin_new, oa_destination_new; + AP_OAPathPlanner::OAPathPlannerUsed path_planner_used; + const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin_oabak, _destination_oabak, oa_origin_new, oa_destination_new, path_planner_used); + switch (oa_retstate) { + + case AP_OAPathPlanner::OA_NOT_REQUIRED: + if (_oa_active) { + // object avoidance has become inactive so reset target to original destination + if (!AR_WPNav::set_desired_location(_destination_oabak)) { + // this should never happen because we should have an EKF origin and the destination must be valid + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + _oa_active = false; + // ToDo: handle "if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET)" + } + break; + + case AP_OAPathPlanner::OA_PROCESSING: + case AP_OAPathPlanner::OA_ERROR: + // during processing or in case of error, slow vehicle to a stop + stop_vehicle = true; + _oa_active = false; + break; + + case AP_OAPathPlanner::OA_SUCCESS: + // handling of returned destination depends upon path planner used + switch (path_planner_used) { + + case AP_OAPathPlanner::OAPathPlannerUsed::None: + case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical: + // this should never happen. this means the path planner has returned success but has returned an invalid planner + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + _oa_active = false; + stop_vehicle = true; + return; + + case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras: + // Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed + if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) { + if (AR_WPNav::set_desired_location(oa_destination_new)) { + // if new target set successfully, update oa state and destination + _oa_active = true; + _oa_origin = oa_origin_new; + _oa_destination = oa_destination_new; + } else { + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + } + break; + + case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: { + // calculate final destination as an offset from EKF origin in NE + Vector2f dest_NE_ftype; + if (oa_destination_new.get_vector_xy_from_origin_NE(dest_NE_ftype)) { + // initialise position controller if required + if (!_oa_active) { + _pos_control.init(); + _oa_active = true; + } + _oa_origin = oa_origin_new; + _oa_destination = oa_destination_new; + + // check for pivot turn + update_oa_distance_and_bearing_to_destination(); + _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); + + if (!_pivot.active()) { + // pass the desired position directly to the position controller + dest_NE_ftype *= 0.01; // convert to meters + Vector2p dest_NE(dest_NE_ftype.x, dest_NE_ftype.y); + _pos_control.input_pos_target(dest_NE, dt); + + // run position controllers + _pos_control.update(dt); + } + + update_distance_and_bearing_to_destination(); + + // update_steering_and_speed + update_steering_and_speed(current_loc, dt); + + // return without calling parent AC_WPNav + return; + } else { + // this should never happen because we can only get here if we have an EKF origin + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + } + break; + + } // switch (path_planner_used) { + } // switch (oa_retstate) { + } // if (oa != nullptr) { + + update_oa_distance_and_bearing_to_destination(); + + // handle stopping vehicle if avoidance has failed + if (stop_vehicle) { + // decelerate to speed to zero and set turn rate to zero + _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; + _desired_turn_rate_rads = 0.0f; + return; + } + + // call parent update + AR_WPNav::update(dt); +} + +// set desired location and (optionally) next_destination +// next_destination should be provided if known to allow smooth cornering +bool AR_WPNav_OA::set_desired_location(const struct Location& destination, Location next_destination) +{ + const bool ret = AR_WPNav::set_desired_location(destination, next_destination); + + if (ret) { + // disable object avoidance, it will be re-enabled (if necessary) on next update + _oa_active = false; + } + + return ret; +} + +// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck +bool AR_WPNav_OA::reached_destination() const +{ + // object avoidance should always be deactivated before reaching final destination + if (_oa_active) { + return false; + } + + return AR_WPNav::reached_destination(); +} + +// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) +const Location &AR_WPNav_OA::get_oa_origin() const +{ + if (_oa_active) { + return _oa_origin; + } + + return _origin; +} + +// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) +const Location &AR_WPNav_OA::get_oa_destination() const +{ + if (_oa_active) { + return _oa_destination; + } + + return AR_WPNav::get_oa_destination(); +} + +// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) +float AR_WPNav_OA::oa_wp_bearing_cd() const +{ + if (_oa_active) { + return _oa_wp_bearing_cd; + } + + return AR_WPNav::oa_wp_bearing_cd(); +} + +// update distance from vehicle's current position to destination +void AR_WPNav_OA::update_oa_distance_and_bearing_to_destination() +{ + // update OA adjusted values + Location current_loc; + if (_oa_active && AP::ahrs().get_location(current_loc)) { + _oa_distance_to_destination = current_loc.get_distance(_oa_destination); + _oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination); + } else { + _oa_distance_to_destination = AR_WPNav::get_distance_to_destination(); + _oa_wp_bearing_cd = AR_WPNav::wp_bearing_cd(); + } +} diff --git a/libraries/AR_WPNav/AR_WPNav_OA.h b/libraries/AR_WPNav/AR_WPNav_OA.h new file mode 100644 index 00000000000..195ed3028f2 --- /dev/null +++ b/libraries/AR_WPNav/AR_WPNav_OA.h @@ -0,0 +1,43 @@ +#pragma once + +#include "AR_WPNav.h" + +class AR_WPNav_OA : public AR_WPNav { +public: + + // re-use parent's constructor + using AR_WPNav::AR_WPNav; + + // update navigation + void update(float dt) override; + + // set desired location and (optionally) next_destination + // next_destination should be provided if known to allow smooth cornering + bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED; + + // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck + bool reached_destination() const override; + + // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + const Location &get_oa_origin() const override; + + // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + const Location &get_oa_destination() const override; + + // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) + float oa_wp_bearing_cd() const override; + +private: + + // update distance and bearing from vehicle's current position to destination + void update_oa_distance_and_bearing_to_destination(); + + // object avoidance variables + bool _oa_active; // true if we should use alternative destination to avoid obstacles + Location _origin_oabak; // backup of _origin so it can be restored when oa completes + Location _destination_oabak; // backup of _desitnation so it can be restored when oa completes + Location _oa_origin; // intermediate origin during avoidance + Location _oa_destination; // intermediate destination during avoidance + float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters + float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees +}; From 3d048ddcc277415c4a8d113c14337d9266be3969 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 12:44:27 +0900 Subject: [PATCH 0459/3101] AR_WPNav: fix pivot activation when reversing --- libraries/AR_WPNav/AR_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index e6f21d5fd35..ce12571123d 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -180,7 +180,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location // or journey to previous waypoint was interrupted or navigation has just started if (!_fast_waypoint) { _pivot.deactivate(); - _pivot.check_activation(oa_wp_bearing_cd() * 0.01); + _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); } // convert origin and destination to offset from EKF origin From b1237ffe801eceadb3bf495aff8a62bb863b85e3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 20 Dec 2021 19:53:10 +0900 Subject: [PATCH 0460/3101] AR_WPNav: always pivot at corners of at least WP_PIVOT_ANGLE --- libraries/AR_WPNav/AR_WPNav.cpp | 62 ++++++++++++++++++++++----------- libraries/AR_WPNav/AR_WPNav.h | 5 +++ 2 files changed, 46 insertions(+), 21 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index ce12571123d..0028cbe0211 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -115,6 +115,7 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float // ensure pivot turns are deactivated _pivot.deactivate(); + _pivot_at_next_wp = false; // initialise origin and destination to stopping point Location stopping_loc; @@ -180,7 +181,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location // or journey to previous waypoint was interrupted or navigation has just started if (!_fast_waypoint) { _pivot.deactivate(); - _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); + _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01, _pivot_at_next_wp); } // convert origin and destination to offset from EKF origin @@ -210,28 +211,33 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location } // handle next destination + _scurve_next_leg.init(); + _fast_waypoint = false; + _pivot_at_next_wp = false; if (next_destination.initialised()) { - // convert next_destination to offset from EKF origin - Vector2f next_destination_NE; - if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) { - return false; + // check if vehicle should pivot at next waypoint + const float next_wp_yaw_change = get_corner_angle(_origin, destination, next_destination); + _pivot_at_next_wp = _pivot.would_activate(next_wp_yaw_change); + if (!_pivot_at_next_wp) { + // convert next_destination to offset from EKF origin + Vector2f next_destination_NE; + if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) { + return false; + } + next_destination_NE *= 0.01f; + _scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f}, + Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f}, + _pos_control.get_speed_max(), + _pos_control.get_speed_max(), // speed up (not used) + _pos_control.get_speed_max(), // speed down (not used) + _pos_control.get_accel_max(), + _pos_control.get_accel_max(), // vertical accel (not used) + 1.0, // jerk time + _scurve_jerk); + + // next destination provided so fast waypoint + _fast_waypoint = true; } - next_destination_NE *= 0.01f; - _scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f}, - Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f}, - _pos_control.get_speed_max(), - _pos_control.get_speed_max(), // speed up (not used) - _pos_control.get_speed_max(), // speed down (not used) - _pos_control.get_accel_max(), - _pos_control.get_accel_max(), // vertical accel (not used) - 1.0, // jerk time - _scurve_jerk); - - // next destination provided so fast waypoint - _fast_waypoint = true; - } else { - _scurve_next_leg.init(); - _fast_waypoint = false; } update_distance_and_bearing_to_destination(); @@ -458,3 +464,17 @@ float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const // calculate distance to target track, for reporting return fabsf(veh_from_origin % dest_from_origin); } + +// calculate yaw change at next waypoint in degrees +// returns zero if the angle cannot be calculated because some points are on top of others +float AR_WPNav::get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const +{ + // sanity check + if (!loc1.initialised() || !loc2.initialised() || !loc3.initialised()) { + return 0; + } + const float loc1_to_loc2_deg = loc1.get_bearing_to(loc2) * 0.01; + const float loc2_to_loc3_deg = loc2.get_bearing_to(loc3) * 0.01; + const float diff_yaw_deg = wrap_180(loc2_to_loc3_deg - loc1_to_loc2_deg); + return diff_yaw_deg; +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 91e4eeef3bb..30b62374a99 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -116,6 +116,10 @@ class AR_WPNav { // calculate the crosstrack error (does not rely on L1 controller) float calc_crosstrack_error(const Location& current_loc) const; + // calculate yaw change at next waypoint in degrees + // returns zero if the angle cannot be calculated because some points are on top of others + float get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const; + // parameters AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners @@ -132,6 +136,7 @@ class AR_WPNav { SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory float _scurve_jerk; // scurve jerk max in m/s/s/s bool _fast_waypoint; // true if vehicle will stop at the next waypoint + bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle // variables held in vehicle code (for now) From 47ab961f429bd9404e028e6c25563db6316fbf1b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 08:52:20 +0900 Subject: [PATCH 0461/3101] AR_WPNav: fix application of turn-max-g --- libraries/AR_WPNav/AR_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 0028cbe0211..72d0dfde48f 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -230,7 +230,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), _pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed down (not used) - _pos_control.get_accel_max(), + MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), _pos_control.get_accel_max(), // vertical accel (not used) 1.0, // jerk time _scurve_jerk); From 461c5196e72984e2c3352756918b0236dcc01121 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Dec 2021 11:27:01 +0900 Subject: [PATCH 0462/3101] AR_WPNav: add ACCEL and JERK params and accessors allows users to specify a different acceleration and jerk for Auto, Guided, RTL, etc compared with manual modes (Acro, etc) --- libraries/AR_WPNav/AR_WPNav.cpp | 42 ++++++++++++++++++++++++++++++--- libraries/AR_WPNav/AR_WPNav.h | 8 +++++++ 2 files changed, 47 insertions(+), 3 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 72d0dfde48f..5b05a197d5f 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -68,6 +68,24 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { // @Path: AR_PivotTurn.cpp AP_SUBGROUPINFO(_pivot, "PIVOT_", 8, AR_WPNav, AR_PivotTurn), + // @Param: ACCEL + // @DisplayName: Waypoint acceleration + // @Description: Waypoint acceleration. If zero then ATC_ACCEL_MAX is used + // @Units: m/s/s + // @Range: 0 100 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("ACCEL", 9, AR_WPNav, _accel_max, 0), + + // @Param: JERK + // @DisplayName: Waypoint jerk + // @Description: Waypoint jerk (change in acceleration). If zero then jerk is same as acceleration + // @Units: m/s/s/s + // @Range: 0 100 + // @Increment: 0.1 + // @User: Standard + AP_GROUPINFO("JERK", 10, AR_WPNav, _jerk_max, 0), + AP_GROUPEND }; @@ -91,18 +109,18 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float speed_max = _speed_max; } if (!is_positive(accel_max)) { - accel_max = _atc.get_accel_max(); + accel_max = get_accel_max(); } if (!is_positive(lat_accel_max)) { lat_accel_max = _atc.get_turn_lat_accel_max(); } if (!is_positive(jerk_max)) { - jerk_max = _atc.get_accel_max(); + jerk_max = get_jerk_max(); } _scurve_jerk = jerk_max; // initialise position controller - _pos_control.set_limits(speed_max, accel_max, lat_accel_max); + _pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max); _scurve_prev_leg.init(); _scurve_this_leg.init(); @@ -284,6 +302,24 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vecto return set_desired_location(dest_loc, next_dest_loc); } +// get max acceleration in m/s/s +float AR_WPNav::get_accel_max() const +{ + if (is_positive(_accel_max)) { + return _accel_max; + } + return _atc.get_accel_max(); +} + +// get max jerk in m/s/s/s +float AR_WPNav::get_jerk_max() const +{ + if (is_positive(_jerk_max)) { + return _jerk_max; + } + return get_accel_max(); +} + // calculate vehicle stopping point using current location, velocity and maximum acceleration bool AR_WPNav::get_stopping_location(Location& stopping_loc) { diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 30b62374a99..ee4e680a7ef 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -88,6 +88,12 @@ class AR_WPNav { float get_radius() const { return _radius; } float get_pivot_rate() const { return _pivot.get_rate_max(); } + // get max acceleration in m/s/s + float get_accel_max() const; + + // get max jerk in m/s/s/s + float get_jerk_max() const; + // calculate stopping location using current position and attitude controller provided maximum deceleration // returns true on success, false on failure bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED; @@ -125,6 +131,8 @@ class AR_WPNav { AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached AR_PivotTurn _pivot; // pivot turn controller + AP_Float _accel_max; // max acceleration. If zero then attitude controller's specified max accel is used + AP_Float _jerk_max; // max jerk (change in acceleration). If zero then value is same as accel_max // references AR_AttitudeControl& _atc; // rover attitude control library From 1fd9da710d01becf6fe478618b4c1eca4e1994e4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 10:06:42 +0900 Subject: [PATCH 0463/3101] AR_WPNav: add internal error if invalid destination received --- libraries/AR_WPNav/AR_WPNav.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 5b05a197d5f..1f374586c87 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -207,6 +207,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location Vector2f destination_NE; if (!_origin.get_vector_xy_from_origin_NE(origin_NE) || !_destination.get_vector_xy_from_origin_NE(destination_NE)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return false; } origin_NE *= 0.01f; @@ -240,6 +241,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location // convert next_destination to offset from EKF origin Vector2f next_destination_NE; if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); return false; } next_destination_NE *= 0.01f; From f769a18996701b47119348bc99c4b5b9fc009458 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 11:48:33 +0900 Subject: [PATCH 0464/3101] AR_WPNav: add set_desired_location_expect_fast_update --- libraries/AR_WPNav/AR_WPNav.cpp | 84 +++++++++++++++++++++++++++++++-- libraries/AR_WPNav/AR_WPNav.h | 18 ++++++- 2 files changed, 97 insertions(+), 5 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 1f374586c87..5daed036e26 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -168,7 +168,14 @@ void AR_WPNav::update(float dt) // advance target along path unless vehicle is pivoting if (!_pivot.active()) { - advance_wp_target_along_track(current_loc, dt); + switch (_nav_control_type) { + case NavControllerType::NAV_SCURVE: + advance_wp_target_along_track(current_loc, dt); + break; + case NavControllerType::NAV_PSC_INPUT_SHAPING: + update_psc_input_shaping(dt); + break; + } } // update_steering_and_speed @@ -179,8 +186,8 @@ void AR_WPNav::update(float dt) // next_destination should be provided if known to allow smooth cornering bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination) { - // re-initialise if previous destination has been interrupted - if (!is_active() || !_reached_destination) { + // re-initialise if inactive, previous destination has been interrupted or different controller was used + if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) { init(0,0,0,0); } @@ -260,6 +267,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location } } + // scurves used for navigation to destination + _nav_control_type = NavControllerType::NAV_SCURVE; + update_distance_and_bearing_to_destination(); return true; @@ -304,6 +314,32 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vecto return set_desired_location(dest_loc, next_dest_loc); } +// set desired location but expect the destination to be updated again in the near future +// position controller input shaping will be used for navigation instead of scurves +// Note: object avoidance is not supported if this method is used +bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destination) +{ + // initialise if not active + if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) { + init(0,0,0,0); + } + + // initialise some variables + _origin = _destination; + _destination = destination; + _orig_and_dest_valid = true; + _reached_destination = false; + + update_distance_and_bearing_to_destination(); + + // check if vehicle should pivot + _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); + + // position controller input shaping used for navigation to destination + _nav_control_type = NavControllerType::NAV_PSC_INPUT_SHAPING; + return true; +} + // get max acceleration in m/s/s float AR_WPNav::get_accel_max() const { @@ -357,7 +393,7 @@ bool AR_WPNav::is_active() const return ((AP_HAL::millis() - _last_update_ms) < AR_WPNAV_TIMEOUT_MS); } -// move target location along track from origin to destination +// move target location along track from origin to destination using SCurves navigation void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float dt) { // exit immediately if no current location, destination or disarmed @@ -401,6 +437,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); // pass new target to the position controller + init_pos_control_if_necessary(); Vector2p target_pos_ptype{target_pos_3d_ftype.x, target_pos_3d_ftype.y}; _pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy()); @@ -418,6 +455,31 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float } } +// update psc input shaping navigation controller +void AR_WPNav::update_psc_input_shaping(float dt) +{ + // convert destination location to offset from EKF origin (in meters) + Vector2f pos_target_cm; + if (!_destination.get_vector_xy_from_origin_NE(pos_target_cm)) { + return; + } + + // initialise position controller if not called recently + init_pos_control_if_necessary(); + + // convert to meters and update target + const Vector2p pos_target = pos_target_cm.topostype() * 0.01; + _pos_control.input_pos_target(pos_target, dt); + + // update reached_destination + if (!_reached_destination) { + // calculate position difference between destination and position controller input shaped target + Vector2p pos_target_diff = pos_target - _pos_control.get_pos_target(); + // vehicle has reached destination when the target is within 1cm of the destination and vehicle is within waypoint radius + _reached_destination = (pos_target_diff.length_squared() < sq(0.01)) && (_pos_control.get_pos_error().length_squared() < sq(_radius)); + } +} + // update distance from vehicle's current position to destination void AR_WPNav::update_distance_and_bearing_to_destination() { @@ -516,3 +578,17 @@ float AR_WPNav::get_corner_angle(const Location& loc1, const Location& loc2, con const float diff_yaw_deg = wrap_180(loc2_to_loc3_deg - loc1_to_loc2_deg); return diff_yaw_deg; } + +// helper function to initialise position controller if it hasn't been called recently +// this should be called before updating the position controller with new targets but after the EKF has a good position estimate +void AR_WPNav::init_pos_control_if_necessary() +{ + // initialise position controller if not called recently + if (!_pos_control.is_active()) { + if (!_pos_control.init()) { + // this should never fail because we should always have a valid position estimate at this point + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + } +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index ee4e680a7ef..af112af426d 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -54,6 +54,11 @@ class AR_WPNav { bool set_desired_location_NED(const Vector3f& destination) WARN_IF_UNUSED; bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED; + // set desired location but expect the destination to be updated again in the near future + // position controller input shaping will be used for navigation instead of scurves + // Note: object avoidance is not supported if this method is used + bool set_desired_location_expect_fast_update(const Location &destination) WARN_IF_UNUSED; + // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck virtual bool reached_destination() const { return _reached_destination; } @@ -106,9 +111,12 @@ class AR_WPNav { // true if update has been called recently bool is_active() const; - // move target location along track from origin to destination + // move target location along track from origin to destination using SCurves navigation void advance_wp_target_along_track(const Location ¤t_loc, float dt); + // update psc input shaping navigation controller + void update_psc_input_shaping(float dt); + // update distance and bearing from vehicle's current position to destination void update_distance_and_bearing_to_destination(); @@ -126,6 +134,10 @@ class AR_WPNav { // returns zero if the angle cannot be calculated because some points are on top of others float get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const; + // helper function to initialise position controller if it hasn't been called recently + // this should be called before updating the position controller with new targets but after the EKF has a good position estimate + void init_pos_control_if_necessary(); + // parameters AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners @@ -156,6 +168,10 @@ class AR_WPNav { Location _destination; // destination Location when in Guided_WP bool _orig_and_dest_valid; // true if the origin and destination have been set bool _reversed; // execute the mission by backing up + enum class NavControllerType { + NAV_SCURVE = 0, // scurves used for navigation + NAV_PSC_INPUT_SHAPING // position controller input shaping used for navigation + } _nav_control_type; // navigation controller that should be used to travel from _origin to _destination // main outputs from navigation library float _desired_speed; // desired speed in m/s From 0bcae12ceba22c387a3b89191b954ee202093253 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 12:26:51 +0900 Subject: [PATCH 0465/3101] AR_WPNav_OA: use wpnav's expect fast updates --- libraries/AR_WPNav/AR_WPNav_OA.cpp | 44 +++++++----------------------- 1 file changed, 10 insertions(+), 34 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp index 68d51e3c514..9bbb7ea8c5c 100644 --- a/libraries/AR_WPNav/AR_WPNav_OA.cpp +++ b/libraries/AR_WPNav/AR_WPNav_OA.cpp @@ -99,42 +99,18 @@ void AR_WPNav_OA::update(float dt) break; case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: { - // calculate final destination as an offset from EKF origin in NE - Vector2f dest_NE_ftype; - if (oa_destination_new.get_vector_xy_from_origin_NE(dest_NE_ftype)) { - // initialise position controller if required - if (!_oa_active) { - _pos_control.init(); + // BendyRuler. Action is only needed if path planner has just became active or the target destination's lat or lon has changed + if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) { + if (AR_WPNav::set_desired_location_expect_fast_update(oa_destination_new)) { + // if new target set successfully, update oa state and destination _oa_active = true; + _oa_origin = oa_origin_new; + _oa_destination = oa_destination_new; + } else { + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; } - _oa_origin = oa_origin_new; - _oa_destination = oa_destination_new; - - // check for pivot turn - update_oa_distance_and_bearing_to_destination(); - _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); - - if (!_pivot.active()) { - // pass the desired position directly to the position controller - dest_NE_ftype *= 0.01; // convert to meters - Vector2p dest_NE(dest_NE_ftype.x, dest_NE_ftype.y); - _pos_control.input_pos_target(dest_NE, dt); - - // run position controllers - _pos_control.update(dt); - } - - update_distance_and_bearing_to_destination(); - - // update_steering_and_speed - update_steering_and_speed(current_loc, dt); - - // return without calling parent AC_WPNav - return; - } else { - // this should never happen because we can only get here if we have an EKF origin - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - stop_vehicle = true; } } break; From e0a628bb8c0e33c71ec13293f606ba03bfe9bd33 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 14:29:40 +0900 Subject: [PATCH 0466/3101] AR_WPNav: add is_fast_waypoint accessor --- libraries/AR_WPNav/AR_WPNav.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index af112af426d..1696c76d207 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -103,6 +103,9 @@ class AR_WPNav { // returns true on success, false on failure bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED; + // is_fast_waypoint returns true if vehicle will not stop at destination (e.g. set_desired_location provided a next_destination) + bool is_fast_waypoint() const { return _fast_waypoint; } + // parameter var table static const struct AP_Param::GroupInfo var_info[]; From 729dfee01cf9f25a2cfafd474597d765f11941c3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 4 Jan 2022 20:37:35 +0900 Subject: [PATCH 0467/3101] AR_WPNav: pivot turns stop before turning --- libraries/AR_WPNav/AR_WPNav.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 5daed036e26..6b72178a4e0 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -499,13 +499,12 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { // handle pivot turns if (_pivot.active()) { - _cross_track_error = calc_crosstrack_error(current_loc); - _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); - _desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt); - _desired_lat_accel = 0.0f; - // decelerate to zero _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); + _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0; + _desired_lat_accel = 0.0f; + _cross_track_error = calc_crosstrack_error(current_loc); return; } From 74889f1c7af732d7e1cc7d267cdc7cba62dfee1e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Nov 2021 12:05:23 +0900 Subject: [PATCH 0468/3101] Rover: guided integrates wpnav's support of fast updates also guided implements wp_bearing, nav_bearing, crosstrack_error and get_desired_lat_accel --- Rover/Parameters.cpp | 11 ++++- Rover/Parameters.h | 3 ++ Rover/mode.h | 15 ++++++ Rover/mode_guided.cpp | 109 ++++++++++++++++++++++++++++++++++++++---- 4 files changed, 128 insertions(+), 10 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 8df6413fc9e..2ee83c7a6f7 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -637,8 +637,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f), // @Param: FS_OPTIONS - // @DisplayName: Rover Failsafe Options - // @Description: Bitmask to enable Rover failsafe options + // @DisplayName: Failsafe Options + // @Description: Bitmask to enable failsafe options // @Values: 0:None,1:Failsafe enabled in Hold mode // @Bitmask: 0:Failsafe enabled in Hold mode // @User: Advanced @@ -660,6 +660,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/APM_Control/AR_PosControl.cpp AP_SUBGROUPINFO(pos_control, "PSC", 51, ParametersG2, AR_PosControl), + // @Param: GUID_OPTIONS + // @DisplayName: Guided mode options + // @Description: Options that can be applied to change guided mode behaviour + // @Bitmask: 6:SCurves used for navigation + // @User: Advanced + AP_GROUPINFO("GUID_OPTIONS", 52, ParametersG2, guided_options, 0), + AP_GROUPEND }; diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 6f3ade2e177..da9e6ebe5fc 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -414,6 +414,9 @@ class ParametersG2 { // position controller AR_PosControl pos_control; + + // guided options bitmask + AP_Int32 guided_options; }; extern const AP_Param::Info var_info[]; diff --git a/Rover/mode.h b/Rover/mode.h index 94a45cfdb92..acc27c42fe7 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -406,6 +406,12 @@ class ModeGuided : public Mode // return if external control is allowed in this mode (Guided or Guided-within-Auto) bool in_guided_mode() const override { return true; } + // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) + float wp_bearing() const override; + float nav_bearing() const override; + float crosstrack_error() const override; + float get_desired_lat_accel() const override; + // return distance (in meters) to destination float get_distance_to_destination() const override; @@ -452,8 +458,17 @@ class ModeGuided : public Mode Guided_Stop }; + // enum for GUID_OPTIONS parameter + enum class Options : int32_t { + SCurvesUsedForNavigation = (1U << 6) + }; + bool _enter() override; + // returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping + // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) + bool use_scurves_for_navigation() const; + GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in // attitude control diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 524d0f9d596..a80527904d9 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -142,6 +142,83 @@ void ModeGuided::update() } } +// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) +float ModeGuided::wp_bearing() const +{ + switch (_guided_mode) { + case Guided_WP: + return g2.wp_nav.wp_bearing_cd() * 0.01f; + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + return 0.0f; + case Guided_Loiter: + return rover.mode_loiter.wp_bearing(); + case Guided_SteeringAndThrottle: + case Guided_Stop: + return 0.0f; + } + + // we should never reach here but just in case, return 0 + return 0.0f; +} + +float ModeGuided::nav_bearing() const +{ + switch (_guided_mode) { + case Guided_WP: + return g2.wp_nav.nav_bearing_cd() * 0.01f; + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + return 0.0f; + case Guided_Loiter: + return rover.mode_loiter.nav_bearing(); + case Guided_SteeringAndThrottle: + case Guided_Stop: + return 0.0f; + } + + // we should never reach here but just in case, return 0 + return 0.0f; +} + +float ModeGuided::crosstrack_error() const +{ + switch (_guided_mode) { + case Guided_WP: + return g2.wp_nav.crosstrack_error(); + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + return 0.0f; + case Guided_Loiter: + return rover.mode_loiter.crosstrack_error(); + case Guided_SteeringAndThrottle: + case Guided_Stop: + return 0.0f; + } + + // we should never reach here but just in case, return 0 + return 0.0f; +} + +float ModeGuided::get_desired_lat_accel() const +{ + switch (_guided_mode) { + case Guided_WP: + return g2.wp_nav.get_lat_accel(); + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + return 0.0f; + case Guided_Loiter: + return rover.mode_loiter.get_desired_lat_accel(); + case Guided_SteeringAndThrottle: + case Guided_Stop: + return 0.0f; + } + + // we should never reach here but just in case, return 0 + return 0.0f; +} + // return distance (in meters) to destination float ModeGuided::get_distance_to_destination() const { @@ -234,15 +311,24 @@ bool ModeGuided::get_desired_location(Location& destination) const // set desired location bool ModeGuided::set_desired_location(const Location &destination, Location next_destination) { - if (g2.wp_nav.set_desired_location(destination, next_destination)) { - - // handle guided specific initialisation and logging - _guided_mode = ModeGuided::Guided_WP; - send_notification = true; - rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f)); - return true; + if (use_scurves_for_navigation()) { + // use scurves for navigation + if (!g2.wp_nav.set_desired_location(destination, next_destination)) { + return false; + } + } else { + // use position controller input shaping for navigation + // this does not support object avoidance but does allow faster updates of the target + if (!g2.wp_nav.set_desired_location_expect_fast_update(destination)) { + return false; + } } - return false; + + // handle guided specific initialisation and logging + _guided_mode = ModeGuided::Guided_WP; + send_notification = true; + rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f)); + return true; } // set desired attitude @@ -353,3 +439,10 @@ bool ModeGuided::limit_breached() const // if we got this far we must be within limits return false; } + +// returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping +// scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping) +bool ModeGuided::use_scurves_for_navigation() const +{ + return ((rover.g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0); +} From 34447fa7a80231a680f05fa2943d90832254c092 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 22 Nov 2021 13:38:20 +0900 Subject: [PATCH 0469/3101] Rover: integrate AR_WPNav_OA --- Rover/Parameters.cpp | 2 +- Rover/Parameters.h | 2 +- Rover/Rover.h | 2 +- Rover/mode.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 2ee83c7a6f7..3001eea1e36 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -609,7 +609,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Group: WP_ // @Path: ../libraries/AR_WPNav/AR_WPNav.cpp - AP_SUBGROUPINFO(wp_nav, "WP_", 43, ParametersG2, AR_WPNav), + AP_SUBGROUPINFO(wp_nav, "WP_", 43, ParametersG2, AR_WPNav_OA), // @Group: SAIL_ // @Path: sailboat.cpp diff --git a/Rover/Parameters.h b/Rover/Parameters.h index da9e6ebe5fc..fa1017f13f7 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -385,7 +385,7 @@ class ParametersG2 { #endif // AP_SCRIPTING_ENABLED // waypoint navigation - AR_WPNav wp_nav; + AR_WPNav_OA wp_nav; // Sailboat functions Sailboat sailboat; diff --git a/Rover/Rover.h b/Rover/Rover.h index c919ad332a5..d9fa99fc76b 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #include // Mode Filter from Filter library diff --git a/Rover/mode.h b/Rover/mode.h index acc27c42fe7..c330aad0cd4 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include "defines.h" From 147b1284c99a49b6ceed048a363ea353e43f0ed2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 12:51:29 +0900 Subject: [PATCH 0470/3101] Rover: guided mode fix for reached_destination during wpnav --- Rover/mode_guided.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index a80527904d9..b05519dd892 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -244,7 +244,7 @@ bool ModeGuided::reached_destination() const { switch (_guided_mode) { case Guided_WP: - return _reached_destination; + return g2.wp_nav.reached_destination(); case Guided_HeadingAndSpeed: case Guided_TurnRateAndSpeed: case Guided_Loiter: From a6b22e4db9df972bec815a8c7b167607d4c64c3d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 14:12:41 +0900 Subject: [PATCH 0471/3101] Rover: guided does not set _reached_destination member --- Rover/mode_guided.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index b05519dd892..7d3d53633ac 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -337,7 +337,6 @@ void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_ // initialisation and logging _guided_mode = ModeGuided::Guided_HeadingAndSpeed; _des_att_time_ms = AP_HAL::millis(); - _reached_destination = false; // record targets _desired_yaw_cd = yaw_angle_cd; @@ -364,7 +363,6 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ // handle initialisation _guided_mode = ModeGuided::Guided_TurnRateAndSpeed; _des_att_time_ms = AP_HAL::millis(); - _reached_destination = false; // record targets _desired_yaw_rate_cds = turn_rate_cds; From c9369b05b39ed52be69393c358c598952d0b41e4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 14:12:59 +0900 Subject: [PATCH 0472/3101] Rover: auto does not set unused _reached_destination member --- Rover/mode_auto.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 67ee4828084..27cff1e894c 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -703,7 +703,6 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max); _desired_yaw_cd = desired_heading_cd; _reached_heading = false; - _reached_destination = false; _submode = Auto_HeadingAndSpeed; } From 2f6e098f23a491bd76d6060c41c2c3eb30a95bbb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 15:47:26 +0900 Subject: [PATCH 0473/3101] AR_WPNav: always use local calculation of cross track also calc_crosstrack_error may return negative values --- libraries/AR_WPNav/AR_WPNav.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 6b72178a4e0..939d12f7eaf 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -497,6 +497,8 @@ void AR_WPNav::update_distance_and_bearing_to_destination() // calculate steering and speed to drive along line from origin to destination waypoint void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { + _cross_track_error = calc_crosstrack_error(current_loc); + // handle pivot turns if (_pivot.active()) { // decelerate to zero @@ -504,7 +506,6 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0; _desired_lat_accel = 0.0f; - _cross_track_error = calc_crosstrack_error(current_loc); return; } @@ -513,7 +514,6 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) _desired_speed_limited = _pos_control.get_desired_speed(); _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); _desired_lat_accel = _pos_control.get_desired_lat_accel(); - _cross_track_error = _pos_control.get_crosstrack_error(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) @@ -561,7 +561,7 @@ float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const const Vector2f veh_from_origin = orig.get_distance_NE(current_loc); // calculate distance to target track, for reporting - return fabsf(veh_from_origin % dest_from_origin); + return veh_from_origin % dest_from_origin; } // calculate yaw change at next waypoint in degrees From 653afbeb15f216d79fb8718b1b8a2dcf2a99e08b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 14:52:06 +0900 Subject: [PATCH 0474/3101] AR_WPNav: enable_overspeed added to improve sailboat support --- libraries/AR_WPNav/AR_WPNav.cpp | 4 +++- libraries/AR_WPNav/AR_WPNav.h | 6 ++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 939d12f7eaf..51df03e2ec2 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_RADIUS_DEFAULT 2.0f +#define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -419,7 +420,8 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction); float track_velocity = curr_vel_NED.xy().dot(track_direction); // set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation. - track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); + const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f; + track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, time_scaler_dt_max); } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk float track_scaler_tc = 1.0f; diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 1696c76d207..f3b49d110c6 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -88,6 +88,11 @@ class AR_WPNav { // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) void set_turn_params(float turn_radius, bool pivot_possible); + // enable speeding up position target to catch-up with vehicles travelling faster than WP_SPEED + // designed to support sailboats that do not have precise speed control + // only supported when using SCurves and not when using position controller input shaping + void enable_overspeed(bool enable) { _overspeed_enabled = enable; } + // accessors for parameter values float get_default_speed() const { return _speed_max; } float get_radius() const { return _radius; } @@ -160,6 +165,7 @@ class AR_WPNav { float _scurve_jerk; // scurve jerk max in m/s/s/s bool _fast_waypoint; // true if vehicle will stop at the next waypoint bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint + bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle // variables held in vehicle code (for now) From ee317299fc931384a8b81e5c90cada28b0b85ca0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 15:14:25 +0900 Subject: [PATCH 0475/3101] Rover: sailboats enable wpnav's overspeed support --- Rover/sailboat.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index 7a8d1372129..c547a9cb1fa 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -141,6 +141,10 @@ void Sailboat::init() // sailboat defaults if (sail_enabled()) { rover.g2.crash_angle.set_default(0); + + // sailboats without motors may travel faster than WP_SPEED so allow waypoint navigation to + // speedup to catch the vehicle instead of asking the vehicle to slow down + rover.g2.wp_nav.enable_overspeed(motor_state != UseMotor::USE_MOTOR_ALWAYS); } if (tack_enabled()) { From d03e801b65c4cfd2fe349156fc5f642c78f66272 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 20:22:32 +0900 Subject: [PATCH 0476/3101] AR_WPNav: remove get_accel_max, get_jerk_max --- libraries/AR_WPNav/AR_WPNav.cpp | 18 ------------------ libraries/AR_WPNav/AR_WPNav.h | 6 ------ 2 files changed, 24 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 51df03e2ec2..b30a1e20b0c 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -341,24 +341,6 @@ bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destinati return true; } -// get max acceleration in m/s/s -float AR_WPNav::get_accel_max() const -{ - if (is_positive(_accel_max)) { - return _accel_max; - } - return _atc.get_accel_max(); -} - -// get max jerk in m/s/s/s -float AR_WPNav::get_jerk_max() const -{ - if (is_positive(_jerk_max)) { - return _jerk_max; - } - return get_accel_max(); -} - // calculate vehicle stopping point using current location, velocity and maximum acceleration bool AR_WPNav::get_stopping_location(Location& stopping_loc) { diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index f3b49d110c6..210816c243d 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -98,12 +98,6 @@ class AR_WPNav { float get_radius() const { return _radius; } float get_pivot_rate() const { return _pivot.get_rate_max(); } - // get max acceleration in m/s/s - float get_accel_max() const; - - // get max jerk in m/s/s/s - float get_jerk_max() const; - // calculate stopping location using current position and attitude controller provided maximum deceleration // returns true on success, false on failure bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED; From 54eee7d311e026b5009d483bb257cf832b82e1cb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 20:23:19 +0900 Subject: [PATCH 0477/3101] AR_WPNav: simplify init --- libraries/AR_WPNav/AR_WPNav.cpp | 30 ++++++++---------------------- libraries/AR_WPNav/AR_WPNav.h | 6 +----- 2 files changed, 9 insertions(+), 27 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index b30a1e20b0c..f5dac7082d0 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -99,29 +99,15 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : } // initialise waypoint controller -// speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed -// accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration -// lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration -// jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration -void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float jerk_max) +void AR_WPNav::init() { - // default max speed and accel - if (!is_positive(speed_max)) { - speed_max = _speed_max; - } - if (!is_positive(accel_max)) { - accel_max = get_accel_max(); - } - if (!is_positive(lat_accel_max)) { - lat_accel_max = _atc.get_turn_lat_accel_max(); - } - if (!is_positive(jerk_max)) { - jerk_max = get_jerk_max(); - } - _scurve_jerk = jerk_max; + // sanity check parameters + const float speed_max = MAX(0, _speed_max); + const float accel_max = is_positive(_accel_max) ? _accel_max : _atc.get_accel_max(); + const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; // initialise position controller - _pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max); + _pos_control.set_limits(speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max); _scurve_prev_leg.init(); _scurve_this_leg.init(); @@ -189,7 +175,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location { // re-initialise if inactive, previous destination has been interrupted or different controller was used if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) { - init(0,0,0,0); + init(); } // shift this leg to previous leg @@ -322,7 +308,7 @@ bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destinati { // initialise if not active if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) { - init(0,0,0,0); + init(); } // initialise some variables diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 210816c243d..df4c7ac6fa6 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -14,11 +14,7 @@ class AR_WPNav { AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control); // initialise waypoint controller - // speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed - // accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration - // lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration - // jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration - void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max); + void init(); // update navigation virtual void update(float dt); From 537874c075955c5372b04aed7fa8123341f3cf61 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 20:24:16 +0900 Subject: [PATCH 0478/3101] AR_WPNav: jerk_max maintained in position controller --- libraries/AR_WPNav/AR_WPNav.cpp | 8 ++++---- libraries/AR_WPNav/AR_WPNav.h | 1 - 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index f5dac7082d0..64c560203de 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -220,7 +220,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), _pos_control.get_accel_max(), // vertical accel (not used) 1.0, // jerk time - _scurve_jerk); + _pos_control.get_jerk_max()); } // handle next destination @@ -247,7 +247,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), _pos_control.get_accel_max(), // vertical accel (not used) 1.0, // jerk time - _scurve_jerk); + _pos_control.get_jerk_max()); // next destination provided so fast waypoint _fast_waypoint = true; @@ -393,8 +393,8 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk float track_scaler_tc = 1.0f; - if (is_positive(_scurve_jerk)) { - track_scaler_tc = _pos_control.get_accel_max() / _scurve_jerk; + if (is_positive(_pos_control.get_jerk_max())) { + track_scaler_tc = _pos_control.get_accel_max() / _pos_control.get_jerk_max(); } _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index df4c7ac6fa6..06c4ab616e3 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -152,7 +152,6 @@ class AR_WPNav { SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory SCurve _scurve_this_leg; // current scurve trajectory SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory - float _scurve_jerk; // scurve jerk max in m/s/s/s bool _fast_waypoint; // true if vehicle will stop at the next waypoint bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED From a91025fa163dd42d8f541ecb7ed60f5dd3dc383b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 5 Jan 2022 20:25:25 +0900 Subject: [PATCH 0479/3101] AR_WPNav: remove unused speed_min --- libraries/AR_WPNav/AR_WPNav.cpp | 23 +---------------------- libraries/AR_WPNav/AR_WPNav.h | 5 ----- 2 files changed, 1 insertion(+), 27 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 64c560203de..87cf46d94f4 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -53,16 +53,7 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = { // 4 was PIVOT_ANGLE // 5 was PIVOT_RATE - - // @Param: SPEED_MIN - // @DisplayName: Waypoint speed minimum - // @Description: Vehicle will not slow below this speed for corners. Should be set to boat's plane speed. Does not apply to pivot turns. - // @Units: m/s - // @Range: 0 100 - // @Increment: 0.1 - // @User: Standard - AP_GROUPINFO("SPEED_MIN", 6, AR_WPNav, _speed_min, 0), - + // 6 was SPEED_MIN // 7 was PIVOT_DELAY // @Group: PIVOT_ @@ -493,18 +484,6 @@ void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible) _pivot.enable(pivot_possible); } -// adjust speed to ensure it does not fall below value held in SPEED_MIN -// desired_speed should always be positive (or zero) -void AR_WPNav::apply_speed_min(float &desired_speed) const -{ - if (!is_positive(_speed_min) || (_speed_min > _speed_max)) { - return; - } - - // ensure speed does not fall below minimum - desired_speed = MAX(desired_speed, _speed_min); -} - // calculate the crosstrack error float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const { diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 06c4ab616e3..e167e0b03ff 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -121,10 +121,6 @@ class AR_WPNav { // calculate steering and speed to drive along line from origin to destination waypoint void update_steering_and_speed(const Location ¤t_loc, float dt); - // adjust speed to ensure it does not fall below value held in SPEED_MIN - // desired_speed should always be positive (or zero) - void apply_speed_min(float &desired_speed) const; - // calculate the crosstrack error (does not rely on L1 controller) float calc_crosstrack_error(const Location& current_loc) const; @@ -138,7 +134,6 @@ class AR_WPNav { // parameters AP_Float _speed_max; // target speed between waypoints in m/s - AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached AR_PivotTurn _pivot; // pivot turn controller AP_Float _accel_max; // max acceleration. If zero then attitude controller's specified max accel is used From 199608528221d8799a621fe323f7e6557e2010de Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 09:01:11 +0900 Subject: [PATCH 0480/3101] AR_WPNav: init accepts speed max also remove unused set_desired_speed_to_default also init previous leg in set_desired_location init accepts speed but inforces minimum --- libraries/AR_WPNav/AR_WPNav.cpp | 48 ++++++++++++++++++++++++--------- libraries/AR_WPNav/AR_WPNav.h | 10 +++---- 2 files changed, 40 insertions(+), 18 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 87cf46d94f4..15d36c787f5 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f +#define AR_WPNAV_SPEED_MIN 0.05f // minimum speed between waypoints in m/s #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED @@ -89,12 +90,16 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : AP_Param::setup_object_defaults(this, var_info); } -// initialise waypoint controller -void AR_WPNav::init() +// initialise waypoint controller. speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed) +void AR_WPNav::init(float speed_max) { - // sanity check parameters - const float speed_max = MAX(0, _speed_max); - const float accel_max = is_positive(_accel_max) ? _accel_max : _atc.get_accel_max(); + // determine max speed, acceleration and jerk + if (!is_positive(speed_max)) { + speed_max = _speed_max; + } + speed_max = MAX(AR_WPNAV_SPEED_MIN, speed_max); + const float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); + const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max; const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; // initialise position controller @@ -114,12 +119,8 @@ void AR_WPNav::init() _pivot_at_next_wp = false; // initialise origin and destination to stopping point - Location stopping_loc; - if (get_stopping_location(stopping_loc)) { - _origin = _destination = stopping_loc; - } else { - // handle not current location - } + _orig_and_dest_valid = false; + set_origin_and_destination_to_stopping_point(); } // update navigation @@ -166,7 +167,13 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location { // re-initialise if inactive, previous destination has been interrupted or different controller was used if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) { - init(); + if (!set_origin_and_destination_to_stopping_point()) { + return false; + } + // clear scurves + _scurve_prev_leg.init(); + _scurve_this_leg.init(); + _scurve_next_leg.init(); } // shift this leg to previous leg @@ -299,7 +306,9 @@ bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destinati { // initialise if not active if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) { - init(); + if (!set_origin_and_destination_to_stopping_point()) { + return false; + } } // initialise some variables @@ -540,3 +549,16 @@ void AR_WPNav::init_pos_control_if_necessary() } } } + +// set origin and destination to stopping point +bool AR_WPNav::set_origin_and_destination_to_stopping_point() +{ + // initialise origin and destination to stopping point + Location stopping_loc; + if (!get_stopping_location(stopping_loc)) { + return false; + } + _origin = _destination = stopping_loc; + _orig_and_dest_valid = true; + return true; +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index e167e0b03ff..e612fc11ea2 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -13,8 +13,8 @@ class AR_WPNav { // constructor AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control); - // initialise waypoint controller - void init(); + // initialise waypoint controller. speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed) + void init(float speed_max = 0); // update navigation virtual void update(float dt); @@ -25,9 +25,6 @@ class AR_WPNav { // set desired speed in m/s void set_desired_speed(float speed) { _desired_speed = MAX(speed, 0.0f); } - // restore desired speed to default from parameter value - void set_desired_speed_to_default() { _desired_speed = _speed_max; } - // execute the mission in reverse (i.e. drive backwards to destination) bool get_reversed() const { return _reversed; } void set_reversed(bool reversed) { _reversed = reversed; } @@ -132,6 +129,9 @@ class AR_WPNav { // this should be called before updating the position controller with new targets but after the EKF has a good position estimate void init_pos_control_if_necessary(); + // set origin and destination to stopping point + bool set_origin_and_destination_to_stopping_point(); + // parameters AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached From 394a5fd2fcc97a33f56cd0ef80b2ea48d00a3a5e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 09:43:01 +0900 Subject: [PATCH 0481/3101] AR_WPNav: get/set_desired_speed renamed to get/set_speed_max set_speed_max updates position controller limits and triggers recalculation of scurves local _desired_speed member is no longer required because max speed is held in position controller --- libraries/AR_WPNav/AR_WPNav.cpp | 24 ++++++++++++++++++++++++ libraries/AR_WPNav/AR_WPNav.h | 10 ++++------ 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 15d36c787f5..30eb35832b4 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -161,6 +161,30 @@ void AR_WPNav::update(float dt) update_steering_and_speed(current_loc, dt); } +// set maximum speed in m/s. returns true on success +// this should not be called at more than 3hz or else SCurve path planning may not advance properly +bool AR_WPNav::set_speed_max(float speed_max) +{ + // range check target speed + if (speed_max < AR_WPNAV_SPEED_MIN) { + return false; + } + + // ignore calls that do not change the speed + if (is_equal(speed_max, _pos_control.get_speed_max())) { + return true; + } + + // update position controller max speed + _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); + + // change track speed + _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); + _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); + + return true; +} + // set desired location and (optionally) next_destination // next_destination should be provided if known to allow smooth cornering bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination) diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index e612fc11ea2..594e8c0bb33 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -19,11 +19,10 @@ class AR_WPNav { // update navigation virtual void update(float dt); - // return desired speed - float get_desired_speed() const { return _desired_speed; } - - // set desired speed in m/s - void set_desired_speed(float speed) { _desired_speed = MAX(speed, 0.0f); } + // get or set maximum speed in m/s + // set_speed_max should not be called at more than 3hz or else SCurve path planning may not advance properly + float get_speed_max() const { return _pos_control.get_speed_max(); } + bool set_speed_max(float speed_max); // execute the mission in reverse (i.e. drive backwards to destination) bool get_reversed() const { return _reversed; } @@ -167,7 +166,6 @@ class AR_WPNav { } _nav_control_type; // navigation controller that should be used to travel from _origin to _destination // main outputs from navigation library - float _desired_speed; // desired speed in m/s float _desired_speed_limited; // desired speed (above) but accel/decel limited float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise) float _desired_lat_accel; // desired lateral acceleration (for reporting only) From c6dffb990fe70155fe3c3b8009b7bd9016d5d5a9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 13:07:03 +0900 Subject: [PATCH 0482/3101] AR_WPNav: add set_nudge_speed_max allows pilot to override speed_max from RC input --- libraries/AR_WPNav/AR_WPNav.cpp | 63 ++++++++++++++++++++++++--------- libraries/AR_WPNav/AR_WPNav.h | 17 +++++++-- 2 files changed, 62 insertions(+), 18 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 30eb35832b4..ec7da187e07 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -27,6 +27,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_TIMEOUT_MS 100 #define AR_WPNAV_SPEED_DEFAULT 2.0f #define AR_WPNAV_SPEED_MIN 0.05f // minimum speed between waypoints in m/s +#define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED @@ -94,16 +95,18 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) : void AR_WPNav::init(float speed_max) { // determine max speed, acceleration and jerk - if (!is_positive(speed_max)) { - speed_max = _speed_max; + if (is_positive(speed_max)) { + _base_speed_max = speed_max; + } else { + _base_speed_max = _speed_max; } - speed_max = MAX(AR_WPNAV_SPEED_MIN, speed_max); + _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _base_speed_max); const float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max; const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; // initialise position controller - _pos_control.set_limits(speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max); + _pos_control.set_limits(_base_speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max); _scurve_prev_leg.init(); _scurve_this_leg.init(); @@ -121,6 +124,9 @@ void AR_WPNav::init(float speed_max) // initialise origin and destination to stopping point _orig_and_dest_valid = false; set_origin_and_destination_to_stopping_point(); + + // initialise nudge speed to zero + set_nudge_speed_max(0); } // update navigation @@ -145,6 +151,9 @@ void AR_WPNav::update(float dt) update_distance_and_bearing_to_destination(); + // handle change in max speed + update_speed_max(); + // advance target along path unless vehicle is pivoting if (!_pivot.active()) { switch (_nav_control_type) { @@ -170,21 +179,17 @@ bool AR_WPNav::set_speed_max(float speed_max) return false; } - // ignore calls that do not change the speed - if (is_equal(speed_max, _pos_control.get_speed_max())) { - return true; - } - - // update position controller max speed - _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); - - // change track speed - _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); - _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); - + _base_speed_max = speed_max; return true; } +// set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max +// nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing +void AR_WPNav::set_nudge_speed_max(float nudge_speed_max) +{ + _nudge_speed_max = nudge_speed_max; +} + // set desired location and (optionally) next_destination // next_destination should be provided if known to allow smooth cornering bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination) @@ -586,3 +591,29 @@ bool AR_WPNav::set_origin_and_destination_to_stopping_point() _orig_and_dest_valid = true; return true; } + +// check for changes in _base_speed_max or _nudge_speed_max +// updates position controller limits and recalculate scurve path if required +void AR_WPNav::update_speed_max() +{ + const float speed_max = MAX(_base_speed_max, _nudge_speed_max); + + // ignore calls that do not change the speed + if (is_equal(speed_max, _pos_control.get_speed_max())) { + return; + } + + // protect against rapid updates + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_speed_update_ms < AR_WPNAV_SPEED_UPDATE_MIN_MS) { + return; + } + _last_speed_update_ms = now_ms; + + // update position controller max speed + _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max()); + + // change track speed + _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); + _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max()); +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 594e8c0bb33..780c50481c2 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -20,10 +20,14 @@ class AR_WPNav { virtual void update(float dt); // get or set maximum speed in m/s - // set_speed_max should not be called at more than 3hz or else SCurve path planning may not advance properly - float get_speed_max() const { return _pos_control.get_speed_max(); } + // if set_speed_max is called in rapid succession changes in speed may be delayed by up to 0.5sec + float get_speed_max() const { return _base_speed_max; } bool set_speed_max(float speed_max); + // set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max + // nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing + void set_nudge_speed_max(float nudge_speed_max); + // execute the mission in reverse (i.e. drive backwards to destination) bool get_reversed() const { return _reversed; } void set_reversed(bool reversed) { _reversed = reversed; } @@ -131,6 +135,10 @@ class AR_WPNav { // set origin and destination to stopping point bool set_origin_and_destination_to_stopping_point(); + // check for changes in _base_speed_max or _nudge_speed_max + // updates position controller limits and recalculate scurve path if required + void update_speed_max(); + // parameters AP_Float _speed_max; // target speed between waypoints in m/s AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached @@ -165,6 +173,11 @@ class AR_WPNav { NAV_PSC_INPUT_SHAPING // position controller input shaping used for navigation } _nav_control_type; // navigation controller that should be used to travel from _origin to _destination + // speed_max handling + float _base_speed_max; // speed max (in m/s) derived from parameters or passed into init + float _nudge_speed_max; // "nudge" speed max (in m/s) normally from the pilot. has no effect if less than _base_speed_max. always positive. + uint32_t _last_speed_update_ms; // system time that speed_max was last update. used to ensure speed_max is not update too quickly + // main outputs from navigation library float _desired_speed_limited; // desired speed (above) but accel/decel limited float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise) From a1b9fe50974e4bc9f0d586f64978b10be1555d5a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Jan 2022 09:20:39 +0900 Subject: [PATCH 0483/3101] AR_WPNav: jerk time reduced to 0.1 sec --- libraries/AR_WPNav/AR_WPNav.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index ec7da187e07..df92b124c8c 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -30,6 +30,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED +#define AR_WPNAV_JERK_TIME_SEC 0.1 // jerk time (the time taken for jerk to climb to its maximum) is hard-coded to 0.1 const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -246,7 +247,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), // speed down (not used) MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), _pos_control.get_accel_max(), // vertical accel (not used) - 1.0, // jerk time + AR_WPNAV_JERK_TIME_SEC, // jerk time _pos_control.get_jerk_max()); } @@ -273,7 +274,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), // speed down (not used) MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), _pos_control.get_accel_max(), // vertical accel (not used) - 1.0, // jerk time + AR_WPNAV_JERK_TIME_SEC, // jerk time _pos_control.get_jerk_max()); // next destination provided so fast waypoint From 5d96804ef361b4cee60f32ad96a939b3503d4647 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 26 Jan 2022 15:15:26 +0900 Subject: [PATCH 0484/3101] AR_WPNav: add protection against zero accel max --- libraries/AR_WPNav/AR_WPNav.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index df92b124c8c..3dd77c4326e 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -31,6 +31,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED #define AR_WPNAV_JERK_TIME_SEC 0.1 // jerk time (the time taken for jerk to climb to its maximum) is hard-coded to 0.1 +#define AR_WPNAV_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -102,7 +103,11 @@ void AR_WPNav::init(float speed_max) _base_speed_max = _speed_max; } _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _base_speed_max); - const float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); + float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max()); + if (!is_positive(atc_accel_max)) { + // accel_max of zero means no limit so use maximum acceleration + atc_accel_max = AR_WPNAV_ACCEL_MAX; + } const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max; const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max; From a5638f5699edeff9417774f9c7510020e961aa29 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 09:01:49 +0900 Subject: [PATCH 0485/3101] Rover: auto, guided, rtl call wpnav.init instead of set-desired-speed also smart-rtl --- Rover/mode_auto.cpp | 4 ++-- Rover/mode_guided.cpp | 4 ++-- Rover/mode_rtl.cpp | 10 +++------- Rover/mode_smart_rtl.cpp | 10 +++------- 4 files changed, 10 insertions(+), 18 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 27cff1e894c..38e41527385 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -11,8 +11,8 @@ bool ModeAuto::_enter() return false; } - // initialise waypoint speed - g2.wp_nav.set_desired_speed_to_default(); + // initialise waypoint navigation library + g2.wp_nav.init(); // other initialisation auto_triggered = false; diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 7d3d53633ac..20575e41536 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -12,8 +12,8 @@ bool ModeGuided::_enter() start_stop(); } - // initialise waypoint speed - g2.wp_nav.set_desired_speed_to_default(); + // initialise waypoint navigation library + g2.wp_nav.init(); send_notification = false; diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp index 9da8bc5c458..fe9d4200f57 100644 --- a/Rover/mode_rtl.cpp +++ b/Rover/mode_rtl.cpp @@ -8,6 +8,9 @@ bool ModeRTL::_enter() return false; } + // initialise waypoint navigation library + g2.wp_nav.init(MAX(0, g2.rtl_speed)); + // set target to the closest rally point or home #if AP_RALLY == ENABLED if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) { @@ -20,13 +23,6 @@ bool ModeRTL::_enter() } #endif - // initialise waypoint speed - if (is_positive(g2.rtl_speed)) { - g2.wp_nav.set_desired_speed(g2.rtl_speed); - } else { - g2.wp_nav.set_desired_speed_to_default(); - } - send_notification = true; _loitering = false; return true; diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp index f1e76697ac1..4112e906c37 100644 --- a/Rover/mode_smart_rtl.cpp +++ b/Rover/mode_smart_rtl.cpp @@ -14,18 +14,14 @@ bool ModeSmartRTL::_enter() return false; } + // initialise waypoint navigation library + g2.wp_nav.init(MAX(0, g2.rtl_speed)); + // set desired location to reasonable stopping point if (!g2.wp_nav.set_desired_location_to_stopping_location()) { return false; } - // initialise waypoint speed - if (is_positive(g2.rtl_speed)) { - g2.wp_nav.set_desired_speed(g2.rtl_speed); - } else { - g2.wp_nav.set_desired_speed_to_default(); - } - // init state smart_rtl_state = SmartRTL_WaitForPathCleanup; _loitering = false; From 5341070af41e617500ea806b09fe9ae222c57b2c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 09:43:58 +0900 Subject: [PATCH 0486/3101] Rover: integrate rename of AR_WPNav::set_speed_max range checks are no longer required because they are implemented within AR_WPNav --- Rover/mode_auto.cpp | 6 +----- Rover/mode_guided.cpp | 8 ++------ Rover/mode_rtl.cpp | 6 +----- Rover/mode_smart_rtl.cpp | 6 +----- 4 files changed, 5 insertions(+), 21 deletions(-) diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 38e41527385..5cd5182d236 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -250,11 +250,7 @@ bool ModeAuto::set_desired_speed(float speed) switch (_submode) { case Auto_WP: case Auto_Stop: - if (!is_negative(speed)) { - g2.wp_nav.set_desired_speed(speed); - return true; - } - return false; + return g2.wp_nav.set_speed_max(speed); case Auto_HeadingAndSpeed: _desired_speed = speed; return true; diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index 20575e41536..0efac5af406 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -262,11 +262,7 @@ bool ModeGuided::set_desired_speed(float speed) { switch (_guided_mode) { case Guided_WP: - if (!is_negative(speed)) { - g2.wp_nav.set_desired_speed(speed); - return true; - } - return false; + return g2.wp_nav.set_speed_max(speed); case Guided_HeadingAndSpeed: case Guided_TurnRateAndSpeed: // speed is set from mavlink message @@ -327,7 +323,7 @@ bool ModeGuided::set_desired_location(const Location &destination, Location next // handle guided specific initialisation and logging _guided_mode = ModeGuided::Guided_WP; send_notification = true; - rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f)); + rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f)); return true; } diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp index fe9d4200f57..53ebefc0ebc 100644 --- a/Rover/mode_rtl.cpp +++ b/Rover/mode_rtl.cpp @@ -81,9 +81,5 @@ bool ModeRTL::reached_destination() const // set desired speed in m/s bool ModeRTL::set_desired_speed(float speed) { - if (is_negative(speed)) { - return false; - } - g2.wp_nav.set_desired_speed(speed); - return true; + return g2.wp_nav.set_speed_max(speed); } diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp index 4112e906c37..62c3556ea4a 100644 --- a/Rover/mode_smart_rtl.cpp +++ b/Rover/mode_smart_rtl.cpp @@ -127,11 +127,7 @@ bool ModeSmartRTL::get_desired_location(Location& destination) const // set desired speed in m/s bool ModeSmartRTL::set_desired_speed(float speed) { - if (is_negative(speed)) { - return false; - } - g2.wp_nav.set_desired_speed(speed); - return true; + return g2.wp_nav.set_speed_max(speed); } // save current position for use by the smart_rtl flight mode From aea6b48d702d0e9a3e8e01234e8bc6ddf82a595f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 Jan 2022 13:08:08 +0900 Subject: [PATCH 0487/3101] Rover: navigate_to_waypoint supports integrates wpnav set_nuidge_speed_max --- Rover/mode.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 00a8d894c56..94ba81013ce 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -420,14 +420,19 @@ float Mode::calc_speed_nudge(float target_speed, bool reversed) // this function updates _distance_to_destination void Mode::navigate_to_waypoint() { + // apply speed nudge from pilot + // calc_speed_nudge's "desired_speed" argument should be negative when vehicle is reversing + // AR_WPNav nudge_speed_max argu,ent should always be positive even when reversing + const float calc_nudge_input_speed = g2.wp_nav.get_speed_max() * (g2.wp_nav.get_reversed() ? -1.0 : 1.0); + const float nudge_speed_max = calc_speed_nudge(calc_nudge_input_speed, g2.wp_nav.get_reversed()); + g2.wp_nav.set_nudge_speed_max(fabsf(nudge_speed_max)); + // update navigation controller g2.wp_nav.update(rover.G_Dt); _distance_to_destination = g2.wp_nav.get_distance_to_destination(); - // pass speed to throttle controller after applying nudge from pilot - float desired_speed = g2.wp_nav.get_speed(); - desired_speed = calc_speed_nudge(desired_speed, g2.wp_nav.get_reversed()); - calc_throttle(desired_speed, true); + // pass desired speed to throttle controller + calc_throttle(g2.wp_nav.get_speed(), true); float desired_heading_cd = g2.wp_nav.oa_wp_bearing_cd(); if (g2.sailboat.use_indirect_route(desired_heading_cd)) { From f7a33a8900c4643df2652247977d8833e17b350c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Jan 2022 16:50:13 +0900 Subject: [PATCH 0488/3101] Rover: navigate-to-waypoint leaves simple avoidance to position controller also navigate-to-waypoint may trigger tacking --- Rover/mode.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 94ba81013ce..8f6a184cae7 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -431,8 +431,15 @@ void Mode::navigate_to_waypoint() g2.wp_nav.update(rover.G_Dt); _distance_to_destination = g2.wp_nav.get_distance_to_destination(); + // sailboats trigger tack if simple avoidance becomes active + if (g2.sailboat.tack_enabled() && g2.avoid.limits_active()) { + // we are a sailboat trying to avoid fence, try a tack + rover.control_mode->handle_tack_request(); + } + // pass desired speed to throttle controller - calc_throttle(g2.wp_nav.get_speed(), true); + // do not do simple avoidance because this is already handled in the position controller + calc_throttle(g2.wp_nav.get_speed(), false); float desired_heading_cd = g2.wp_nav.oa_wp_bearing_cd(); if (g2.sailboat.use_indirect_route(desired_heading_cd)) { From 995ff30cd698112fb6d1748c5b7c77dc9a7d67f6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Jan 2022 16:48:52 +0900 Subject: [PATCH 0489/3101] AC_Avoid: enable slide behaviour for rover --- libraries/AC_Avoidance/AC_Avoid.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 59c05576a50..b77d6e3934e 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -67,12 +67,12 @@ const AP_Param::GroupInfo AC_Avoid::var_info[] = { // @User: Standard AP_GROUPINFO("MARGIN", 4, AC_Avoid, _margin, 2.0f), - // @Param{Copter}: BEHAVE + // @Param{Copter, Rover}: BEHAVE // @DisplayName: Avoidance behaviour // @Description: Avoidance behaviour (slide or stop) // @Values: 0:Slide,1:Stop // @User: Standard - AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), + AP_GROUPINFO_FRAME("BEHAVE", 5, AC_Avoid, _behavior, AP_AVOID_BEHAVE_DEFAULT, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER | AP_PARAM_FRAME_ROVER), // @Param: BACKUP_SPD // @DisplayName: Avoidance maximum backup speed From 8ff6972b7e2ded580ec77227822cd560bd50da4c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Nov 2021 10:30:22 +0900 Subject: [PATCH 0490/3101] Tools: rover autotest DriveRTL timeout increased by 10sec --- Tools/autotest/rover.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index fb10d75c032..b61ef4de733 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -547,7 +547,7 @@ def drive_rtl_mission(self, timeout=120): (m.wp_dist, wp_dist_min,)) # wait for mission to complete - self.wait_statustext("Mission Complete", timeout=60) + self.wait_statustext("Mission Complete", timeout=70) # the EKF doesn't pull us down to 0 speed: self.wait_groundspeed(0, 0.5, timeout=600) From ea5e3a8ae06d7b08951129ac718a9abed34924f8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Nov 2021 10:47:16 +0900 Subject: [PATCH 0491/3101] Tools: rover autotests send position-target-global-int more slowly SCurves do not work with very fast changes of target --- Tools/autotest/rover.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index b61ef4de733..9fc2dcc1ce4 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -3590,7 +3590,7 @@ def wait_location_sending_target(self, loc, target_system=1, target_component=1, now = self.get_sim_time_cached() if now - tstart > timeout: raise AutoTestTimeoutException("Did not get to location") - if now - last_sent > 1: + if now - last_sent > 10: last_sent = now self.mav.mav.set_position_target_global_int_send( 0, @@ -3645,7 +3645,7 @@ def drive_somewhere_breach_boundary_and_rtl(self, loc, target_system=1, target_c now = self.get_sim_time_cached() if now - tstart > timeout: raise NotAchievedException("Did not breach boundary + RTL") - if now - last_sent > 1: + if now - last_sent > 10: last_sent = now self.mav.mav.set_position_target_global_int_send( 0, @@ -3710,7 +3710,7 @@ def drive_somewhere_stop_at_boundary(self, now = self.get_sim_time_cached() if now - tstart > timeout: raise NotAchievedException("Did not arrive and stop at boundary") - if now - last_sent > 1: + if now - last_sent > 10: last_sent = now self.mav.mav.set_position_target_global_int_send( 0, From b6af126514ea23cf5607dbbb824a563b82ef8bd0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 4 Jan 2022 13:49:33 +0900 Subject: [PATCH 0492/3101] Tools: relax rover sprayer test vehicle's speed is slightly lower when using SCurve navigation --- Tools/autotest/rover.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 9fc2dcc1ce4..6eca1fc0802 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -310,7 +310,7 @@ def test_sprayer(self): self.change_mode("AUTO") # self.send_debug_trap() self.progress("Waiting for sprayer to start") - self.wait_servo_channel_value(pump_ch, 1300, timeout=60, comparator=operator.gt) + self.wait_servo_channel_value(pump_ch, 1250, timeout=60, comparator=operator.gt) self.progress("Waiting for sprayer to stop") self.wait_servo_channel_value(pump_ch, pump_ch_min, timeout=120) From 74df1c0b2256214550b6b51fda44fa14368a78d3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 9 Feb 2022 12:07:14 +0900 Subject: [PATCH 0493/3101] AR_PosControl: reduce default I term to zero --- libraries/APM_Control/AR_PosControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index ac868d47776..4317915439b 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal; #define AR_POSCON_TIMEOUT_MS 100 // timeout after 0.1 sec #define AR_POSCON_POS_P 0.2f // default position P gain #define AR_POSCON_VEL_P 1.0f // default velocity P gain -#define AR_POSCON_VEL_I 0.2f // default velocity I gain +#define AR_POSCON_VEL_I 0.0f // default velocity I gain #define AR_POSCON_VEL_D 0.0f // default velocity D gain #define AR_POSCON_VEL_FF 0.0f // default velocity FF gain #define AR_POSCON_VEL_IMAX 1.0f // default velocity IMAX From c233f114bd10d8753d8b46ed62eeadcc859a4f25 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 22 Feb 2022 14:36:10 +0900 Subject: [PATCH 0494/3101] AR_PosControl: fixup logging to record desired and target velocity and acceleration --- libraries/APM_Control/AR_PosControl.cpp | 92 ++++++++++++------------- libraries/APM_Control/AR_PosControl.h | 12 ++-- 2 files changed, 53 insertions(+), 51 deletions(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 4317915439b..fad162d756d 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -135,43 +135,43 @@ void AR_PosControl::update(float dt) _pid_vel.set_dt(dt); // calculate position error and convert to desired velocity - Vector2f des_vel_NE; + _vel_target.zero(); if (_pos_target_valid) { Vector2p pos_target = _pos_target; - des_vel_NE = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); + _vel_target = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); } // calculation velocity error - if (_vel_target_valid) { + if (_vel_desired_valid) { // add target velocity to desired velocity from position error - des_vel_NE += _vel_target; + _vel_target += _vel_desired; } // limit velocity to maximum speed - des_vel_NE.limit_length(get_speed_max()); + _vel_target.limit_length(get_speed_max()); // Limit the velocity to prevent fence violations bool backing_up = false; AC_Avoid *avoid = AP::ac_avoid(); if (avoid != nullptr) { - Vector3f vel_3d_cms{des_vel_NE.x * 100.0f, des_vel_NE.y * 100.0f, 0.0f}; + Vector3f vel_3d_cms{_vel_target.x * 100.0f, _vel_target.y * 100.0f, 0.0f}; const float accel_max_cmss = MIN(_accel_max, _lat_accel_max) * 100.0; avoid->adjust_velocity(vel_3d_cms, backing_up, _p_pos.kP(), accel_max_cmss, _p_pos.kP(), accel_max_cmss, dt); - des_vel_NE.x = vel_3d_cms.x * 0.01; - des_vel_NE.y = vel_3d_cms.y * 0.01; + _vel_target.x = vel_3d_cms.x * 0.01; + _vel_target.y = vel_3d_cms.y * 0.01; } // calculate desired acceleration // To-Do: fixup _limit_vel used below - Vector2f des_accel_NE = _pid_vel.update_all(des_vel_NE, curr_vel_NED.xy(), _limit_vel); - if (_accel_target_valid) { - des_accel_NE += _accel_target; + _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), _limit_vel); + if (_accel_desired_valid) { + _accel_target += _accel_desired; } // convert desired acceleration to desired forward-back speed, desired lateral speed and desired turn rate // rotate acceleration into body frame using current heading - const Vector2f des_accel_FR = AP::ahrs().earth_to_body2D(des_accel_NE); + const Vector2f accel_target_FR = AP::ahrs().earth_to_body2D(_accel_target); // calculate minimum turn speed which is the max speed the vehicle could turn through the corner // given the vehicle's turn radius and half its max lateral acceleration @@ -179,22 +179,22 @@ void AR_PosControl::update(float dt) float turn_speed_min = MAX(safe_sqrt(_atc.get_turn_lat_accel_max() * 0.5 * _turn_radius), 0); // rotate target velocity from earth-frame to body frame - const Vector2f des_vel_FR = AP::ahrs().earth_to_body2D(des_vel_NE); + const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target); // desired speed is normally the forward component (only) of the target velocity // but we do not let it fall below the minimum turn speed unless the vehicle is slowing down - const float abs_des_speed_min = MIN(des_vel_NE.length(), turn_speed_min); + const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); float des_speed; if (_reversed != backing_up) { // if reversed or backing up desired speed will be negative - des_speed = MIN(-abs_des_speed_min, des_vel_FR.x); + des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); } else { - des_speed = MAX(abs_des_speed_min, des_vel_FR.x); + des_speed = MAX(abs_des_speed_min, vel_target_FR.x); } _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); // calculate turn rate from desired lateral acceleration - _desired_lat_accel = des_accel_FR.y; + _desired_lat_accel = accel_target_FR.y; _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); } @@ -242,8 +242,10 @@ bool AR_PosControl::init() _pos_target.y = pos_NE.y; // set target velocity and acceleration - _vel_target = vel_NED.xy(); - _accel_target = AP::ahrs().get_accel_ef_blended().xy(); + _vel_desired = vel_NED.xy(); + _vel_target.zero(); + _accel_desired = AP::ahrs().get_accel_ef_blended().xy(); + _accel_target.zero(); // clear reversed setting _reversed = false; @@ -260,37 +262,37 @@ bool AR_PosControl::init() void AR_PosControl::input_pos_target(const Vector2p &pos, float dt) { // adjust target position, velocity and acceleration forward by dt - update_pos_vel_accel_xy(_pos_target, _vel_target, _accel_target, dt, Vector2f(), Vector2f(), Vector2f()); + update_pos_vel_accel_xy(_pos_target, _vel_desired, _accel_desired, dt, Vector2f(), Vector2f(), Vector2f()); // call shape_pos_vel_accel_xy to pull target towards final destination Vector2f vel; Vector2f accel; const float accel_max = MIN(_accel_max, _lat_accel_max); - shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_target, _accel_target, + shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_desired, _accel_desired, _speed_max, accel_max, _jerk_max, dt, false); - // set flags so update will consume target position, velocity and acceleration + // set flags so update will consume target position, desired velocity and desired acceleration _pos_target_valid = true; - _vel_target_valid = true; - _accel_target_valid = true; + _vel_desired_valid = true; + _accel_desired_valid = true; } -// set position, velocity and acceleration targets +// set target position, desired velocity and acceleration. These should be from an externally created path and are not "input shaped" void AR_PosControl::set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel) { _pos_target = pos; - _vel_target = vel; - _accel_target = accel; + _vel_desired = vel; + _accel_desired = accel; _pos_target_valid = true; - _vel_target_valid = true; - _accel_target_valid = true; + _vel_desired_valid = true; + _accel_desired_valid = true; } // returns desired velocity vector (i.e. feed forward) in cm/s in lat and lon direction Vector2f AR_PosControl::get_desired_velocity() const { - if (_vel_target_valid) { - return _vel_target; + if (_vel_desired_valid) { + return _vel_desired; } return Vector2f(); } @@ -298,8 +300,8 @@ Vector2f AR_PosControl::get_desired_velocity() const // return desired acceleration vector in m/s in lat and lon direction Vector2f AR_PosControl::get_desired_accel() const { - if (_accel_target_valid) { - return _accel_target; + if (_accel_desired_valid) { + return _accel_desired; } return Vector2f(); } @@ -333,28 +335,26 @@ void AR_PosControl::write_log() } // get acceleration - const Vector3f curr_accel_NED;// = AP::ahrs().get_accel_ef_blended; + const Vector3f curr_accel_NED = AP::ahrs().get_accel_ef_blended() * 100.0; - // convert position, velocity and accel targets to required format + // convert position to required format Vector2f pos_target_2d_cm = get_pos_target().tofloat() * 100.0; - Vector2f vel_target_2d_cm = get_desired_velocity() * 100.0; - Vector2f accel_target_2d_cm = get_desired_accel() * 100.0; AP::logger().Write_PSCN(pos_target_2d_cm.x, // position target curr_pos_NED.x * 100.0, // position - 0.0, // desired velocity - vel_target_2d_cm.x, // target velocity + _vel_desired.x * 100.0, // desired velocity + _vel_target.x * 100.0, // target velocity curr_vel_NED.x * 100.0, // velocity - 0.0, // desired accel - accel_target_2d_cm.x, // target accel + _accel_desired.x * 100.0, // desired accel + _accel_target.x * 100.0, // target accel curr_accel_NED.x); // accel AP::logger().Write_PSCE(pos_target_2d_cm.y, // position target curr_pos_NED.y * 100.0, // position - 0.0, // desired velocity - vel_target_2d_cm.y, // target velocity + _vel_desired.y * 100.0, // desired velocity + _vel_target.y * 100.0, // target velocity curr_vel_NED.y * 100.0, // velocity - 0.0, // desired accel - accel_target_2d_cm.y, // target accel + _accel_desired.y * 100.0, // desired accel + _accel_target.y * 100.0, // target accel curr_accel_NED.y); // accel } @@ -382,7 +382,7 @@ void AR_PosControl::handle_ekf_xy_reset() if (!AP::ahrs().get_velocity_NED(vel_NED)) { return; } - _vel_target = vel_NED.xy() + _pid_vel.get_error(); + _vel_desired = vel_NED.xy() + _pid_vel.get_error(); _ekf_xy_reset_ms = reset_ms; } diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h index ed0d65853fe..87ed9324265 100644 --- a/libraries/APM_Control/AR_PosControl.h +++ b/libraries/APM_Control/AR_PosControl.h @@ -43,7 +43,7 @@ class AR_PosControl { // init should be called once before starting to use these methods void input_pos_target(const Vector2p &pos, float dt); - // set position, velocity and acceleration targets. These should be from an externally created path and are not "input shaped" + // set target position, desired velocity and acceleration. These should be from an externally created path and are not "input shaped" void set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel); // get outputs for forward-back speed (in m/s), lateral speed (in m/s) and turn rate (in rad/sec) @@ -54,10 +54,10 @@ class AR_PosControl { // get position target const Vector2p& get_pos_target() const { return _pos_target; } - // returns desired velocity vector (i.e. feed forward) in m/s in lat and lon direction + // returns desired velocity vector (i.e. feed forward) in m/s in NE frame Vector2f get_desired_velocity() const; - // return desired acceleration vector in m/s in lat and lon direction + // return desired acceleration vector in m/s in NE frame Vector2f get_desired_accel() const; /// get position error as a vector from the current position to the target position @@ -96,11 +96,13 @@ class AR_PosControl { // position and velocity targets Vector2p _pos_target; // position target as an offset (in meters) from the EKF origin + Vector2f _vel_desired; // desired velocity in m/s in NE frame. This is the "feed forward" provided by SCurves Vector2f _vel_target; // velocity target in m/s in NE frame + Vector2f _accel_desired; // desired accel in m/s/s in NE frame. This is the "feed forward" provided by SCurves Vector2f _accel_target; // accel target in m/s/s in NE frame bool _pos_target_valid; // true if _pos_target is valid - bool _vel_target_valid; // true if _vel_target is valid - bool _accel_target_valid; // true if _accel_target is valid + bool _vel_desired_valid; // true if _vel_desired is valid + bool _accel_desired_valid; // true if _accel_desired is valid // variables for navigation uint32_t _last_update_ms; // system time of last call to update From 0dd9a8028b5ea1ccbd9f27e78c64f24341914133 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 4 Mar 2022 15:27:58 +0900 Subject: [PATCH 0495/3101] AR_WPNav: integrate scurve snap --- libraries/AR_WPNav/AR_WPNav.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 3dd77c4326e..1d4bbb5b6e6 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -30,7 +30,7 @@ extern const AP_HAL::HAL& hal; #define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds #define AR_WPNAV_RADIUS_DEFAULT 2.0f #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED -#define AR_WPNAV_JERK_TIME_SEC 0.1 // jerk time (the time taken for jerk to climb to its maximum) is hard-coded to 0.1 +#define AR_WPNAV_SNAP_MAX 15.0f // scurve snap (change in jerk) in m/s/s/s/s #define AR_WPNAV_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit const AP_Param::GroupInfo AR_WPNav::var_info[] = { @@ -250,9 +250,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), _pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed down (not used) - MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), + _pos_control.get_accel_max(), // forward back acceleration _pos_control.get_accel_max(), // vertical accel (not used) - AR_WPNAV_JERK_TIME_SEC, // jerk time + AR_WPNAV_SNAP_MAX, // snap _pos_control.get_jerk_max()); } @@ -277,9 +277,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _pos_control.get_speed_max(), _pos_control.get_speed_max(), // speed up (not used) _pos_control.get_speed_max(), // speed down (not used) - MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()), + _pos_control.get_accel_max(), // forward back acceleration _pos_control.get_accel_max(), // vertical accel (not used) - AR_WPNAV_JERK_TIME_SEC, // jerk time + AR_WPNAV_SNAP_MAX, // snap _pos_control.get_jerk_max()); // next destination provided so fast waypoint @@ -439,7 +439,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float // update target position, velocity and acceleration const float wp_radius = MAX(_radius, _turn_radius); - bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); + bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _pos_control.get_lat_accel_max(), _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel); // pass new target to the position controller init_pos_control_if_necessary(); From 2f03e2a75fba7c9afad214c67db744984136c3da Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 29 Mar 2022 19:30:11 +0900 Subject: [PATCH 0496/3101] Copter: 4.2.0-beta3 release notes --- ArduCopter/ReleaseNotes.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 0cd837856eb..b871bf31c54 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,22 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Copter 4.2.0-beta3 30-Mar-2022 +Changes from 4.2.0-beta2 +1) Minor Enhancements + a) BATT_OPTIONS supports sending resting voltage (corrected for internal resistance) to the ground station + b) KakuteH7-bdshot support + c) MatekH743 uses a 32 bit timer to resolve occasional 68ms timing glitch + d) RC input protocol text message sent to GCS (helps pilot awareness during RC handover) + e) Autotune code changes to reduce flash size (no functional impact) +2) Bug fixes + a) Battery remaining percentage fixed when using Sum battery + b) DShot reversal bug with IOMCU based boards (see SERVO_BLH_RVMASK) + c) GPS blending fix that could have resulted in the wrong GPS being used for a short time + d) Param conversion bug (impacted airspeed enable) + e) RC handover between IOMCU RC input and a secondary RC input on a serial port fixed + f) Terrain reference adjustment ensures alt-above-terrain is zero at takeoff (see TERRAIN_OFS_MAX) + g) QioTek Zealot H743 SLCAN port and relays fixed +------------------------------------------------------------------ Copter 4.2.0-beta2 10-Mar-2022 Changes from 4.2.0-beta1 1) Auto and Guided mode changes From beb13d91fbb4b9ba41e41dd856d2d750e5852873 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 29 Mar 2022 19:30:50 +0900 Subject: [PATCH 0497/3101] Rover: 4.2.0-beta3 release notes --- Rover/release-notes.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index d41265e328e..46d91ed5dd6 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,22 @@ Rover Release Notes: ------------------------------------------------------------------ +Rover 4.2.0-beta3 30-Mar-2022 +Changes from 4.2.0-beta2 +1) Minor Enhancements + a) BATT_OPTIONS supports sending resting voltage (corrected for internal resistance) to the ground station + b) KakuteH7-bdshot support + c) MatekH743 uses a 32 bit timer to resolve occasional 68ms timing glitch + d) RC input protocol text message sent to GCS (helps pilot awareness during RC handover) +2) Bug fixes + a) Balance bot stands in acro mode even with no GPS + b) Battery remaining percentage fixed when using Sum battery + c) DShot reversal bug with IOMCU based boards (see SERVO_BLH_RVMASK) + d) DShot commands fixed (could cause random motor movements) + e) GPS blending fix that could have resulted in the wrong GPS being used for a short time + f) Param conversion bug (impacted airspeed enable) + g) RC handover between IOMCU RC input and a secondary RC input on a serial port fixed + h) QioTek Zealot H743 SLCAN port and relays fixed +------------------------------------------------------------------ Rover 4.2.0-beta2 10-Mar-2022 Changes from 4.2.0-beta1 1) Follow mode supports FOLLOW_TARGET message (sent by QGC ground station) From c1e0e9c305ace7cab0972f27110442a4918a21bf Mon Sep 17 00:00:00 2001 From: Mirko Denecke Date: Sun, 6 Mar 2022 14:38:10 +0100 Subject: [PATCH 0498/3101] AP_HAL_ChibiOS: fix ms5611 probe for NucleoH743 --- libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat index 09cc01d0fca..5e28cecc9b5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat @@ -67,6 +67,6 @@ define ALLOW_ARM_NO_COMPASS IMU Invensense SPI:mpu6000 ROTATION_NONE # and ms5611 baro -BARO MS56XX SPI:ms56xx +BARO MS56XX SPI:ms5611 # define HAL_DISABLE_DCACHE From 2c42c056d8a88fed43f0e8d4145f81a19afbb1cb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 31 Mar 2022 12:19:41 +1100 Subject: [PATCH 0499/3101] Plane: correct EnableLandResponsition->EnableLandReposition --- ArduPlane/quadplane.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0bd05402d8d..82c58d71cad 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -257,7 +257,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: OPTIONS // @DisplayName: quadplane options // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. - // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandResponsition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL + // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), From a01ca359391133fbf772ed461165fbb935ce315d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Apr 2022 09:07:55 +1100 Subject: [PATCH 0500/3101] AP_Logger: increase stack of log_io thread by 256 This was seem on omnibusf4pro, it is a bit too close: log_io PRI= 59 sp=0x20015CC0 STACK=144/1656 --- libraries/AP_Logger/AP_Logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index f223ab5251f..db002f9ced7 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -27,7 +27,7 @@ extern const AP_HAL::HAL& hal; #endif #ifndef HAL_LOGGING_STACK_SIZE -#define HAL_LOGGING_STACK_SIZE 1324 +#define HAL_LOGGING_STACK_SIZE 1580 #endif #ifndef HAL_LOGGING_MAV_BUFSIZE From 875f3f7d9791dfb440c6efa51d7a1ee519971751 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 30 Mar 2022 08:12:36 -0500 Subject: [PATCH 0501/3101] Blimp: add temporary GCS FS behavior of disarming Blimp --- Blimp/events.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 7c3c5577a65..b3856bc9605 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -120,7 +120,7 @@ void Blimp::failsafe_gcs_check() } else if (last_gcs_update_ms > gcs_timeout_ms && !failsafe.gcs) { // New GCS failsafe event, trigger events set_failsafe_gcs(true); - // failsafe_gcs_on_event(); + arming.disarm(AP_Arming::Method::GCSFAILSAFE); // failsafe_gcs_on_event() should replace this when written } } @@ -174,4 +174,4 @@ void Blimp::gpsglitch_check() gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared"); } } -} \ No newline at end of file +} From 3cc75ecd387c6800ff9b796ecafc054c14cce16d Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 30 Mar 2022 07:35:08 -0500 Subject: [PATCH 0502/3101] Tools: add blimp as vehicle option in sim_vehicle --- Tools/autotest/sim_vehicle.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 14cb075f5d6..2c2e227d76b 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -902,6 +902,7 @@ def generate_frame_help(): vehicle_choices.append("copter") # should change to ArduCopter at some stage vehicle_choices.append("plane") # should change to ArduPlane at some stage vehicle_choices.append("sub") # should change to Sub at some stage +vehicle_choices.append("blimp") # should change to Blimp at some stage parser.add_option("-v", "--vehicle", type='choice', @@ -1305,6 +1306,7 @@ def generate_frame_help(): "copter": "ArduCopter", # will switch eventually "plane": "ArduPlane", # will switch eventually "sub": "ArduSub", # will switch eventually + "blimp" : "Blimp", # will switch eventually } if cmd_opts.vehicle in vehicle_map: progress("%s is now known as %s" % From 8e37c93e7d7716885f97addd941fa0df3b47040d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 29 Mar 2022 17:39:57 +1100 Subject: [PATCH 0503/3101] Plane: fixed rudder control when ARMING_RUDDER != 2 when rudder disarm is disabled we should allow full yaw control regardless of throttle level. We should also only disable left yaw when throttle is at zero, as right yaw does not indicate pilot may be trying to disarm --- ArduPlane/quadplane.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 82c58d71cad..c638730acfe 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1172,13 +1172,15 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) */ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const { + const auto rudder_in = plane.channel_rudder->get_control_in(); bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && !is_positive(plane.get_throttle_input()) && (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && - plane.arming.get_rudder_arming_type() != AP_Arming::RudderArming::IS_DISABLED && + plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM && + rudder_in < 0 && fabsf(inertial_nav.get_velocity_z_up_cms()) < 0.5 * get_pilot_velocity_z_max_dn()) { - // the user may be trying to disarm + // the user may be trying to disarm, disable pilot yaw control return 0; } @@ -1204,7 +1206,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const // must have a non-zero max yaw rate for scaling to work max_rate = (yaw_rate_max < 1.0f) ? 1 : yaw_rate_max; } - return plane.channel_rudder->get_control_in() * max_rate * (1/45.0); + return rudder_in * max_rate * (1/45.0); } /* @@ -1217,11 +1219,7 @@ float QuadPlane::get_desired_yaw_rate_cds(bool should_weathervane) // use bank angle to get desired yaw rate yaw_cds += desired_auto_yaw_rate_cds(); } - bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); - if (!is_positive(plane.get_throttle_input()) && !plane.control_mode->does_auto_throttle() && !manual_air_mode && !(inertial_nav.get_velocity_z_up_cms() < -0.5 * get_pilot_velocity_z_max_dn())) { - // the user may be trying to disarm - return 0; - } + // add in pilot input yaw_cds += get_pilot_input_yaw_rate_cds(); From 62fe90caa1c14fc0fae24045ea3ff39d1dadc96f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 18:24:51 +1100 Subject: [PATCH 0504/3101] autotest: add more convenience methods for checking received data --- Tools/autotest/common.py | 57 +++++++++++++++++++++++++++++++++++----- 1 file changed, 51 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index c96f616a00c..1519e3604a8 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -3283,21 +3283,44 @@ def assert_reach_imu_temperature(self, target, timeout): if not temp_ok: raise NotAchievedException("target temperature") - def assert_message_field_values(self, m, fieldvalues, verbose=True): + def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): for (fieldname, value) in fieldvalues.items(): got = getattr(m, fieldname) - if got != value: - raise NotAchievedException("Expected %s.%s to be %s, got %s" % - (m.get_type(), fieldname, value, got)) + if math.isnan(value) or math.isnan(got): + equal = math.isnan(value) and math.isnan(got) + elif epsilon is not None: + equal = abs(got - value) <= epsilon + else: + equal = got == value + + if not equal: + self.progress("Expected %s.%s to be %s, got %s" % + (m.get_type(), fieldname, value, got)) + return False if verbose: self.progress("%s.%s has expected value %s" % (m.get_type(), fieldname, value)) + return True + + def assert_message_field_values(self, m, fieldvalues, verbose=True, epsilon=None): + if self.message_has_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon): + return + raise NotAchievedException("Did not get expected field values") - def assert_received_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False): + def assert_received_message_field_values(self, message, fieldvalues, verbose=True, very_verbose=False, epsilon=None): m = self.assert_receive_message(message, verbose=verbose, very_verbose=very_verbose) - self.assert_message_field_values(m, fieldvalues, verbose=verbose) + self.assert_message_field_values(m, fieldvalues, verbose=verbose, epsilon=epsilon) return m + def wait_message_field_values(self, message, fieldvalues, timeout=10, epsilon=None): + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Field never reached values") + m = self.assert_receive_message(message) + if self.message_has_field_values(m, fieldvalues, epsilon=epsilon): + break + def onboard_logging_not_log_disarmed(self): self.start_subtest("Test LOG_DISARMED-is-false behaviour") self.set_parameter("LOG_DISARMED", 0) @@ -3671,6 +3694,21 @@ def assert_receive_message(self, type, timeout=1, verbose=False, very_verbose=Fa raise NotAchievedException("Did not get %s" % type) return m + def assert_receive_named_value_float(self, name, timeout=10): + tstart = self.get_sim_time_cached() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Did not get NAMED_VALUE_FLOAT %s" % name) + m = self.assert_receive_message('NAMED_VALUE_FLOAT', verbose=1, very_verbose=1, timeout=timeout) + if m.name != name: + continue + return m + + def assert_receive_named_value_float_value(self, name, value, epsilon=0.0001, timeout=10): + m = self.assert_receive_named_value_float_value(name, timeout=timeout) + if abs(m.value - value) > epsilon: + raise NotAchievedException("Bad %s want=%f got=%f" % (name, value, m.value)) + def assert_rally_files_same(self, file1, file2): self.progress("Comparing (%s) and (%s)" % (file1, file2, )) f1 = open(file1) @@ -4076,6 +4114,9 @@ def rc_defaults(self): def set_rc_from_map(self, _map, timeout=20): map_copy = _map.copy() + for v in map_copy.values(): + if type(v) != int: + raise NotAchievedException("RC values must be integers") self.rc_queue.put(map_copy) if self.rc_thread is None: @@ -9749,6 +9790,10 @@ def current_onboard_log_contains_message(self, messagetype): print("m=%s" % str(m)) return m is not None + def assert_current_onboard_log_contains_message(self, messagetype): + if not self.current_onboard_log_contains_message(messagetype): + raise NotAchievedException("Current onboard log does not contain message %s" % messagetype) + def run_tests(self, tests): """Autotest vehicle in SITL.""" if self.run_tests_called: From 2fb1bde457ac52506c231922789a3115c6ec1c0b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 23:26:16 +1100 Subject: [PATCH 0505/3101] autotest: add test for earth-frame-accel --- Tools/autotest/arducopter.py | 124 +++++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 46439e9ede8..f75aeb88eae 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1093,6 +1093,126 @@ def test_vibration_failsafe(self): self.context_pop() self.reboot_sitl() + def assert_dataflash_message_field_level_at(self, + mtype, + field, + level, + maintain=1, + tolerance=0.05, + timeout=30, + condition=None, + dfreader_start_timestamp=None, + verbose=False): + '''wait for EKF's accel bias to reach a level and maintain it''' + + if verbose: + self.progress("current onboard log filepath: %s" % self.current_onboard_log_filepath()) + dfreader = self.dfreader_for_current_onboard_log() + + achieve_start = None + current_value = None + while True: + m = dfreader.recv_match(type=mtype, condition=condition) + if m is None: + raise NotAchievedException("%s.%s did not maintain %f" % + (mtype, field, level)) + if dfreader_start_timestamp is not None: + if m.TimeUS < dfreader_start_timestamp: + continue + if verbose: + print("m=%s" % str(m)) + current_value = getattr(m, field) + + if abs(current_value - level) > tolerance: + if achieve_start is not None: + self.progress("Achieve stop at %u" % m.TimeUS) + achieve_start = None + continue + + dfreader_now = m.TimeUS + if achieve_start is None: + self.progress("Achieve start at %u (got=%f want=%f)" % + (dfreader_now, current_value, level)) + if maintain is None: + return + achieve_start = m.TimeUS + continue + + # we're achieving.... + if dfreader_now - achieve_start > maintain*1e6: + return dfreader_now + + # Tests any EK3 accel bias is subtracted from the correct IMU data + def EK3AccelBias(self): + + self.context_push() + + self.start_test("Test zero bias") + dfreader_tstart = self.assert_dataflash_message_field_level_at( + "XKF2", + "AZ", + 0.0, + condition="XKF2.C==1", + ) + + # Add 2m/s/s bias to the second IMU + self.set_parameters({ + 'SIM_ACC2_BIAS_Z': 0.7, + }) + + self.start_subtest("Ensuring second core has bias") + self.delay_sim_time(30) + dfreader_tstart = self.assert_dataflash_message_field_level_at( + "XKF2", "AZ", 0.7, + condition="XKF2.C==1", + ) + + self.start_subtest("Ensuring earth frame is compensated") + self.assert_dataflash_message_field_level_at( + "RATE", "A", 0, + maintain=1, + tolerance=2, # RATE.A is in cm/s/s + dfreader_start_timestamp=dfreader_tstart, + ) + + # now switch the EKF to only using the second core: + self.set_parameters({ + 'SIM_ACC2_BIAS_Z': 0.0, + "EK3_IMU_MASK": 0b10, + }) + self.reboot_sitl() + + self.delay_sim_time(30) + dfreader_tstart = self.assert_dataflash_message_field_level_at( + "XKF2", "AZ", 0.0, + condition="XKF2.C==0", + ) + + # Add 2m/s/s bias to the second IMU + self.set_parameters({ + 'SIM_ACC2_BIAS_Z': 0.7, + }) + + self.start_subtest("Ensuring first core now has bias") + self.delay_sim_time(30) + dfreader_tstart = self.assert_dataflash_message_field_level_at( + "XKF2", "AZ", 0.7, + condition="XKF2.C==0", + ) + + self.start_subtest("Ensuring earth frame is compensated") + self.assert_dataflash_message_field_level_at( + "RATE", "A", 0, + maintain=1, + tolerance=2, # RATE.A is in cm/s/s + dfreader_start_timestamp=dfreader_tstart, + verbose=True, + ) + + # revert simulated accel bias and reboot to restore EKF health + self.context_pop() + self.reboot_sitl() + # fly_stability_patch - fly south, then hold loiter within 5m # position and altitude and reduce 1 motor to 60% efficiency def fly_stability_patch(self, @@ -8437,6 +8557,10 @@ def tests1c(self): "Test Vibration Failsafe", self.test_vibration_failsafe), + ("EK3AccelBias", + "Test EK3 Accel Bias data", + self.EK3AccelBias), + ("StabilityPatch", "Fly stability patch", lambda: self.fly_stability_patch(30)), # 17s From fd666dc4f1a7da85d5ec14a70258797fcef6ab1c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 18:13:51 +1100 Subject: [PATCH 0506/3101] AP_AHRS: subtract accel bias from correct ins accel instance In the case that you have INS_USE indicating IMUs should be used, but EK3_IMU_MASK leaving some IMUs unused, we subtract the bias from the wrong INS data --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index a37ac73bbd7..6cbb8edc6d7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -606,7 +606,7 @@ void AP_AHRS::update_EKF3(void) // update _accel_ef_ekf for (uint8_t i=0; i<_ins.get_accel_count(); i++) { Vector3f accel = _ins.get_accel(i); - if (i==_ins.get_primary_accel()) { + if (i == primary_imu) { accel -= abias; } if (_ins.get_accel_health(i)) { From 1c5a954b244104ece7b3d4d0cea43b2b9f05ce40 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 14:57:18 +1000 Subject: [PATCH 0507/3101] HAL_ChibiOS: incorrect class of class in uart TX timeout need to call chEvtGetAndClearEventsI() as we are in a system lock state --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index edbea114710..ba47a65ad4d 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -1021,7 +1021,7 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n) if (tx_len > 0) { _last_write_completed_us = AP_HAL::micros(); } - chEvtGetAndClearEvents(EVT_TRANSMIT_DMA_COMPLETE); + chEvtGetAndClearEventsI(EVT_TRANSMIT_DMA_COMPLETE); chSysUnlock(); } // clean up pending locks From c3e071579d60a84d91160aed56b473d7d80192e1 Mon Sep 17 00:00:00 2001 From: Nicholas Kruzan Date: Mon, 4 Apr 2022 13:06:29 +0000 Subject: [PATCH 0508/3101] AP_Filesystem: AP_Filesystem_ESP32 allow_absolute_paths in ::open() --- libraries/AP_Filesystem/AP_Filesystem_ESP32.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_ESP32.cpp b/libraries/AP_Filesystem/AP_Filesystem_ESP32.cpp index 3e34ba4a234..de285e358ef 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_ESP32.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_ESP32.cpp @@ -26,7 +26,7 @@ extern const AP_HAL::HAL& hal; -int AP_Filesystem_ESP32::open(const char *fname, int flags) +int AP_Filesystem_ESP32::open(const char *fname, int flags, bool allow_absolute_paths) { #if FSDEBUG printf("DO open %s \n", fname); From d42a287db06caad92808d950eb3c186553a5eded Mon Sep 17 00:00:00 2001 From: Nicholas Kruzan Date: Mon, 4 Apr 2022 14:14:09 +0000 Subject: [PATCH 0509/3101] AP_HAL_ESP32: partitions.csv - increase app partition to 3M --- libraries/AP_HAL_ESP32/targets/partitions.csv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ESP32/targets/partitions.csv b/libraries/AP_HAL_ESP32/targets/partitions.csv index d245f35d50e..70f23f3af89 100644 --- a/libraries/AP_HAL_ESP32/targets/partitions.csv +++ b/libraries/AP_HAL_ESP32/targets/partitions.csv @@ -1,5 +1,5 @@ # Name, Type, SubType, Offset, Size nvs, data, nvs, , 0x6000 phy_init, data, phy, , 0x1000 -factory, app, factory, , 2M +factory, app, factory, , 3M storage, 0x45, 0x0, , 128K From 11b3eccf66273f8a174969afad2f0c064520ea7b Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Mon, 4 Apr 2022 11:26:10 +1000 Subject: [PATCH 0510/3101] AP_Periph: Hwdef CarbonixL496 update Modification of CarbonixL496 to add more features. Features: To use internal clock of MCU, Oscillator value is set to 0. LED pin changed from PA13 to PA15 Enabled JTAG Add interface to GPS and ADSB. --- .../hwdef/CarbonixL496/hwdef-bl.dat | 12 +++++----- .../hwdef/CarbonixL496/hwdef.dat | 23 ++++++++++++++----- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef-bl.dat index a7b8e7937ac..68b3974cbb1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef-bl.dat @@ -15,8 +15,8 @@ APJ_BOARD_ID 1053 # setup build for a peripheral firmware env AP_PERIPH 1 -# crystal frequency -OSCILLATOR_HZ 12000000 +# crystal frequency set to 0 to use internal clock +OSCILLATOR_HZ 0 # assume 256k flash part FLASH_SIZE_KB 256 @@ -28,7 +28,7 @@ STDOUT_BAUDRATE 57600 SERIAL_ORDER OTG1 USART2 # a fault LED -PA13 LED_BOOTLOADER OUTPUT HIGH # blue +PA15 LED_BOOTLOADER OUTPUT HIGH # blue define HAL_LED_ON 0 # USART1 @@ -60,9 +60,9 @@ define HAL_DISABLE_LOOP_DELAY PB8 CAN1_RX CAN1 PB9 CAN1_TX CAN1 -# debugger support, disabled as PA13 used for LED -# PA13 JTMS-SWDIO SWD -# PA14 JTCK-SWCLK SWD +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD # make bl baudrate match debug baudrate for easier debugging define BOOTLOADER_BAUDRATE 57600 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index 8ca74a93f54..c81c43ad10a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -18,8 +18,8 @@ env AP_PERIPH 1 # enable watchdog -# crystal frequency -OSCILLATOR_HZ 12000000 +# crystal frequency set to 0 to use internal clock +OSCILLATOR_HZ 0 # assume the 256k flash part FLASH_SIZE_KB 256 @@ -55,7 +55,7 @@ PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 # LED, active low -PA13 LED OUTPUT HIGH GPIO(1) +PA15 LED OUTPUT HIGH GPIO(1) # spi2 PB10 SPI2_SCK SPI2 @@ -78,9 +78,9 @@ I2C_ORDER I2C4 # allow for reboot command for faster development define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 -# debugger support (disabled as conflicts with LED) -#PA13 JTMS-SWDIO SWD -#PA14 JTCK-SWCLK SWD +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD define HAL_NO_GPIO_IRQ define SERIAL_BUFFERS_SIZE 512 @@ -126,6 +126,17 @@ define HAL_PERIPH_ENABLE_RC_OUT define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 +# enable GPS +define HAL_PERIPH_ENABLE_GPS +define HAL_PERIPH_GPS_PORT_DEFAULT 2 +#define HAL_PERIPH_ENABLE_NOTIFY +#define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY + +# default ADSB off by setting 0 baudrate +define HAL_PERIPH_ENABLE_ADSB +define HAL_PERIPH_ADSB_PORT_DEFAULT 3 +define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 + BARO MS56XX I2C:0:0x76 COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE From fa81f7813c4a807f3fec0fd76b2bac115cc64e11 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 1 Apr 2022 10:55:42 -0500 Subject: [PATCH 0511/3101] AP_Airspeed: correct metadata for params for non-Plane vehicles --- libraries/AP_Airspeed/AP_Airspeed.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 7efb651be10..6653f80cb98 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -114,7 +114,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { #ifndef HAL_BUILD_AP_PERIPH // @Param: _USE // @DisplayName: Airspeed use - // @Description: Enables airspeed use for automatic throttle modes and replaces control from THR_TRIM. Continues to display and log airspeed if set to 0. Uses airspeed for control if set to 1. Only uses airspeed when throttle = 0 if set to 2 (useful for gliders with airspeed sensors behind propellers). + // @Description{Plane}: Enables airspeed use for automatic throttle modes and replaces control from THR_TRIM. Continues to display and log airspeed if set to 0. Uses airspeed for control if set to 1. Only uses airspeed when throttle = 0 if set to 2 (useful for gliders with airspeed sensors behind propellers). + // @Description{Copter, Blimp, Rover, Sub}: This parameter is not used by this vehicle. Always set to 0. // @Values: 0:DoNotUse,1:Use,2:UseWhenZeroThrottle // @User: Standard AP_GROUPINFO("_USE", 1, AP_Airspeed, param[0].use, 0), @@ -142,7 +143,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { #if AP_AIRSPEED_AUTOCAL_ENABLE // @Param: _AUTOCAL - // @DisplayName: Automatic airspeed ratio calibration + // @DisplayName{Plane}: Automatic airspeed ratio calibration + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @Description: Enables automatic adjustment of ARSPD_RATIO during a calibration flight based on estimation of ground speed and true airspeed. New ratio saved every 2 minutes if change is > 5%. Should not be left enabled. // @User: Advanced AP_GROUPINFO("_AUTOCAL", 5, AP_Airspeed, param[0].autocal, 0), @@ -157,7 +159,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { #ifndef HAL_BUILD_AP_PERIPH // @Param: _SKIP_CAL - // @DisplayName: Skip airspeed calibration on startup + // @DisplayName: Skip airspeed offset calibration on startup // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot. // @Values: 0:Disable,1:Enable // @User: Advanced @@ -191,7 +193,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { #ifndef HAL_BUILD_AP_PERIPH // @Param: _OPTIONS // @DisplayName: Airspeed options bitmask - // @Description: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction + // @Description{Plane}: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @Bitmask: 0:SpeedMismatchDisable, 1:AllowSpeedMismatchRecovery, 2:DisableVoltageCorrection // @User: Advanced AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, OPTIONS_DEFAULT), @@ -199,6 +202,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: _WIND_MAX // @DisplayName: Maximum airspeed and ground speed difference // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will be marked unhealthy. Using ARSPD_OPTION this health value can be used to disable the sensor. + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @Units: m/s // @User: Advanced AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0), @@ -206,6 +210,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: _WIND_WARN // @DisplayName: Airspeed and ground speed difference that gives a warning // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used. + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @Units: m/s // @User: Advanced AP_GROUPINFO("_WIND_WARN", 23, AP_Airspeed, _wind_warn, 0), @@ -221,7 +226,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: 2_USE // @DisplayName: Enable use of 2nd airspeed sensor - // @Description: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller + // @Description{Plane}: use airspeed for flight control. When set to 0 airspeed sensor can be logged and displayed on a GCS but won't be used for flight. When set to 1 it will be logged and used. When set to 2 it will be only used when the throttle is zero, which can be useful in gliders with airspeed sensors behind a propeller + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @Values: 0:Don't Use,1:use,2:UseWhenZeroThrottle // @User: Standard AP_GROUPINFO("2_USE", 12, AP_Airspeed, param[1].use, 0), @@ -248,7 +254,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: 2_AUTOCAL // @DisplayName: Automatic airspeed ratio calibration for 2nd airspeed sensor - // @Description: If this is enabled then the autopilot will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended. + // @Description{Plane}: If this is enabled then the autopilot will automatically adjust the ARSPD_RATIO during flight, based upon an estimation filter using ground speed and true airspeed. The automatic calibration will save the new ratio to EEPROM every 2 minutes if it changes by more than 5%. This option should be enabled for a calibration flight then disabled again when calibration is complete. Leaving it enabled all the time is not recommended. + // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0. // @User: Advanced AP_GROUPINFO("2_AUTOCAL", 16, AP_Airspeed, param[1].autocal, 0), @@ -260,7 +267,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { AP_GROUPINFO("2_TUBE_ORDR", 17, AP_Airspeed, param[1].tube_order, 2), // @Param: 2_SKIP_CAL - // @DisplayName: Skip airspeed calibration on startup for 2nd sensor + // @DisplayName: Skip airspeed offset calibration on startup for 2nd sensor // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot. // @Values: 0:Disable,1:Enable // @User: Advanced From 82235163a20b5e014789a5f5be0d7cc6f49f3856 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 2 Apr 2022 14:28:24 -0500 Subject: [PATCH 0512/3101] AP_Scripting: remove original aerobatics example to avoid confusion --- .../examples/plane_aerobatics.lua | 395 ------------------ 1 file changed, 395 deletions(-) delete mode 100644 libraries/AP_Scripting/examples/plane_aerobatics.lua diff --git a/libraries/AP_Scripting/examples/plane_aerobatics.lua b/libraries/AP_Scripting/examples/plane_aerobatics.lua deleted file mode 100644 index e823263f606..00000000000 --- a/libraries/AP_Scripting/examples/plane_aerobatics.lua +++ /dev/null @@ -1,395 +0,0 @@ --- perform simple aerobatic manoeuvres in AUTO mode - -local running = false - -local roll_stage = 0 - -local ROLL_TCONST = param:get('RLL2SRV_TCONST') * 0.5 -local PITCH_TCONST = param:get('PTCH2SRV_TCONST') * 0.5 - -DO_JUMP = 177 -NAV_WAYPOINT = 16 - -k_throttle = 70 - -function bind_param(name) - local p = Parameter() - assert(p:init(name), string.format('could not find %s parameter', name)) - return p -end - -local SCR_USER1 = bind_param("SCR_USER1") -- height P gain -local SCR_USER2 = bind_param("SCR_USER2") -- height I gain -local SCR_USER3 = bind_param("SCR_USER3") -- throttle FF from pitch -local SCR_USER4 = bind_param("SCR_USER4") -- height knifeedge addition for pitch -local SCR_USER5 = bind_param("SCR_USER5") -- speed P gain -local SCR_USER6 = bind_param("SCR_USER6") -- speed I gain -local TRIM_THROTTLE = bind_param("TRIM_THROTTLE") -local TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM") - -local last_roll_err = 0.0 -local last_id = 0 -local initial_yaw_deg = 0 -local wp_yaw_deg = 0 -local initial_height = 0 - --- constrain a value between limits -function constrain(v, vmin, vmax) - if v < vmin then - v = vmin - end - if v > vmax then - v = vmax - end - return v -end - --- a controller to target a zero roll angle, coping with inverted flight --- output is a body frame roll rate, with convergence over time tconst in seconds -function roll_zero_controller(tconst) - local roll_deg = math.deg(ahrs:get_roll()) - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_err = 0.0 - if math.abs(pitch_deg) > 85 then - -- close to 90 we retain the last roll rate - roll_err = last_roll_err - elseif roll_deg > 90 then - roll_err = 180 - roll_deg - elseif roll_deg < -90 then - roll_err = (-180) - roll_deg - else - roll_err = -roll_deg - end - last_roll_err = roll_err - return roll_err / tconst -end - - -function wrap_360(angle) - local res = math.fmod(angle, 360.0) - if res < 0 then - res = res + 360.0 - end - return res -end - -function wrap_180(angle) - local res = wrap_360(angle) - if res > 180 then - res = res - 360 - end - return res -end - --- a PI controller implemented as a Lua object -local function PI_controller(kP,kI,iMax) - -- the new instance. You can put public variables inside this self - -- declaration if you want to - local self = {} - - -- private fields as locals - local _kP = kP or 0.0 - local _kI = kI or 0.0 - local _kD = kD or 0.0 - local _iMax = iMax - local _last_t = nil - local _I = 0 - local _P = 0 - local _total = 0 - local _counter = 0 - local _target = 0 - local _current = 0 - - -- update the controller. - function self.update(target, current) - local now = millis():tofloat() * 0.001 - if not _last_t then - _last_t = now - end - local dt = now - _last_t - _last_t = now - local err = target - current - _counter = _counter + 1 - - local P = _kP * err - _I = _I + _kI * err * dt - if _iMax then - _I = constrain(_I, -_iMax, iMax) - end - local I = _I - local ret = P + I - - _target = target - _current = current - _P = P - _total = ret - return ret - end - - -- reset integrator to an initial value - function self.reset(integrator) - _I = integrator - end - - function self.set_I(I) - _kI = I - end - - function self.set_P(P) - _kP = P - end - - function self.set_Imax(Imax) - _iMax = Imax - end - - -- log the controller internals - function self.log(name, add_total) - -- allow for an external addition to total - logger.write(name,'Targ,Curr,P,I,Total,Add','ffffff',_target,_current,_P,_I,_total,add_total) - end - - -- return the instance - return self -end - -local function height_controller(kP_param,kI_param,KnifeEdge_param,Imax) - local self = {} - local kP = kP_param - local kI = kI_param - local KnifeEdge = KnifeEdge_param - local PI = PI_controller(kP:get(), kI:get(), Imax) - - function self.update(target) - local target_pitch = PI.update(initial_height, ahrs:get_location():alt()*0.01) - local roll_rad = ahrs:get_roll() - local ke_add = math.abs(math.sin(roll_rad)) * KnifeEdge:get() - target_pitch = target_pitch + ke_add - PI.log("HPI", ke_add) - return target_pitch - end - - function self.reset() - PI.reset(math.max(math.deg(ahrs:get_pitch()), 3.0)) - PI.set_P(kP:get()) - PI.set_I(kI:get()) - end - - return self -end - -local height_PI = height_controller(SCR_USER1, SCR_USER2, SCR_USER4, 20.0) -local speed_PI = PI_controller(SCR_USER5:get(), SCR_USER6:get(), 100.0) - --- a controller to target a zero pitch angle and zero heading change, used in a roll --- output is a body frame pitch rate, with convergence over time tconst in seconds -function pitch_controller(target_pitch_deg, target_yaw_deg, tconst) - local roll_deg = math.deg(ahrs:get_roll()) - local pitch_deg = math.deg(ahrs:get_pitch()) - local yaw_deg = math.deg(ahrs:get_yaw()) - - -- get earth frame pitch and yaw rates - local ef_pitch_rate = (target_pitch_deg - pitch_deg) / tconst - local ef_yaw_rate = wrap_180(target_yaw_deg - yaw_deg) / tconst - - local bf_pitch_rate = math.sin(math.rad(roll_deg)) * ef_yaw_rate + math.cos(math.rad(roll_deg)) * ef_pitch_rate - local bf_yaw_rate = math.cos(math.rad(roll_deg)) * ef_yaw_rate - math.sin(math.rad(roll_deg)) * ef_pitch_rate - return bf_pitch_rate, bf_yaw_rate -end - --- a controller for throttle to account for pitch -function throttle_controller(tconst) - local pitch_rad = ahrs:get_pitch() - local thr_ff = SCR_USER3:get() - local throttle = TRIM_THROTTLE:get() + math.sin(pitch_rad) * thr_ff - return constrain(throttle, 0.0, 100.0) -end - -function do_axial_roll(arg1, arg2) - -- constant roll rate axial roll - if not running then - running = true - roll_stage = 0 - height_PI.reset() - gcs:send_text(0, string.format("Starting roll")) - end - local roll_rate = arg1 - local throttle = arg2 - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_deg = math.deg(ahrs:get_roll()) - if roll_stage == 0 then - if roll_deg > 45 then - roll_stage = 1 - end - elseif roll_stage == 1 then - if roll_deg > -5 and roll_deg < 5 then - running = false - -- we're done - gcs:send_text(0, string.format("Finished roll r=%.1f p=%.1f", roll_deg, pitch_deg)) - vehicle:nav_script_time_done(last_id) - roll_stage = 2 - return - end - end - if roll_stage < 2 then - target_pitch = height_PI.update(initial_height) - pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST) - vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate) - end -end - -local loop_stage = 0 - -function do_loop(arg1, arg2) - -- do one loop with controllable pitch rate and throttle - if not running then - running = true - loop_stage = 0 - gcs:send_text(0, string.format("Starting loop")) - end - local pitch_rate = arg1 - local throttle = throttle_controller() - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_deg = math.deg(ahrs:get_roll()) - if loop_stage == 0 then - if pitch_deg > 60 then - loop_stage = 1 - end - elseif loop_stage == 1 then - if math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5 then - running = false - -- we're done - gcs:send_text(0, string.format("Finished loop p=%.1f", pitch_deg)) - vehicle:nav_script_time_done(last_id) - loop_stage = 2 - return - end - end - if loop_stage < 2 then - local roll_rate = roll_zero_controller(ROLL_TCONST) - vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) - end -end - -local rolling_circle_stage = 0 -local rolling_circle_yaw = 0 -local rolling_circle_last_ms = 0 - -function do_rolling_circle(arg1, arg2) - -- constant roll rate circle roll - if not running then - running = true - rolling_circle_stage = 0 - rolling_circle_yaw = 0 - rolling_circle_last_ms = millis() - height_PI.reset() - - speed_PI.set_P(SCR_USER5:get()) - speed_PI.set_I(SCR_USER6:get()) - speed_PI.reset(math.max(SRV_Channels:get_output_scaled(k_throttle), TRIM_THROTTLE:get())) - gcs:send_text(0, string.format("Starting rolling circle")) - end - local radius = arg1 - local num_rolls = arg2 - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_deg = math.deg(ahrs:get_roll()) - local yaw_deg = math.deg(ahrs:get_yaw()) - local gspeed = ahrs:groundspeed_vector():length() - local circumference = math.abs(math.pi * 2.0 * radius) - local circle_time = circumference / gspeed - local yaw_rate_dps = 360.0 / circle_time - local now_ms = millis() - local dt = (now_ms - rolling_circle_last_ms):tofloat() * 0.001 - rolling_circle_last_ms = now_ms - - if radius < 0.0 then - yaw_rate_dps = -yaw_rate_dps - end - - local roll_rate = (360.0 * num_rolls) / circle_time - - rolling_circle_yaw = rolling_circle_yaw + yaw_rate_dps * dt - - if rolling_circle_stage == 0 then - if math.abs(rolling_circle_yaw) > 10.0 then - rolling_circle_stage = 1 - end - elseif rolling_circle_stage == 1 then - if math.abs(rolling_circle_yaw) >= 360.0 then - running = false - -- we're done - gcs:send_text(0, string.format("Finished rollcircle r=%.1f p=%.1f", roll_deg, pitch_deg)) - vehicle:nav_script_time_done(last_id) - rolling_circle_stage = 2 - return - end - end - - local target_roll = num_rolls * math.abs(rolling_circle_yaw) - local roll_error = wrap_180(target_roll - roll_deg) - local roll_error_P = 0.5 - local roll_rate_corrected = roll_rate + roll_error * roll_error_P - - if rolling_circle_stage < 2 then - target_pitch = height_PI.update(initial_height) - vel = ahrs:get_velocity_NED() - throttle = speed_PI.update(TRIM_ARSPD_CM:get()*0.01, vel:length()) - throttle = constrain(throttle, 0, 100.0) - speed_PI.log("SPI", 0.0) - pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(rolling_circle_yaw+initial_yaw_deg), PITCH_TCONST) - vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_corrected, pitch_rate, yaw_rate) - end -end - ---- get a location object for a given WP number -function get_wp_location(i) - local m = mission:get_item(i) - local loc = Location() - loc:lat(m:x()) - loc:lng(m:y()) - loc:relative_alt(false) - loc:terrain_alt(false) - loc:origin_alt(false) - loc:alt(math.floor(m:z()*100)) - return loc -end - -function resolve_jump(i) - local m = mission:get_item(i) - while m:command() == DO_JUMP do - i = math.floor(m:param1()) - m = mission:get_item(i) - end - return i -end - -function update() - id, cmd, arg1, arg2 = vehicle:nav_script_time() - if id then - if id ~= last_id then - -- we've started a new command - running = false - last_id = id - initial_yaw_deg = math.deg(ahrs:get_yaw()) - initial_height = ahrs:get_location():alt()*0.01 - - -- work out yaw between previous WP and next WP - local cnum = mission:get_current_nav_index() - local loc_prev = get_wp_location(cnum-1) - local loc_next = get_wp_location(resolve_jump(cnum+1)) - wp_yaw_deg = math.deg(loc_prev:get_bearing(loc_next)) - end - if cmd == 1 then - do_axial_roll(arg1, arg2) - elseif cmd == 2 then - do_loop(arg1, arg2) - elseif cmd == 3 then - do_rolling_circle(arg1, arg2) - end - else - running = false - end - return update, 10 -end - -return update() From a95b429acce1846bee56b6f64c3f68cff3743e10 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 08:17:10 +1000 Subject: [PATCH 0513/3101] AP_Math: added unsigned versions of constrain functions sometimes it really does matter that we use constrain_uint32() instead of constrain_int32(). For example, if we have a value like 0xFFFFFFFF then the result will be very different we should use unsigned constrain when dealing with unsigned values --- libraries/AP_Math/AP_Math.cpp | 4 ++++ libraries/AP_Math/AP_Math.h | 15 +++++++++++++++ libraries/AP_Math/tests/test_math.cpp | 4 ++++ 3 files changed, 23 insertions(+) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index 9362f236d34..aba033bf19d 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -313,9 +313,13 @@ T constrain_value(const T amt, const T low, const T high) } template int constrain_value(const int amt, const int low, const int high); +template unsigned int constrain_value(const unsigned int amt, const unsigned int low, const unsigned int high); template long constrain_value(const long amt, const long low, const long high); +template unsigned long constrain_value(const unsigned long amt, const unsigned long low, const unsigned long high); template long long constrain_value(const long long amt, const long long low, const long long high); +template unsigned long long constrain_value(const unsigned long long amt, const unsigned long long low, const unsigned long long high); template short constrain_value(const short amt, const short low, const short high); +template unsigned short constrain_value(const unsigned short amt, const unsigned short low, const unsigned short high); template float constrain_value(const float amt, const float low, const float high); template double constrain_value(const double amt, const double low, const double high); diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index fa51036ae6f..81b837ecd42 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -179,16 +179,31 @@ inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16 return constrain_value(amt, low, high); } +inline uint16_t constrain_uint16(const uint16_t amt, const uint16_t low, const uint16_t high) +{ + return constrain_value(amt, low, high); +} + inline int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high) { return constrain_value(amt, low, high); } +inline uint32_t constrain_uint32(const uint32_t amt, const uint32_t low, const uint32_t high) +{ + return constrain_value(amt, low, high); +} + inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64_t high) { return constrain_value(amt, low, high); } +inline uint64_t constrain_uint64(const uint64_t amt, const uint64_t low, const uint64_t high) +{ + return constrain_value(amt, low, high); +} + // degrees -> radians static inline constexpr ftype radians(ftype deg) { diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index e5a04112683..f9a90accbf8 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -383,6 +383,10 @@ TEST(MathTest, Constrain) EXPECT_EQ(19.9, constrain_value(19.8, 19.9, 20.1)); EXPECT_EQ(19.9f, constrain_value(19.8f, 19.9f, 20.1f)); + // test that constrain on 32 bit integer works correctly. Note the asymmetry + EXPECT_EQ(10, constrain_int32( 0xFFFFFFFFU, 10U, 1200U)); + EXPECT_EQ(1200U, constrain_uint32(0xFFFFFFFFU, 10U, 1200U)); + #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX EXPECT_EQ(1.0f, constrain_float(nanf("0x4152"), 1.0f, 1.0f)); EXPECT_EQ(1.0f, constrain_value(nanf("0x4152"), 1.0f, 1.0f)); From 4c977eb7a0888602ced512bf81a2ae11645058ed Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Apr 2022 19:05:30 +1100 Subject: [PATCH 0514/3101] HAL_ChibiOS: prevent long timeouts in DShot this prevents bad calculated timeouts in DShot. The timeout would sometimes come out as 0xFFFFFFFF, which led to an assert and could block the thread This fix is meant to be minimilistic to allow it to be merged easily into 4.2. A better fix would fix all the uint32_t wrap handling in DShot --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5d328f957c2..c7dbd225869 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -253,14 +253,23 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us) // if we have time left wait for the event eventmask_t mask = 0; const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us; + uint32_t wait_us = 0; if (now < time_out_us) { - mask = chEvtWaitOneTimeout(group.dshot_event_mask, - chTimeUS2I(MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us))); + wait_us = MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us); } else if (pulse_elapsed_us < group.dshot_pulse_send_time_us) { // better to let the burst write in progress complete rather than cancelling mid way through - mask = chEvtWaitOneTimeout(group.dshot_event_mask, - chTimeUS2I(group.dshot_pulse_send_time_us - pulse_elapsed_us)); + wait_us = group.dshot_pulse_send_time_us - pulse_elapsed_us; } + + // waiting for a very short period of time can cause a + // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA + // as minimum. Don't allow for a very long delay (over 1200us) + // to prevent bugs in handling timer wrap + const uint32_t max_delay_us = 1200; + const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA + wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us); + mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(wait_us)); + // no time left cancel and restart if (!mask) { dma_cancel(group); From 5ef9c5aaa57b3b4644899916468d63fa46c8b28b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 09:54:20 +1000 Subject: [PATCH 0515/3101] HAL_ChibiOS: constrain more timer timeouts --- libraries/AP_HAL_ChibiOS/EventSource.cpp | 3 ++- libraries/AP_HAL_ChibiOS/GPIO.cpp | 11 +++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp index dfb21919b6b..4977a46be11 100644 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ b/libraries/AP_HAL_ChibiOS/EventSource.cpp @@ -1,4 +1,5 @@ #include "EventSource.h" +#include using namespace ChibiOS; @@ -13,7 +14,7 @@ bool EventSource::wait(uint64_t duration, AP_HAL::EventHandle *evt_handle) if (duration == 0) { ret = chEvtWaitAnyTimeout(evt_mask, TIME_IMMEDIATE); } else { - ret = chEvtWaitAnyTimeout(evt_mask, chTimeUS2I(duration)); + ret = chEvtWaitAnyTimeout(evt_mask, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration))); } ch_evt_src_.unregister(&evt_listener); return ret == MSG_OK; diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 1bb06ab9a05..e9df9407652 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -28,6 +28,7 @@ #include #endif #include +#include using namespace ChibiOS; @@ -462,8 +463,7 @@ static void pal_interrupt_wait(void *arg) } /* - block waiting for a pin to change. A timeout of 0 means wait - forever. Return true on pin change, false on timeout + block waiting for a pin to change. Return true on pin change, false on timeout */ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_us) { @@ -486,8 +486,11 @@ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_u osalSysUnlock(); return false; } - - msg_t msg = osalThreadSuspendTimeoutS(&g->thd_wait, TIME_US2I(timeout_us)); + + // don't allow for very long timeouts, or below the delta + timeout_us = constrain_uint32(TIME_US2I(timeout_us), CH_CFG_ST_TIMEDELTA, TIME_US2I(30000U)); + + msg_t msg = osalThreadSuspendTimeoutS(&g->thd_wait, timeout_us); _attach_interruptI(g->pal_line, palcallback_t(nullptr), nullptr, From f6f6521b28b984b8aafb95a7c8d8eb9d6315062e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 15:56:20 +1000 Subject: [PATCH 0516/3101] Revert "hwdef: changed MatekH743 to a 32 bit timer" This reverts commit 7dc59115729a3d8be6ab26b07ceb83ff12ddb7a9. --- .../hwdef/MatekH743-bdshot/hwdef.dat | 9 ++++++++- .../AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat | 17 ++++++++--------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat index ec449a63038..06e4834e908 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743-bdshot/hwdef.dat @@ -5,7 +5,7 @@ include ../MatekH743/hwdef.dat # undefine the pins we are going to change -undef PC7 PA0 PD14 PD15 PE5 PE6 PB0 PB1 +undef PC7 PB0 PB1 PA0 PA1 PA15 PD14 PD15 PE5 PE6 PC7 USART6_RX USART6 @@ -13,6 +13,7 @@ PC7 USART6_RX USART6 PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) PA0 TIM2_CH1 TIM2 PWM(3) GPIO(52) BIDIR +PA1 TIM2_CH2 TIM2 PWM(4) GPIO(53) # Disable DMA on PWM9-12 so that the LEDs get a channel PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) NODMA @@ -20,5 +21,11 @@ PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59) NODMA PE5 TIM15_CH1 TIM15 PWM(11) GPIO(60) NODMA PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) NODMA +# Beeper +PA15 BUZZER OUTPUT GPIO(32) LOW +define HAL_BUZZER_PIN 32 +define HAL_BUZZER_ON 1 +define HAL_BUZZER_OFF 0 + DMA_PRIORITY S* TIM3* TIM2* DMA_NOSHARE SPI1* SPI4* TIM3* TIM2* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat index ffb4b9bddca..6f930300db3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat @@ -16,8 +16,10 @@ env OPTIMIZE -Os # bootloader takes first sector FLASH_RESERVE_START_KB 128 + # ChibiOS system timer -STM32_ST_USE_TIMER 5 +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 # USB PA11 OTG_FS_DM OTG1 @@ -143,10 +145,10 @@ PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) # Motors PB0 TIM8_CH2N TIM8 PWM(1) GPIO(50) PB1 TIM8_CH3N TIM8 PWM(2) GPIO(51) -PA0 TIM2_CH1 TIM2 PWM(3) GPIO(52) -PA1 TIM2_CH2 TIM2 PWM(4) GPIO(53) -PA2 TIM2_CH3 TIM2 PWM(5) GPIO(54) -PA3 TIM2_CH4 TIM2 PWM(6) GPIO(55) +PA0 TIM5_CH1 TIM5 PWM(3) GPIO(52) +PA1 TIM5_CH2 TIM5 PWM(4) GPIO(53) +PA2 TIM5_CH3 TIM5 PWM(5) GPIO(54) +PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55) PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57) PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) @@ -156,10 +158,7 @@ PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED # Beeper -PA15 BUZZER OUTPUT GPIO(32) LOW -define HAL_BUZZER_PIN 32 -define HAL_BUZZER_ON 1 -define HAL_BUZZER_OFF 0 +PA15 TIM2_CH1 TIM2 GPIO(32) ALARM # microSD support PC8 SDMMC1_D0 SDMMC1 From a0c867bce756e97d80dbc3a34c251e0356af72ed Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 17:08:40 +1000 Subject: [PATCH 0517/3101] HAL_ChibiOS: disable interrupts during flash operations we have now shown that interrupts being enabled during flash operations can cause the infamous "68ms" bug, or watchdog when using a 32 bit timer on boards using flash for storage The issue is quite repeatable with a load of a very large waypoint file (over 500 waypoints) using "wp ftpload" in MAVProxy. This puts a huge load on flash storage. Our current working theory is that while doing flash writes for storage on dual-bank we block access to only one bank, so if another thread uses a timeout function with a short timeout while the flash write is happening and chVTDoTickI calls code which crosses the flash bank boundary then it can cause chVTDoTickI to violate the assumption that no more than CH_CFG_ST_DELTA ticks pass while it is calculating the value to set in the system timer. In that case we get a delay of a full timer wrap, which is 68ms on boards with 16 bit timer and 70 minutes on boards with 32 bit timer --- libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c index 89d2b2c755f..9e8fc2ce7cf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -75,7 +75,7 @@ // optionally disable interrupts during flash writes #ifndef STM32_FLASH_DISABLE_ISR -#define STM32_FLASH_DISABLE_ISR 0 +#define STM32_FLASH_DISABLE_ISR 1 #endif // the 2nd bank of flash needs to be handled differently From 1b6620b42fb9e152fcaf97e62862a504a13fd389 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 17:09:38 +1000 Subject: [PATCH 0518/3101] HAL_ChibiOS: added expected delays on flash writes this prevents a long loop internal error on disarm after a large amount of flash writes when armed on a board with flash storage --- libraries/AP_HAL_ChibiOS/Storage.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 4bcd9744c24..56bc9d822d2 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -346,6 +346,7 @@ void Storage::_flash_load(void) bool Storage::_flash_write(uint16_t line) { #ifdef STORAGE_FLASH_PAGE + EXPECT_DELAY_MS(1); return _flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE); #else return false; @@ -360,6 +361,7 @@ bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t * #ifdef STORAGE_FLASH_PAGE size_t base_address = hal.flash->getpageaddr(_flash_page+sector); for (uint8_t i=0; iwrite(base_address+offset, data, length)) { return true; } @@ -412,13 +414,10 @@ bool Storage::_flash_erase_sector(uint8_t sector) thread. We can't use EXPECT_DELAY_MS() as it checks we are in the main thread */ - ChibiOS::Scheduler *sched = (ChibiOS::Scheduler *)hal.scheduler; - sched->_expect_delay_ms(1000); + EXPECT_DELAY_MS(1000); if (hal.flash->erasepage(_flash_page+sector)) { - sched->_expect_delay_ms(0); return true; } - sched->_expect_delay_ms(0); hal.scheduler->delay(1); } return false; From 055ee6ba82830e653fa1a9be5529e1584fc16d53 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 17:10:20 +1000 Subject: [PATCH 0519/3101] HAL_ChibiOS: increase monitor thread stack by 512 this is needed to ensure that the log writes of MON and WDOG don't overflow monitor thread stack --- libraries/AP_HAL_ChibiOS/Scheduler.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 301ab7e824e..95a9ce4d55e 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -76,7 +76,7 @@ #endif #ifndef MONITOR_THD_WA_SIZE -#define MONITOR_THD_WA_SIZE 512 +#define MONITOR_THD_WA_SIZE 1024 #endif /* Scheduler implementation: */ From 093acc157707cad55067f49936f7f02ac04dd603 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 4 Apr 2022 18:50:34 +1000 Subject: [PATCH 0520/3101] HAL_ChibiOS: use 16 byte lines for flash storage on H7 this halves the number of flash writes needed, and makes flash storage twice as space efficient on H7 On H7 we need to write 32 bytes at a time to flash, which corresponds to 30 bytes of data in AP_FlashStorage. By using a 16 byte storage line we don't waste as much space --- libraries/AP_HAL_ChibiOS/Storage.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index 54577f67ef4..beccd623b18 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -30,7 +30,11 @@ // when using flash storage we use a small line size to make storage // compact and minimise the number of erase cycles needed #ifdef STORAGE_FLASH_PAGE +#if defined(STM32H7XX) +#define CH_STORAGE_LINE_SHIFT 4 +#else #define CH_STORAGE_LINE_SHIFT 3 +#endif #elif defined(USE_POSIX) && !defined(HAL_WITH_RAMTRON) #define CH_STORAGE_LINE_SHIFT 9 #else From 392e80001f977b9fa7669064dea0364227aaefcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Apr 2022 08:46:57 +1000 Subject: [PATCH 0521/3101] HAL_ChibiOS: fixed H7 flash storage this fixes the flash re-init problem when flash storage fills on H7. It was caused by rejecting writes where one or more of the 32 byte chunks was not all 0xff but was equal to the current data. That happens when writing to the sector header in AP_FlashStorage it also moves the interrupt disable inside the loop to allow for other interrupts to run between blocks --- libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 44 +++++++++++-------- 1 file changed, 25 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c index 9e8fc2ce7cf..efd31e632cb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -546,28 +546,37 @@ static bool stm32_flash_write_h7(uint32_t addr, const void *buf, uint32_t count) return false; } - // check for erasure - if (!stm32h7_check_all_ones(addr, count >> 2)) { - return false; - } + stm32_flash_unlock(); + bool success = true; + + while (count >= 32) { + const uint8_t *b2 = (const uint8_t *)addr; + // if the bytes already match then skip this chunk + if (memcmp(b, b2, 32) != 0) { + // check for erasure + if (!stm32h7_check_all_ones(addr, 8)) { + return false; + } #if STM32_FLASH_DISABLE_ISR - syssts_t sts = chSysGetStatusAndLockX(); + syssts_t sts = chSysGetStatusAndLockX(); #endif - stm32_flash_unlock(); - bool success = true; + bool ok = stm32h7_flash_write32(addr, b); - while (count >= 32) { - if (!stm32h7_flash_write32(addr, b)) { - success = false; - goto failed; - } +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif - // check contents - if (memcmp((void*)addr, b, 32) != 0) { - success = false; - goto failed; + if (!ok) { + success = false; + goto failed; + } + // check contents + if (memcmp((void*)addr, b, 32) != 0) { + success = false; + goto failed; + } } addr += 32; @@ -577,9 +586,6 @@ static bool stm32_flash_write_h7(uint32_t addr, const void *buf, uint32_t count) failed: stm32_flash_lock(); -#if STM32_FLASH_DISABLE_ISR - chSysRestoreStatusX(sts); -#endif return success; } From 934a125875176e80169bb80111c71aa051c4a948 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 15:40:39 +1100 Subject: [PATCH 0522/3101] Tools: add script which tells you how Copter.ap changes over time This is a global state object which we really want to get rid of - but while we have it it does contain some useful state. pbarker@bluebottle:~/rc/ardupilot(pr/du32-change)$ ./Tools/scripts/du32_change.py ~/rc/log32.bin 1648874490: Creating connection Original armed_with_airmode_switch: 0 Original auto_armed: 0 Original compass_mot: 0 Original gps_glitching: 0 Original in_arming_delay: 1 Original initialised: 1 Original initialised_params: 1 Original land_complete: 1 Original land_complete_maybe: 1 Original land_repo_active: 0 Original logging_started: 1 Original motor_interlock_switch: 0 Original motor_test: 0 Original new_radio_frame: 1 Original pre_arm_check: 1 Original pre_arm_rc_check: 1 Original prec_land_active: 0 Original rc_receiver_present: 1 Original system_time_set_unused: 0 Original throttle_zero: 1 Original unused1: 0 Original unused2: 0 Original unused3: 0 Original unused_was_simple_mode bit1: 0 Original unused_was_simple_mode bit2: 0 Original usb_connected_unused: 0 Original using_interlock: 0 2022-04-01 08:19:27.03: -in_arming_delay 2022-04-01 08:19:31.04: +auto_armed -throttle_zero 2022-04-01 08:19:38.04: -land_complete -land_complete_maybe 2022-04-01 08:20:34.29: +throttle_zero 2022-04-01 08:20:35.29: -auto_armed +land_complete +land_complete_maybe pbarker@bluebottle:~/rc/ardupilot(pr/du32-change)$ --- Tools/scripts/du32_change.py | 118 +++++++++++++++++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100755 Tools/scripts/du32_change.py diff --git a/Tools/scripts/du32_change.py b/Tools/scripts/du32_change.py new file mode 100755 index 00000000000..b0452b4c572 --- /dev/null +++ b/Tools/scripts/du32_change.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python + +""" +Parses a log file and shows how Copter's du32 changes over time + +AP_FLAKE8_CLEAN + +""" + +from __future__ import print_function + +import optparse +import sys +import time + +from pymavlink import mavutil + + +class DU32Change(object): + def __init__(self, master): + self.master = master + + def progress(self, text): + '''emit text with possible timestamps etc''' + print("%u: %s" % (time.time(), text)) + + def run(self): + + self.progress("Creating connection") + self.conn = mavutil.mavlink_connection(master) + + # this eas was generated from Copter.h's structure for ap_t: + bit_descriptions_list = [ + "unused1", + "unused_was_simple_mode bit1", + "unused_was_simple_mode bit2", + "pre_arm_rc_check", + "pre_arm_check", + "auto_armed", + "logging_started", + "land_complete", + "new_radio_frame", + "usb_connected_unused", + "rc_receiver_present", + "compass_mot", + "motor_test", + "initialised", + "land_complete_maybe", + "throttle_zero", + "system_time_set_unused", + "gps_glitching", + "using_interlock", + "land_repo_active", + "motor_interlock_switch", + "in_arming_delay", + "initialised_params", + "unused3", + "unused2", + "armed_with_airmode_switch", + "prec_land_active", + ] + bit_descriptions = {} + count = 0 + for bit in bit_descriptions_list: + bit_descriptions[bit] = count + count += 1 + + old_m = None + + desired_type = "DU32" + while True: + m = self.conn.recv_match(type=desired_type) + if m is None: + break + if m.Id != 7: + # 7 is LOG_DATA_ID from AP_Logger.h + continue + if old_m is not None and m.Value == old_m.Value: + continue + line = "" + if old_m is None: + for bit in sorted(bit_descriptions.keys()): + bit_set = m.Value & (1 << bit_descriptions[bit]) + if bit_set: + print("Original %s: 1" % bit) + else: + print("Original %s: 0" % bit) + else: + for bit in bit_descriptions.keys(): + old_bit_set = old_m.Value & (1 << bit_descriptions[bit]) + new_bit_set = m.Value & (1 << bit_descriptions[bit]) + if new_bit_set and not old_bit_set: + line += " +%s" % bit + elif not new_bit_set and old_bit_set: + line += " -%s" % bit + + timestamp = getattr(m, '_timestamp', 0.0) + formatted_timestamp = "%s.%02u" % ( + time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(timestamp)), + int(timestamp * 100.0) % 100) + + print("%s: %s" % (formatted_timestamp, line)) + old_m = m + + +if __name__ == '__main__': + parser = optparse.OptionParser("du32_change.py [options]") + + (opts, args) = parser.parse_args() + + if len(args) < 1: + parser.print_help() + sys.exit(1) + + master = args[0] + + tester = DU32Change(master) + tester.run() From 582eed12bf83e5797d09ca3f94b01253f365cd52 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 31 Mar 2022 11:33:38 +1100 Subject: [PATCH 0523/3101] autotest: correct extraction of fields from LogWrite messages --- Tools/autotest/common.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 1519e3604a8..517be7703f3 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2306,6 +2306,8 @@ def all_log_format_ids(self): continue if state == state_inside: line = re.sub("//.*", "", line) # trim comments + # cpp-style string concatenation: + line = re.sub(r'"\s*"', '', line) log_write_statement += line if re.match(r".*\);", line): log_write_statements.append(log_write_statement) From d2c3c578c2e4f5683c174b4fd51e541396ec321d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 21 Mar 2022 16:48:06 +1100 Subject: [PATCH 0524/3101] hwdef: add hwdef for CubeOrange-SimOnHardware --- .../CubeOrange-SimOnHardWare/defaults.parm | 57 +++++++++++++++++++ .../hwdef/CubeOrange-SimOnHardWare/hwdef.dat | 32 +++++++++++ 2 files changed, 89 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/defaults.parm new file mode 100644 index 00000000000..f04aacba7e9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/defaults.parm @@ -0,0 +1,57 @@ +AHRS_EKF_TYPE 10 + +ATC_RAT_YAW_P 0.09 +ATC_RAT_YAW_I 0.009 + +BATT_MONITOR 0 + +COMPASS_OFS_X 5 +COMPASS_OFS_Y 13 +COMPASS_OFS_Z -18 +COMPASS_OFS2_X 5 +COMPASS_OFS2_Y 13 +COMPASS_OFS2_Z -18 + +FENCE_RADIUS 150 + +FRAME_TYPE 0 +FRAME_CLASS 1 + +FS_THR_ENABLE 1 + +RC7_OPTION 7 + +FLTMODE1 7 +FLTMODE2 9 +FLTMODE3 6 +FLTMODE4 3 +FLTMODE5 5 +FLTMODE6 0 + +GPS_TYPE 100 + +INS_ACCOFFS_X 0.001 +INS_ACCOFFS_Y 0.001 +INS_ACCOFFS_Z 0.001 +INS_ACCSCAL_X 1.001 +INS_ACCSCAL_Y 1.001 +INS_ACCSCAL_Z 1.001 +INS_ACC2OFFS_X 0.001 +INS_ACC2OFFS_Y 0.001 +INS_ACC2OFFS_Z 0.001 +INS_ACC2SCAL_X 1.001 +INS_ACC2SCAL_Y 1.001 +INS_ACC2SCAL_Z 1.001 + +MOT_THST_EXPO 0.65 +MOT_THST_HOVER 0.39 +MOT_BAT_VOLT_MIN 9.6 +MOT_BAT_VOLT_MAX 12.8 + +SIM_MAG1_DEVID 97539 +SIM_BARO_RND 0 + +SIM_RATE_HZ 400 +SCHED_LOOP_RATE 400 + +BRD_RTC_TYPES 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/hwdef.dat new file mode 100644 index 00000000000..8228b5e156d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-SimOnHardWare/hwdef.dat @@ -0,0 +1,32 @@ +# Firmware suitable for flashing into a CubeOrange to experience Simulation-on-Hardware + +include ../CubeOrange/hwdef.dat + +env SIM_ENABLED 1 + +define INS_MAX_INSTANCES 2 +define HAL_COMPASS_MAX_SENSORS 2 + +define HAL_NAVEKF2_AVAILABLE 0 +define EK3_FEATURE_BODY_ODOM 0 +define EK3_FEATURE_EXTERNAL_NAV 0 +define EK3_FEATURE_DRAG_FUSION 0 +define HAL_ADSB_ENABLED 0 +define HAL_MOUNT_ENABLED 0 +define HAL_PROXIMITY_ENABLED 0 +define HAL_VISUALODOM_ENABLED 0 +define HAL_GENERATOR_ENABLED 0 +# define HAL_LOGGING_ENABLED 0 +define HAL_CRSF_TELEM_ENABLED 0 +#define OSD_ENABLED 0 + +define AP_MOTORS_FRAME_DEFAULT_ENABLED 0 +define AP_MOTORS_FRAME_QUAD_ENABLED 1 + +define LANDING_GEAR_ENABLED 0 +define HAL_MSP_OPTICALFLOW_ENABLED 0 +define HAL_SUPPORT_RCOUT_SERIAL 0 +define HAL_HOTT_TELEM_ENABLED 0 +# define HAL_WITH_DSP 0 + +AUTOBUILD_TARGETS Copter From 06b6632f309d5f58b53d7c8ae26d2aa3cce5c03c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Apr 2022 19:48:00 +1000 Subject: [PATCH 0525/3101] AP_Logger: fixed @SYS file logging the key fix is the reset of the fd to -1. Without that fix we only ever log @SYS/uarts.txt The timing change is needed to get the files out in a reasonable time. The function is actually getting called at 100Hz or less, not 1kHz (measured on MatekH743 copter at 400Hz). So we need to run it faster to get the files logged in a reasonable time --- libraries/AP_Logger/AP_Logger.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index db002f9ced7..7ff623f50f3 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -1532,14 +1532,15 @@ void AP_Logger::file_content_update(FileContent &file_content) return; } - /* this function is called at max 1kHz. We don't want to saturate - the logging with file data, so we reduce the frequency of 64 - byte file writes by a factor of 100. For the file - crash_dump.bin we dump 10x faster so we get it in a reasonable - time (full dump of 450k in about 1 minute) + /* this function is called at around 100Hz on average (tested on + 400Hz copter). We don't want to saturate the logging with file + data, so we reduce the frequency of 64 byte file writes by a + factor of 10. For the file crash_dump.bin we dump 10x faster so + we get it in a reasonable time (full dump of 450k in about 1 + minute) */ file_content.counter++; - const uint8_t frequency = file_content.fast?10:100; + const uint8_t frequency = file_content.fast?1:10; if (file_content.counter % frequency != 0) { return; } @@ -1565,6 +1566,7 @@ void AP_Logger::file_content_update(FileContent &file_content) const auto length = AP::FS().read(file_content.fd, pkt.data, sizeof(pkt.data)); if (length <= 0) { AP::FS().close(file_content.fd); + file_content.fd = -1; file_content.remove_and_free(file); return; } From 5539b55c01e5d6650cc2ab0672e27df8334c1cc6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 17:34:41 +1000 Subject: [PATCH 0526/3101] Copter: factor failsafe reporting --- ArduCopter/Copter.h | 1 + ArduCopter/events.cpp | 41 +++++++++++++++++++++++++---------------- 2 files changed, 26 insertions(+), 16 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f6035075d3a..5bbdd75a52e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -743,6 +743,7 @@ class Copter : public AP_Vehicle { void set_mode_auto_do_land_start_or_RTL(ModeReason reason); bool should_disarm_on_failsafe(); void do_failsafe_action(FailsafeAction action, ModeReason reason); + void announce_failsafe(const char *type, const char *action_undertaken=nullptr); // failsafe.cpp void failsafe_enable(); diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 3cab93e1aee..ed207d10d12 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -43,33 +43,33 @@ void Copter::failsafe_radio_on_event() // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning if (should_disarm_on_failsafe()) { // should immediately disarm when we're on the ground - gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming"); + announce_failsafe("Radio", "Disarming"); arming.disarm(AP_Arming::Method::RADIOFAILSAFE); desired_action = FailsafeAction::NONE; } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { // Allow landing to continue when battery failsafe requires it (not a user option) - gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing"); + announce_failsafe("Radio + Battery", "Continuing Landing"); desired_action = FailsafeAction::LAND; } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { // Allow landing to continue when FS_OPTIONS is set to continue landing - gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing"); + announce_failsafe("Radio", "Continuing Landing"); desired_action = FailsafeAction::LAND; } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) { // Allow mission to continue when FS_OPTIONS is set to continue mission - gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode"); + announce_failsafe("Radio", "Continuing Auto"); desired_action = FailsafeAction::NONE; } else if ((flightmode->in_guided_mode()) && (failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) && (g.failsafe_gcs != FS_GCS_DISABLED)) { // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode. Only if the GCS failsafe is enabled. - gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode"); + announce_failsafe("Radio", "Continuing Guided Mode"); desired_action = FailsafeAction::NONE; } else { - gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe"); + announce_failsafe("Radio"); } // Call the failsafe action handler @@ -85,6 +85,15 @@ void Copter::failsafe_radio_off_event() gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); } +void Copter::announce_failsafe(const char *type, const char *action_undertaken) +{ + if (action_undertaken != nullptr) { + gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe - %s", type, action_undertaken); + } else { + gcs().send_text(MAV_SEVERITY_WARNING, "%s Failsafe", type); + } +} + void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) { AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); @@ -96,14 +105,14 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) // should immediately disarm when we're on the ground arming.disarm(AP_Arming::Method::BATTERYFAILSAFE); desired_action = FailsafeAction::NONE; - gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming"); + announce_failsafe("Battery", "Disarming"); } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != FailsafeAction::NONE) { // Allow landing to continue when FS_OPTIONS is set to continue when landing desired_action = FailsafeAction::LAND; - gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing"); + announce_failsafe("Battery", "Continuing Landing"); } else { - gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe"); + announce_failsafe("Battery"); } // Battery FS options already use the Failsafe_Options enum. So use them directly. @@ -183,35 +192,35 @@ void Copter::failsafe_gcs_on_event(void) // Conditions to deviate from FS_GCS_ENABLE parameter setting if (!motors->armed()) { desired_action = FailsafeAction::NONE; - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); + announce_failsafe("GCS"); } else if (should_disarm_on_failsafe()) { // should immediately disarm when we're on the ground arming.disarm(AP_Arming::Method::GCSFAILSAFE); desired_action = FailsafeAction::NONE; - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming"); + announce_failsafe("GCS", "Disarming"); } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) { // Allow landing to continue when battery failsafe requires it (not a user option) - gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing"); + announce_failsafe("GCS + Battery", "Continuing Landing"); desired_action = FailsafeAction::LAND; } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) { // Allow landing to continue when FS_OPTIONS is set to continue landing - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing"); + announce_failsafe("GCS", "Continuing Landing"); desired_action = FailsafeAction::LAND; } else if (flightmode->mode_number() == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) { // Allow mission to continue when FS_OPTIONS is set to continue mission - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode"); + announce_failsafe("GCS", "Continuing Auto Mode"); desired_action = FailsafeAction::NONE; } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) { // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control"); + announce_failsafe("GCS", "Continuing Pilot Control"); desired_action = FailsafeAction::NONE; } else { - gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe"); + announce_failsafe("GCS"); } // Call the failsafe action handler From 2d37bc9c786f5947274a1dddf464f4c6261730fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E5=AD=9F=E6=99=93?= Date: Wed, 30 Mar 2022 01:06:30 +0000 Subject: [PATCH 0527/3101] AP_HAL_ChibiOS: CUAV-Nora: add usart3 support --- libraries/AP_HAL_ChibiOS/hwdef/CUAV-Nora/hwdef.dat | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-Nora/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-Nora/hwdef.dat index 7a4d48d1697..bfb42fe0cbe 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/CUAV-Nora/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CUAV-Nora/hwdef.dat @@ -21,7 +21,7 @@ STM32_ST_USE_TIMER 2 FLASH_RESERVE_START_KB 128 # order of UARTs (and USB) -SERIAL_ORDER OTG1 USART2 USART6 USART1 UART4 UART8 UART7 OTG2 +SERIAL_ORDER OTG1 USART2 USART6 USART1 UART4 UART8 UART7 USART3 OTG2 # default the 2nd interface to MAVLink2 until MissionPlanner updates drivers define HAL_OTG2_PROTOCOL SerialProtocol_MAVLink2 @@ -157,6 +157,10 @@ PE8 UART7_TX UART7 NODMA PE0 UART8_RX UART8 PE1 UART8_TX UART8 +# USART3 +PD8 USART3_TX USART3 NODMA +PD9 USART3_RX USART3 NODMA + # PWM AUX channels PH10 TIM5_CH1 TIM5 PWM(1) GPIO(50) PH11 TIM5_CH2 TIM5 PWM(2) GPIO(51) From 10d7ea9358990eca1a78dc76b8561a62153b71a4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Apr 2022 11:18:32 +1000 Subject: [PATCH 0528/3101] Tools: recognise "AUTOBUILD_TARGETS None" and make for easier testing on command line --- Tools/scripts/board_list.py | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index b3d3c8d1522..d6a20ca33fa 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -89,9 +89,13 @@ def __init__(self): # a hwdef can specify which vehicles this target is valid for: match = re.match(r"AUTOBUILD_TARGETS\s*(.*)", line) if match is not None: - board.autobuild_targets = [ - x.rstrip().lstrip().lower() for x in match.group(1).split(",") - ] + mname = match.group(1) + if mname.lower() == 'none': + board.autobuild_targets = [] + else: + board.autobuild_targets = [ + x.rstrip().lstrip().lower() for x in mname.split(",") + ] def read_hwdef(self, filepath): fh = open(filepath) @@ -121,12 +125,6 @@ def find_autobuild_boards(self, build_target=None): "iomcu", 'iomcu_f103_8MHz', - # evaluation boards - 'H757I_EVAL', - 'H757I_EVAL_intf', - "Nucleo-G491", - "NucleoH743", - # bdshot "CubeYellow-bdshot", "fmuv3-bdshot", @@ -168,11 +166,6 @@ def find_ap_periph_boards(self): "G4-ESC", "HereID", "HerePro", - - # evaluation boards - "H757I_EVAL", - "Nucleo-L476", - "Nucleo-L496", ] ret = [] for x in self.boards: @@ -186,3 +179,11 @@ def find_ap_periph_boards(self): AUTOBUILD_BOARDS = BoardList().find_autobuild_boards() AP_PERIPH_BOARDS = BoardList().find_ap_periph_boards() + +if __name__ == '__main__': + import sys + if len(sys.argv) < 2: + print("Usage: board_list.py TARGET") + sys.exit(1) + board_list = BoardList() + print(sorted(board_list.find_autobuild_boards(sys.argv[1]))) From c9bbfd94c0bcd6dce19a787835720c57fca8432d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Apr 2022 11:18:48 +1000 Subject: [PATCH 0529/3101] hwdef: set AUTOBUILD_TARGETS None on eval boards --- libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL_intf/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/Nucleo-G491/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L476/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L496/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat | 2 ++ 6 files changed, 12 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL/hwdef.dat index 20714712df3..275ec38ea78 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL/hwdef.dat @@ -1,6 +1,8 @@ # hw definition file for processing by chibios_hwdef.py # for H757 +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32H7xx STM32H757xx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL_intf/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL_intf/hwdef.dat index 828d25435a3..6fe12660389 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL_intf/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/H757I_EVAL_intf/hwdef.dat @@ -1,6 +1,8 @@ # hw definition file for processing by chibios_hwdef.py # for H757 +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32H7xx STM32H757xx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-G491/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-G491/hwdef.dat index f29ec3ed2b8..1063dedd660 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-G491/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-G491/hwdef.dat @@ -1,5 +1,7 @@ # hw definition file for processing by chibios_pins.py +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32G491 STM32G491xx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L476/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L476/hwdef.dat index cbd13e18b27..7af93b68cf2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L476/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L476/hwdef.dat @@ -1,5 +1,7 @@ # hw definition file for processing by chibios_pins.py +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32L476 STM32L476xx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L496/hwdef.dat index a1beacc03ac..2c30a5dc47f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Nucleo-L496/hwdef.dat @@ -1,5 +1,7 @@ # hw definition file for processing by chibios_pins.py +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32L496 STM32L496xx diff --git a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat index 5e28cecc9b5..9b4be799ec8 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH743/hwdef.dat @@ -1,6 +1,8 @@ # hw definition file for processing by chibios_hwdef.py # for H743 bootloader +AUTOBUILD_TARGETS None + # MCU class and specific type MCU STM32H7xx STM32H743xx From 37d8feb928086d7d8a8e058a4155d56c53c2cf84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Apr 2022 10:48:31 +1000 Subject: [PATCH 0530/3101] hwdef: disable GPS drivers on low flash boards only leave uBlox enabled on boards that are running out of flash --- libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/MambaF405v2/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/fmuv2/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/include/minimal_GPS.inc | 5 +++++ libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat | 3 +++ libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat | 3 +++ 10 files changed, 32 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat index 2b654dac4fb..79044ed95dd 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/F35Lightning/hwdef.dat @@ -161,5 +161,8 @@ define HAL_BATT_CURR_PIN 12 define HAL_BATT_VOLT_SCALE 11 define HAL_BATT_CURR_SCALE 25 +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc + # enable IMU fast sampling define HAL_DEFAULT_INS_FAST_SAMPLE 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat index f46e377fcac..7ecc7746719 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat @@ -147,3 +147,6 @@ define HAL_LOGGING_DATAFLASH_ENABLED 1 define OSD_ENABLED 1 define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405v2/hwdef.dat index 6c3385a2284..81900704a36 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MambaF405v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MambaF405v2/hwdef.dat @@ -153,3 +153,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # To complementary channels work we define this #define STM32_PWM_USE_ADVANCED TRUE + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat index d68019ed64d..a13f998eb9e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/OmnibusNanoV6/hwdef.dat @@ -150,3 +150,6 @@ define STM32_PWM_USE_ADVANCED TRUE ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin define HAL_MOUNT_ENABLED 0 + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat index 429f37c7317..dc84b5bbe24 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat @@ -131,3 +131,6 @@ define HAL_SPRAYER_ENABLED 0 # reduce max size of embedded params for apj_tool.py define AP_PARAM_MAX_EMBEDDED_PARAM 1024 + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat index 7fc8e316d51..8b1bc8fb3d3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat @@ -180,3 +180,6 @@ define HAL_RUNCAM_ENABLED 0 define HAL_SPEKTRUM_TELEM_ENABLED 0 define HAL_SOARING_ENABLED 0 define AP_OPTICALFLOW_ENABLED 0 + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv2/hwdef.dat index f07eddc94e2..fdff9065575 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv2/hwdef.dat @@ -13,3 +13,6 @@ define HAL_MINIMIZE_FEATURES 1 # we don't have a flash page spare to write parameters to: undef STORAGE_FLASH_PAGE + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_GPS.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_GPS.inc new file mode 100644 index 00000000000..e5e18f3d682 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_GPS.inc @@ -0,0 +1,5 @@ +# include file to reduce flash by including less GPS drivers + +define AP_GPS_BACKEND_DEFAULT_ENABLED 0 +define AP_GPS_UBLOX_ENABLED 1 + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat index 9413ed81bd1..42c9137c9c0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/omnibusf4v6/hwdef.dat @@ -149,3 +149,6 @@ define STM32_PWM_USE_ADVANCED TRUE define OSD_ENABLED 1 #font for the osd ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat index e0048a9b3f8..f8ffafeb71f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/speedybeef4/hwdef.dat @@ -145,3 +145,6 @@ define HAL_LOGGING_DATAFLASH_ENABLED 1 define OSD_ENABLED 1 define HAL_OSD_TYPE_DEFAULT 1 ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc From 717edae9efb9c3f477e4dbbcb7f51256ca81bbff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Apr 2022 10:37:47 +1000 Subject: [PATCH 0531/3101] hwdef: fix redefinition of Solo values in CubeSolo/CubeGreen-Solo adding fmuv3 into the list of firmwares we might run on Solo was a late addition to the recent PRs. It was forgotten that the inheritance would cause a redefition failure --- libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat | 3 +++ 2 files changed, 5 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat index 12804f9f7d6..385e689ac18 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat @@ -10,8 +10,10 @@ include ../CubeBlack/hwdef.dat # these are parameters the Solo requires for proper operation that are different from the 4 standard defaults. env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param' +undef HAL_OREO_LED_ENABLED define HAL_OREO_LED_ENABLED 1 +undef HAL_SOLO_GIMBAL_ENABLED define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat index 696118b716d..c16fb1fa6fe 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat @@ -49,9 +49,12 @@ COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 # also probe for external compasses define HAL_PROBE_EXTERNAL_I2C_COMPASSES +undef HAL_OREO_LED_ENABLED define HAL_OREO_LED_ENABLED 1 + define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5 +undef HAL_SOLO_GIMBAL_ENABLED define HAL_SOLO_GIMBAL_ENABLED HAL_MOUNT_ENABLED AUTOBUILD_TARGETS Copter From 0a0178988de37079a52da936323a900c7625093f Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 00:24:55 +0000 Subject: [PATCH 0532/3101] AP_Logger: add 1e-9 log multiplyer --- libraries/AP_Logger/LogStructure.h | 1 + libraries/AP_Logger/README.md | 1 + 2 files changed, 2 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 46a90002872..a106cdee073 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -101,6 +101,7 @@ const struct MultiplierStructure log_Multipliers[] = { { 'E', 1e-5 }, { 'F', 1e-6 }, { 'G', 1e-7 }, + { 'I', 1e-9 }, // { '!', 3.6 }, // (ampere*second => milliampere*hour) and (km/h => m/s) { '/', 3600 }, // (ampere*second => ampere*hour) diff --git a/libraries/AP_Logger/README.md b/libraries/AP_Logger/README.md index f1afd1ec9b7..dc42f962946 100644 --- a/libraries/AP_Logger/README.md +++ b/libraries/AP_Logger/README.md @@ -100,5 +100,6 @@ tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name | 'E' | 1e-5 || | 'F' | 1e-6 || | 'G' | 1e-7 || +| 'I' | 1e-9 || | '!' | 3.6 | (milliampere \* hour => ampere \* second) and (km/h => m/s)| | '/' | 3600 | (ampere \* hour => ampere \* second)| From 2589fa1b3b495c61049dfc288e68dd8940d5c225 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 7 Mar 2022 00:24:31 +0000 Subject: [PATCH 0533/3101] AP_GPS: UBlox: add TIM-TM2 logging --- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 72 ++++++++++++++++++++++++++++++- libraries/AP_GPS/AP_GPS_UBLOX.h | 27 +++++++++++- 2 files changed, 96 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 3610dfc4764..5f8abfee5ec 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -392,6 +392,15 @@ AP_GPS_UBLOX::_request_next_config(void) } } } +#endif + break; + case STEP_TIM_TM2: +#if UBLOX_TIM_TM2_LOGGING + if(!_request_message_rate(CLASS_TIM, MSG_TIM_TM2)) { + _next_message--; + } +#else + _unconfigured_messages &= ~CONFIG_TIM_TM2; #endif break; default: @@ -470,6 +479,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { } break; #endif // UBLOX_RXM_RAW_LOGGING +#if UBLOX_TIM_TM2_LOGGING + case CLASS_TIM: + if (msg_id == MSG_TIM_TM2) { + desired_rate = RATE_TIM_TM2; + config_msg_id = CONFIG_TIM_TM2; + break; + } + return; +#endif // UBLOX_TIM_TM2_LOGGING default: return; } @@ -723,6 +741,50 @@ void AP_GPS_UBLOX::log_mon_hw2(void) #endif } +#if UBLOX_TIM_TM2_LOGGING +void AP_GPS_UBLOX::log_tim_tm2(void) +{ +#if HAL_LOGGING_ENABLED + if (!should_log()) { + return; + } + +// @LoggerMessage: UBXT +// @Description: uBlox specific UBX-TIM-TM2 logging, see uBlox interface description +// @Field: TimeUS: Time since system startup +// @Field: I: GPS instance number +// @Field: ch: Channel (i.e. EXTINT) upon which the pulse was measured +// @Field: flags: Bitmask +// @Field: count: Rising edge counter +// @Field: wnR: Week number of last rising edge +// @Field: MsR: Tow of rising edge +// @Field: SubMsR: Millisecond fraction of tow of rising edge in nanoseconds +// @Field: wnF: Week number of last falling edge +// @Field: MsF: Tow of falling edge +// @Field: SubMsF: Millisecond fraction of tow of falling edge in nanoseconds +// @Field: accEst: Accuracy estimate + + AP::logger().WriteStreaming("UBXT", + "TimeUS,I,ch,flags,count,wnR,MsR,SubMsR,wnF,MsF,SubMsF,accEst", + "s#----ss-sss", + "F-----CI-CII", + "QBBBHHIIHIII", + AP_HAL::micros64(), + state.instance, + _buffer.tim_tm2.ch, + _buffer.tim_tm2.flags, + _buffer.tim_tm2.count, + _buffer.tim_tm2.wnR, + _buffer.tim_tm2.towMsR, + _buffer.tim_tm2.towSubMsR, + _buffer.tim_tm2.wnF, + _buffer.tim_tm2.towMsF, + _buffer.tim_tm2.towSubMsF, + _buffer.tim_tm2.accEst); +#endif +} +#endif // UBLOX_TIM_TM2_LOGGING + #if UBLOX_RXM_RAW_LOGGING void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw) { @@ -1199,6 +1261,13 @@ AP_GPS_UBLOX::_parse_gps(void) } #endif // UBLOX_RXM_RAW_LOGGING +#if UBLOX_TIM_TM2_LOGGING + if ((_class == CLASS_TIM) && (_msg_id == MSG_TIM_TM2) && (_payload_length == 28)) { + log_tim_tm2(); + return false; + } +#endif // UBLOX_TIM_TM2_LOGGING + if (_class != CLASS_NAV) { unexpected_message(); return false; @@ -1828,7 +1897,8 @@ static const char *reasons[] = {"navigation rate", "time pulse settings", "TIMEGPS rate", "Time mode settings", - "RTK MB"}; + "RTK MB", + "TIM TM2"}; static_assert((1 << ARRAY_SIZE(reasons)) == CONFIG_LAST, "UBLOX: Missing configuration description"); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index a0b3a6bbb77..4afef3d92f2 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -49,6 +49,9 @@ #define UBLOX_MAX_RXM_RAW_SATS 22 #define UBLOX_MAX_RXM_RAWX_SATS 32 #define UBLOX_GNSS_SETTINGS 1 +#ifndef UBLOX_TIM_TM2_LOGGING + #define UBLOX_TIM_TM2_LOGGING (BOARD_FLASH_SIZE>1024) +#endif #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7 @@ -65,6 +68,7 @@ #define RATE_DOP 1 #define RATE_HW 5 #define RATE_HW2 5 +#define RATE_TIM_TM2 1 #define CONFIG_RATE_NAV (1<<0) #define CONFIG_RATE_POSLLH (1<<1) @@ -84,7 +88,8 @@ #define CONFIG_RATE_TIMEGPS (1<<15) #define CONFIG_TMODE_MODE (1<<16) #define CONFIG_RTK_MOVBASE (1<<17) -#define CONFIG_LAST (1<<18) // this must always be the last bit +#define CONFIG_TIM_TM2 (1<<18) +#define CONFIG_LAST (1<<19) // this must always be the last bit #define CONFIG_REQUIRED_INITIAL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_VELNED) @@ -516,6 +521,19 @@ class AP_GPS_UBLOX : public AP_GPS_Backend uint32_t loadMask; }; + struct PACKED ubx_tim_tm2 { + uint8_t ch; + uint8_t flags; + uint16_t count; + uint16_t wnR; + uint16_t wnF; + uint32_t towMsR; + uint32_t towSubMsR; + uint32_t towMsF; + uint32_t towSubMsF; + uint32_t accEst; + }; + // Receive buffer union PACKED { DEFINE_BYTE_ARRAY_METHODS @@ -548,6 +566,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend ubx_rxm_rawx rxm_rawx; #endif ubx_ack_ack ack; + ubx_tim_tm2 tim_tm2; } _buffer; enum class RELPOSNED { @@ -573,6 +592,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend CLASS_CFG = 0x06, CLASS_MON = 0x0A, CLASS_RXM = 0x02, + CLASS_TIM = 0x0d, MSG_ACK_NACK = 0x00, MSG_ACK_ACK = 0x01, MSG_POSLLH = 0x2, @@ -598,7 +618,8 @@ class AP_GPS_UBLOX : public AP_GPS_Backend MSG_MON_VER = 0x04, MSG_NAV_SVINFO = 0x30, MSG_RXM_RAW = 0x10, - MSG_RXM_RAWX = 0x15 + MSG_RXM_RAWX = 0x15, + MSG_TIM_TM2 = 0x03 }; enum ubx_gnss_identifier { GNSS_GPS = 0x00, @@ -655,6 +676,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend STEP_RAWX, STEP_VERSION, STEP_RTK_MOVBASE, // setup moving baseline + STEP_TIM_TM2, STEP_LAST }; @@ -728,6 +750,7 @@ class AP_GPS_UBLOX : public AP_GPS_Backend void unexpected_message(void); void log_mon_hw(void); void log_mon_hw2(void); + void log_tim_tm2(void); void log_rxm_raw(const struct ubx_rxm_raw &raw); void log_rxm_rawx(const struct ubx_rxm_rawx &raw); From 4332ed8f36838567b903c286e0940bb31d604f99 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 19 Mar 2022 14:48:48 +1100 Subject: [PATCH 0534/3101] AP_GPS: use sq function for squaring numbers --- libraries/AP_GPS/AP_GPS.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 2ccdafe970d..481d38a8091 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1662,7 +1662,7 @@ bool AP_GPS::calc_blend_weights(void) for (uint8_t i=0; i= GPS_OK_FIX_3D) { if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) { - speed_accuracy_sum_sq += state[i].speed_accuracy * state[i].speed_accuracy; + speed_accuracy_sum_sq += sq(state[i].speed_accuracy); } else { // not all receivers support this metric so set it to zero and don't use it speed_accuracy_sum_sq = 0.0f; @@ -1678,7 +1678,7 @@ bool AP_GPS::calc_blend_weights(void) for (uint8_t i=0; i= GPS_OK_FIX_2D) { if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) { - horizontal_accuracy_sum_sq += state[i].horizontal_accuracy * state[i].horizontal_accuracy; + horizontal_accuracy_sum_sq += sq(state[i].horizontal_accuracy); } else { // not all receivers support this metric so set it to zero and don't use it horizontal_accuracy_sum_sq = 0.0f; @@ -1694,7 +1694,7 @@ bool AP_GPS::calc_blend_weights(void) for (uint8_t i=0; i= GPS_OK_FIX_3D) { if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f) { - vertical_accuracy_sum_sq += state[i].vertical_accuracy * state[i].vertical_accuracy; + vertical_accuracy_sum_sq += sq(state[i].vertical_accuracy); } else { // not all receivers support this metric so set it to zero and don't use it vertical_accuracy_sum_sq = 0.0f; @@ -1720,7 +1720,7 @@ bool AP_GPS::calc_blend_weights(void) float sum_of_hpos_weights = 0.0f; for (uint8_t i=0; i= GPS_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) { - hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (state[i].horizontal_accuracy * state[i].horizontal_accuracy); + hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(state[i].horizontal_accuracy); sum_of_hpos_weights += hpos_blend_weights[i]; } } @@ -1740,7 +1740,7 @@ bool AP_GPS::calc_blend_weights(void) float sum_of_vpos_weights = 0.0f; for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) { - vpos_blend_weights[i] = vertical_accuracy_sum_sq / (state[i].vertical_accuracy * state[i].vertical_accuracy); + vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(state[i].vertical_accuracy); sum_of_vpos_weights += vpos_blend_weights[i]; } } @@ -1760,7 +1760,7 @@ bool AP_GPS::calc_blend_weights(void) float sum_of_spd_weights = 0.0f; for (uint8_t i=0; i= GPS_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) { - spd_blend_weights[i] = speed_accuracy_sum_sq / (state[i].speed_accuracy * state[i].speed_accuracy); + spd_blend_weights[i] = speed_accuracy_sum_sq / sq(state[i].speed_accuracy); sum_of_spd_weights += spd_blend_weights[i]; } } From c2112565b5e1c57aa9eacd99d6c48d0de2429bf3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 19 Mar 2022 14:56:03 +1100 Subject: [PATCH 0535/3101] AP_GPS: remove redundant _blend_mask check filling in of these values is already gated by checking the mask --- libraries/AP_GPS/AP_GPS.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 481d38a8091..e2e0d623d80 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1715,7 +1715,7 @@ bool AP_GPS::calc_blend_weights(void) // calculate a weighting using the reported horizontal position float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; - if (horizontal_accuracy_sum_sq > 0.0f && (_blend_mask & BLEND_MASK_USE_HPOS_ACC)) { + if (horizontal_accuracy_sum_sq > 0.0f) { // calculate the weights using the inverse of the variances float sum_of_hpos_weights = 0.0f; for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_VPOS_ACC)) { + if (vertical_accuracy_sum_sq > 0.0f) { // calculate the weights using the inverse of the variances float sum_of_vpos_weights = 0.0f; for (uint8_t i=0; i 0.0f && (_blend_mask & BLEND_MASK_USE_SPD_ACC)) { + if (speed_accuracy_sum_sq > 0.0f) { // calculate the weights using the inverse of the variances float sum_of_spd_weights = 0.0f; for (uint8_t i=0; i Date: Tue, 5 Apr 2022 13:33:32 +1000 Subject: [PATCH 0536/3101] AP_AHRS: remove instance id from EK3 external interface Removes passing of instance id in interfaces where -1 was the only value ever passed in --- libraries/AP_AHRS/AP_AHRS.cpp | 50 +++++++++++++++++------------------ 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 6cbb8edc6d7..77ab99942b6 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -569,7 +569,7 @@ void AP_AHRS::update_EKF3(void) if (active_EKF_type() == EKFType::THREE) { Vector3f eulers; EKF3.getRotationBodyToNED(_dcm_matrix); - EKF3.getEulerAngles(-1,eulers); + EKF3.getEulerAngles(eulers); roll = eulers.x; pitch = eulers.y; yaw = eulers.z; @@ -615,7 +615,7 @@ void AP_AHRS::update_EKF3(void) } _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()]; nav_filter_status filt_state; - EKF3.getFilterStatus(-1,filt_state); + EKF3.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); } } @@ -828,7 +828,7 @@ Vector3f AP_AHRS::wind_estimate(void) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getWind(-1,wind); + EKF3.getWind(wind); break; #endif @@ -918,7 +918,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - ret = EKF3.getWind(-1,wind_vel); + ret = EKF3.getWind(wind_vel); break; #endif @@ -992,7 +992,7 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - return EKF3.getAirSpdVec(-1, vec); + return EKF3.getAirSpdVec(vec); #endif #if AP_AHRS_SIM_ENABLED @@ -1071,7 +1071,7 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const if (!_ekf3_started) { return false; } - EKF3.getQuaternion(-1, quat); + EKF3.getQuaternion(quat); break; #endif #if AP_AHRS_SIM_ENABLED @@ -1123,7 +1123,7 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // EKF3 is secondary - EKF3.getEulerAngles(-1, eulers); + EKF3.getEulerAngles(eulers); return _ekf3_started; #endif @@ -1184,7 +1184,7 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const if (!_ekf3_started) { return false; } - EKF3.getQuaternion(-1, quat); + EKF3.getQuaternion(quat); break; #endif @@ -1269,7 +1269,7 @@ Vector2f AP_AHRS::groundspeed_vector(void) #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getVelNED(-1,vec); + EKF3.getVelNED(vec); return Vector2f(vec.x, vec.y); #endif @@ -1382,7 +1382,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getVelNED(-1,vec); + EKF3.getVelNED(vec); return true; #endif @@ -1419,7 +1419,7 @@ bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getMagNED(-1,vec); + EKF3.getMagNED(vec); return true; #endif @@ -1450,7 +1450,7 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getMagXYZ(-1,vec); + EKF3.getMagXYZ(vec); return true; #endif @@ -1483,7 +1483,7 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - velocity = EKF3.getPosDownDerivative(-1); + velocity = EKF3.getPosDownDerivative(); return true; #endif @@ -1570,7 +1570,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const case EKFType::THREE: { Vector2f posNE; float posD; - if (EKF3.getPosNE(-1,posNE) && EKF3.getPosD(-1,posD)) { + if (EKF3.getPosNE(posNE) && EKF3.getPosD(posD)) { // position is valid vec.x = posNE.x; vec.y = posNE.y; @@ -1653,7 +1653,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { - bool position_is_valid = EKF3.getPosNE(-1,posNE); + bool position_is_valid = EKF3.getPosNE(posNE); return position_is_valid; } #endif @@ -1723,7 +1723,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { - bool position_is_valid = EKF3.getPosD(-1,posD); + bool position_is_valid = EKF3.getPosD(posD); return position_is_valid; } #endif @@ -1860,7 +1860,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const } if (always_use_EKF()) { uint16_t ekf3_faults; - EKF3.getFilterFaults(-1,ekf3_faults); + EKF3.getFilterFaults(ekf3_faults); if (ekf3_faults == 0) { ret = EKFType::THREE; } @@ -1903,7 +1903,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const #endif #if HAL_NAVEKF3_AVAILABLE if (ret == EKFType::THREE) { - EKF3.getFilterStatus(-1,filt_state); + EKF3.getFilterStatus(filt_state); should_use_gps = EKF3.configuredToUseGPSForPosXY(); } #endif @@ -2161,7 +2161,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - EKF3.getFilterStatus(-1,status); + EKF3.getFilterStatus(status); return true; #endif @@ -2711,7 +2711,7 @@ bool AP_AHRS::get_origin(Location &ret) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - if (!EKF3.getOriginLLH(-1,ret)) { + if (!EKF3.getOriginLLH(ret)) { return false; } return true; @@ -2817,7 +2817,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f & #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // use EKF to get innovations - return EKF3.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); + return EKF3.getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); #endif #if AP_AHRS_SIM_ENABLED @@ -2845,7 +2845,7 @@ bool AP_AHRS::is_vibration_affected() const switch (ekf_type()) { #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - return EKF3.isVibrationAffected(-1); + return EKF3.isVibrationAffected(); #endif case EKFType::NONE: #if HAL_NAVEKF2_AVAILABLE @@ -2885,7 +2885,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3 case EKFType::THREE: { // use EKF to get variance Vector2f offset; - return EKF3.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); + return EKF3.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); } #endif @@ -2926,7 +2926,7 @@ bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vecto #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: // use EKF to get variance - return EKF3.getVelInnovationsAndVariancesForSource(-1, (AP_NavEKF_Source::SourceXY)source, innovations, variances); + return EKF3.getVelInnovationsAndVariancesForSource((AP_NavEKF_Source::SourceXY)source, innovations, variances); #endif #if AP_AHRS_SIM_ENABLED @@ -2956,7 +2956,7 @@ uint8_t AP_AHRS::get_active_airspeed_index() const // we only have affinity for EKF3 as of now #if HAL_NAVEKF3_AVAILABLE if (active_EKF_type() == EKFType::THREE) { - uint8_t ret = EKF3.getActiveAirspeed(get_primary_core_index()); + uint8_t ret = EKF3.getActiveAirspeed(); if (ret != 255 && airspeed->healthy(ret)) { return ret; } From adf9c21d4812ef4d4bcb555e99179573aec765c6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 13:33:33 +1000 Subject: [PATCH 0537/3101] AP_NavEKF3: remove instance id from EK3 external interface Removes passing of instance id in interfaces where -1 was the only value ever passed in --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 115 +++++++++++----------------- libraries/AP_NavEKF3/AP_NavEKF3.h | 90 +++++++++------------- 2 files changed, 83 insertions(+), 122 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 815c113b1c8..5d52cfc36fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1154,55 +1154,49 @@ int8_t NavEKF3::getPrimaryCoreIMUIndex(void) const // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF3::getPosNE(int8_t instance, Vector2f &posNE) const +bool NavEKF3::getPosNE(Vector2f &posNE) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getPosNE(posNE); + return core[primary].getPosNE(posNE); } // Write the last calculated D position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF3::getPosD(int8_t instance, float &posD) const +bool NavEKF3::getPosD(float &posD) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getPosD(posD); + return core[primary].getPosD(posD); } // return NED velocity in m/s -void NavEKF3::getVelNED(int8_t instance, Vector3f &vel) const +void NavEKF3::getVelNED(Vector3f &vel) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getVelNED(vel); + core[primary].getVelNED(vel); } } -// return estimate of true airspeed vector in body frame in m/s for the specified instance -// An out of range instance (eg -1) returns data for the primary instance +// return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable -bool NavEKF3::getAirSpdVec(int8_t instance, Vector3f &vel) const +bool NavEKF3::getAirSpdVec(Vector3f &vel) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - return core[instance].getAirSpdVec(vel); + return core[primary].getAirSpdVec(vel); } return false; } // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s -float NavEKF3::getPosDownDerivative(int8_t instance) const +float NavEKF3::getPosDownDerivative() const { - if (instance < 0 || instance >= num_cores) instance = primary; // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration if (core) { - return core[instance].getPosDownDerivative(); + return core[primary].getPosDownDerivative(); } return 0.0f; } @@ -1273,40 +1267,35 @@ void NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // returns true if wind state estimation is active -bool NavEKF3::getWind(int8_t instance, Vector3f &wind) const +bool NavEKF3::getWind(Vector3f &wind) const { - bool ret = false; - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - ret = core[instance].getWind(wind); + if (core == nullptr) { + return false; } - return ret; + return core[primary].getWind(wind); } // return earth magnetic field estimates in measurement units / 1000 -void NavEKF3::getMagNED(int8_t instance, Vector3f &magNED) const +void NavEKF3::getMagNED(Vector3f &magNED) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getMagNED(magNED); + core[primary].getMagNED(magNED); } } // return body magnetic field estimates in measurement units / 1000 -void NavEKF3::getMagXYZ(int8_t instance, Vector3f &magXYZ) const +void NavEKF3::getMagXYZ(Vector3f &magXYZ) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getMagXYZ(magXYZ); + core[primary].getMagXYZ(magXYZ); } } -// return the airspeed sensor in use for the specified instance -uint8_t NavEKF3::getActiveAirspeed(int8_t instance) const +// return the airspeed sensor in use +uint8_t NavEKF3::getActiveAirspeed() const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - return core[instance].getActiveAirspeed(); + return core[primary].getActiveAirspeed(); } else { return 255; } @@ -1343,17 +1332,15 @@ bool NavEKF3::getLLH(struct Location &loc) const return core[primary].getLLH(loc); } -// Return the latitude and longitude and height used to set the NED origin for the specified instance -// An out of range instance (eg -1) returns data for the primary instance +// Return the latitude and longitude and height used to set the NED origin // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set -bool NavEKF3::getOriginLLH(int8_t instance, struct Location &loc) const +bool NavEKF3::getOriginLLH(struct Location &loc) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getOriginLLH(loc); + return core[primary].getOriginLLH(loc); } // set the latitude and longitude and height used to set the NED origin @@ -1393,12 +1380,11 @@ bool NavEKF3::getHAGL(float &HAGL) const return core[primary].getHAGL(HAGL); } -// return the Euler roll, pitch and yaw angle in radians for the specified instance -void NavEKF3::getEulerAngles(int8_t instance, Vector3f &eulers) const +// return the Euler roll, pitch and yaw angle in radians +void NavEKF3::getEulerAngles(Vector3f &eulers) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getEulerAngles(eulers); + core[primary].getEulerAngles(eulers); } } @@ -1422,45 +1408,39 @@ void NavEKF3::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const } // return the quaternions defining the rotation from NED to XYZ (autopilot) axes -void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const +void NavEKF3::getQuaternion(Quaternion &quat) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getQuaternion(quat); + core[primary].getQuaternion(quat); } } -// return the innovations for the specified instance -bool NavEKF3::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +// return the innovations +bool NavEKF3::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { if (core == nullptr) { return false; } - if (instance < 0 || instance >= num_cores) instance = primary; - return core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + return core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); } // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements -bool NavEKF3::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +bool NavEKF3::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { if (core == nullptr) { return false; } - if (instance < 0 || instance >= num_cores) instance = primary; - return core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + return core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); } -// get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance +// get a source's velocity innovations // returns true on success and results are placed in innovations and variances arguments -bool NavEKF3::getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const +bool NavEKF3::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const { - if (instance < 0 || instance >= num_cores) { - instance = primary; - } if (core) { - return core[instance].getVelInnovationsAndVariancesForSource(source, innovations, variances); + return core[primary].getVelInnovationsAndVariancesForSource(source, innovations, variances); } return false; } @@ -1754,11 +1734,10 @@ void NavEKF3::setTerrainHgtStable(bool val) 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ -void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const +void NavEKF3::getFilterFaults(uint16_t &faults) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getFilterFaults(faults); + core[primary].getFilterFaults(faults); } else { faults = 0; } @@ -1767,11 +1746,10 @@ void NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const /* return filter status flags */ -void NavEKF3::getFilterStatus(int8_t instance, nav_filter_status &status) const +void NavEKF3::getFilterStatus(nav_filter_status &status) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getFilterStatus(status); + core[primary].getFilterStatus(status); } else { memset(&status, 0, sizeof(status)); } @@ -2019,14 +1997,11 @@ bool NavEKF3::yawAlignmentComplete(void) const return core[primary].have_aligned_yaw(); } -// returns true when the state estimates for the selected core are significantly degraded by vibration -bool NavEKF3::isVibrationAffected(int8_t instance) const +// returns true when the state estimates are significantly degraded by vibration +bool NavEKF3::isVibrationAffected() const { - if (instance < 0 || instance >= num_cores) { - instance = primary; - } if (core) { - return core[instance].isVibrationAffected(); + return core[primary].isVibrationAffected(); } return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 68f12a04b88..78ae50a44d2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -68,32 +68,27 @@ class NavEKF3 { // return -1 if no primary core selected int8_t getPrimaryCoreIMUIndex(void) const; - // Write the last calculated NE position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the primary instance + // Write the last calculated NE position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(int8_t instance, Vector2f &posNE) const; + bool getPosNE(Vector2f &posNE) const; - // Write the last calculated D position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the primary instance + // Write the last calculated D position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(int8_t instance, float &posD) const; + bool getPosD(float &posD) const; - // return NED velocity in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getVelNED(int8_t instance, Vector3f &vel) const; + // return NED velocity in m/s + void getVelNED(Vector3f &vel) const; - // return estimate of true airspeed vector in body frame in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance + // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable - bool getAirSpdVec(int8_t instance, Vector3f &vel) const; + bool getAirSpdVec(Vector3f &vel) const; - // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state - float getPosDownDerivative(int8_t instance) const; + float getPosDownDerivative() const; // return body axis gyro bias estimates in rad/sec for the specified instance // An out of range instance (eg -1) returns data for the primary instance @@ -117,22 +112,19 @@ class NavEKF3 { // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; - // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) - // An out of range instance (eg -1) returns data for the the primary instance - // returns true if wind state estimation is active - bool getWind(int8_t instance, Vector3f &wind) const; + // return the NED wind speed estimates in m/s (positive is air + // moving in the direction of the axis) returns true if wind state + // estimation is active + bool getWind(Vector3f &wind) const; - // return earth magnetic field estimates in measurement units / 1000 for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getMagNED(int8_t instance, Vector3f &magNED) const; + // return earth magnetic field estimates in measurement units / 1000 + void getMagNED(Vector3f &magNED) const; - // return body magnetic field estimates in measurement units / 1000 for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getMagXYZ(int8_t instance, Vector3f &magXYZ) const; + // return body magnetic field estimates in measurement units / 1000 + void getMagXYZ(Vector3f &magXYZ) const; - // return the sensor in use for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - uint8_t getActiveAirspeed(int8_t instance) const; + // return the airspeed sensor in use + uint8_t getActiveAirspeed() const; // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid @@ -144,11 +136,10 @@ class NavEKF3 { // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control bool getLLH(struct Location &loc) const; - // Return the latitude and longitude and height used to set the NED origin for the specified instance - // An out of range instance (eg -1) returns data for the primary instance + // Return the latitude and longitude and height used to set the NED origin // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set - bool getOriginLLH(int8_t instance, struct Location &loc) const; + bool getOriginLLH(struct Location &loc) const; // set the latitude and longitude and height used to set the NED origin // All NED positions calculated by the filter will be relative to this location @@ -160,9 +151,8 @@ class NavEKF3 { // return false if ground height is not being estimated. bool getHAGL(float &HAGL) const; - // return the Euler roll, pitch and yaw angle in radians for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getEulerAngles(int8_t instance, Vector3f &eulers) const; + // return the Euler roll, pitch and yaw angle in radians + void getEulerAngles(Vector3f &eulers) const; // return the transformation matrix from XYZ (body) to NED axes void getRotationBodyToNED(Matrix3f &mat) const; @@ -171,19 +161,17 @@ class NavEKF3 { void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const; // return the quaternions defining the rotation from NED to XYZ (autopilot) axes - void getQuaternion(int8_t instance, Quaternion &quat) const; + void getQuaternion(Quaternion &quat) const; - // return the innovations for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - bool getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; + // return the innovations + bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; - // return the innovation consistency test ratios for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - bool getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; + // return the innovation consistency test ratios + bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; - // get a source's velocity innovations for the specified instance. Set instance to -1 for the primary instance + // get a source's velocity innovations // returns true on success and results are placed in innovations and variances arguments - bool getVelInnovationsAndVariancesForSource(int8_t instance, AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; + bool getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED; // should we use the compass? This is public so it can be used for // reporting via ahrs.use_compass() @@ -271,8 +259,7 @@ class NavEKF3 { void setTerrainHgtStable(bool val); /* - return the filter fault status as a bitmasked integer for the specified instance - An out of range instance (eg -1) returns data for the primary instance + return the filter fault status as a bitmasked integer 0 = quaternions are NaN 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion @@ -282,13 +269,12 @@ class NavEKF3 { 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ - void getFilterFaults(int8_t instance, uint16_t &faults) const; + void getFilterFaults(uint16_t &faults) const; /* - return filter status flags for the specified instance - An out of range instance (eg -1) returns data for the primary instance + return filter status flags */ - void getFilterStatus(int8_t instance, nav_filter_status &status) const; + void getFilterStatus(nav_filter_status &status) const; // send an EKF_STATUS_REPORT message to GCS void send_status_report(mavlink_channel_t chan) const; @@ -358,9 +344,9 @@ class NavEKF3 { // returns true when the yaw angle has been aligned bool yawAlignmentComplete(void) const; - // returns true when the state estimates for the selected core are significantly degraded by vibration - // if instance < 0, the primary instance will be used - bool isVibrationAffected(int8_t instance) const; + // returns true when the state estimates are significantly + // degraded by vibration + bool isVibrationAffected() const; // get a yaw estimator instance const EKFGSF_yaw *get_yawEstimator(void) const; From 6ac68c58757f8713500a9ca64e50daedaae72c93 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Apr 2022 15:43:01 +1100 Subject: [PATCH 0538/3101] Blimp: remove land_acel_ef_filter Landing is for wimps --- Blimp/Blimp.cpp | 1 - Blimp/Blimp.h | 1 - 2 files changed, 2 deletions(-) diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 861217e0fde..2fbde58cf0c 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -289,7 +289,6 @@ Blimp::Blimp(void) : logger(g.log_bitmask), flight_modes(&g.flight_mode1), control_mode(Mode::Number::MANUAL), - land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF), rc_throttle_control_in_filter(1.0f), inertial_nav(ahrs), param_loader(var_info), diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 5af0ecf5cfc..0325a5d2bbe 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -209,7 +209,6 @@ class Blimp : public AP_Vehicle // Altitude int32_t baro_alt; // barometer altitude in cm above home - LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests // filtered pilot's throttle input used to cancel landing if throttle held high LowPassFilterFloat rc_throttle_control_in_filter; From a546bc797c518b7e8aaf5ff1d09c6e65d67e234d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Apr 2022 15:55:19 +1000 Subject: [PATCH 0539/3101] AP_Logger: source IDs and structs from HAL_ChibiOS --- libraries/AP_Logger/LogStructure.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index a106cdee073..8c40896a0f1 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -135,6 +135,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format struct PACKED LogStructure { @@ -1356,6 +1357,7 @@ LOG_STRUCTURE_FROM_NAVEKF2 \ LOG_STRUCTURE_FROM_NAVEKF3 \ LOG_STRUCTURE_FROM_NAVEKF \ LOG_STRUCTURE_FROM_AHRS \ +LOG_STRUCTURE_FROM_HAL_CHIBIOS \ { LOG_DF_FILE_STATS, sizeof(log_DSF), \ "DSF", "QIHIIII", "TimeUS,Dp,Blk,Bytes,FMn,FMx,FAv", "s--b---", "F--0---" }, \ { LOG_RPM_MSG, sizeof(log_RPM), \ @@ -1422,6 +1424,7 @@ enum LogMessages : uint8_t { LOG_CSRV_MSG, LOG_IDS_FROM_ESC_TELEM, LOG_IDS_FROM_BATTMONITOR, + LOG_IDS_FROM_HAL_CHIBIOS, LOG_IDS_FROM_GPS, From 00de10d7969d380518f084a655bb0872cb3d746e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Apr 2022 15:54:12 +1000 Subject: [PATCH 0540/3101] AP_HAL_ChibiOS: use structure for writing out WDOG,MON message --- libraries/AP_HAL_ChibiOS/LogStructure.h | 77 +++++++++++++++++++++++++ libraries/AP_HAL_ChibiOS/Scheduler.cpp | 65 +++++++++++---------- 2 files changed, 113 insertions(+), 29 deletions(-) create mode 100644 libraries/AP_HAL_ChibiOS/LogStructure.h diff --git a/libraries/AP_HAL_ChibiOS/LogStructure.h b/libraries/AP_HAL_ChibiOS/LogStructure.h new file mode 100644 index 00000000000..236f9bac781 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/LogStructure.h @@ -0,0 +1,77 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_HAL_CHIBIOS \ + LOG_MON_MSG, \ + LOG_WDOG_MSG + +// @LoggerMessage: MON +// @Description: Main loop performance monitoring message. +// @Field: TimeUS: Time since system startup +// @Field: Dly: Loop delay so far +// @Field: Tsk: Current task +// @Field: IErr: Internal error mask +// @Field: IErrCnt: Count of internal error occurances +// @Field: IErrLn: Internal Error line +// @Field: MM: MAVLink message currently being processed +// @Field: MC: MAVLink command currently being processed +// @Field: SmLn: If semaphore taken, line of semaphore take call +// @Field: SPICnt: count of SPI transactions +// @Field: I2CCnt: count of i2c transactions +struct PACKED log_MON { + LOG_PACKET_HEADER; + uint64_t time_us; + uint32_t loop_delay; + int8_t current_task; + uint32_t internal_error_mask; + uint16_t internal_error_count; + uint16_t internal_error_line; + uint16_t mavmsg; + uint16_t mavcmd; + uint16_t semline; + uint32_t spicnt; + uint32_t i2ccnt; +}; + +// @LoggerMessage: WDOG +// @Description: Watchdog diagnostics +// @Field: TimeUS: Time since system startup +// @Field: Tsk: current task number +// @Field: IE: internal error mast +// @Field: IEC: internal error count +// @Field: IEL: line internal error was raised on +// @Field: MvMsg: mavlink message being acted on +// @Field: MvCmd: mavlink command being acted on +// @Field: SmLn: line semaphore was taken on +// @Field: FL: fault_line +// @Field: FT: fault_type +// @Field: FA: fault address +// @Field: FP: fault thread priority +// @Field: ICSR: ICS regiuster +// @Field: LR: long return address +// @Field: TN: Thread name +struct PACKED log_WDOG { + LOG_PACKET_HEADER; + uint64_t time_us; + int8_t scheduler_task; + uint32_t internal_errors; + uint16_t internal_error_count; + uint16_t internal_error_last_line; + uint16_t last_mavlink_msgid; + uint16_t last_mavlink_cmd; + uint16_t semaphore_line; + uint16_t fault_line; + uint16_t fault_type; + uint32_t fault_addr; + uint8_t fault_thd_prio; + uint32_t fault_icsr; + uint32_t fault_lr; + char thread_name4[4]; +}; + +#define LOG_STRUCTURE_FROM_HAL_CHIBIOS \ + { LOG_MON_MSG, sizeof(log_MON), \ + "MON","QIbIHHHHHII","TimeUS,Dly,Tsk,IErr,IErrCnt,IErrLn,MM,MC,SmLn,SPICnt,I2CCnt", "s----------", "F----------", false }, \ + { LOG_WDOG_MSG, sizeof(log_WDOG), \ + "WDOG","QbIHHHHHHHIBIIn","TimeUS,Tsk,IE,IEC,IEL,MvMsg,MvCmd,SmLn,FL,FT,FA,FP,ICSR,LR,TN", "s--------------", "F--------------", false }, diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index c8a566c6091..66c8bb762b1 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -410,19 +410,22 @@ void Scheduler::_monitor_thread(void *arg) #if HAL_LOGGING_ENABLED const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; if (AP_Logger::get_singleton()) { - AP::logger().Write("MON", "TimeUS,LDelay,Task,IErr,IErrCnt,IErrLn,MavMsg,MavCmd,SemLine,SPICnt,I2CCnt", "QIbIHHHHHII", - AP_HAL::micros64(), - loop_delay, - pd.scheduler_task, - pd.internal_errors, - pd.internal_error_count, - pd.internal_error_last_line, - pd.last_mavlink_msgid, - pd.last_mavlink_cmd, - pd.semaphore_line, - pd.spi_count, - pd.i2c_count); - } + const struct log_MON mon{ + LOG_PACKET_HEADER_INIT(LOG_MON_MSG), + time_us : AP_HAL::micros64(), + loop_delay : loop_delay, + current_task : pd.scheduler_task, + internal_error_mask : pd.internal_errors, + internal_error_count : pd.internal_error_count, + internal_error_line : pd.internal_error_last_line, + mavmsg : pd.last_mavlink_msgid, + mavcmd : pd.last_mavlink_cmd, + semline : pd.semaphore_line, + spicnt : pd.spi_count, + i2ccnt : pd.i2c_count + }; + AP::logger().WriteCriticalBlock(&mon, sizeof(mon)); + } #endif } if (loop_delay >= 500 && !sched->in_expected_delay()) { @@ -435,22 +438,26 @@ void Scheduler::_monitor_thread(void *arg) log_wd_counter = 0; // log watchdog message once a second const AP_HAL::Util::PersistentData &pd = hal.util->last_persistent_data; - AP::logger().WriteCritical("WDOG", "TimeUS,Tsk,IE,IEC,IEL,MvMsg,MvCmd,SmLn,FL,FT,FA,FP,ICSR,LR,TN", "QbIHHHHHHHIBIIn", - AP_HAL::micros64(), - pd.scheduler_task, - pd.internal_errors, - pd.internal_error_count, - pd.internal_error_last_line, - pd.last_mavlink_msgid, - pd.last_mavlink_cmd, - pd.semaphore_line, - pd.fault_line, - pd.fault_type, - pd.fault_addr, - pd.fault_thd_prio, - pd.fault_icsr, - pd.fault_lr, - pd.thread_name4); + struct log_WDOG wdog{ + LOG_PACKET_HEADER_INIT(LOG_WDOG_MSG), + time_us : AP_HAL::micros64(), + scheduler_task : pd.scheduler_task, + internal_errors : pd.internal_errors, + internal_error_count : pd.internal_error_count, + internal_error_last_line : pd.internal_error_last_line, + last_mavlink_msgid : pd.last_mavlink_msgid, + last_mavlink_cmd : pd.last_mavlink_cmd, + semaphore_line : pd.semaphore_line, + fault_line : pd.fault_line, + fault_type : pd.fault_type, + fault_addr : pd.fault_addr, + fault_thd_prio : pd.fault_thd_prio, + fault_icsr : pd.fault_icsr, + fault_lr : pd.fault_lr + }; + memcpy(wdog.thread_name4, pd.thread_name4, ARRAY_SIZE(wdog.thread_name4)); + + AP::logger().WriteCriticalBlock(&wdog, sizeof(wdog)); } #endif // HAL_LOGGING_ENABLED From 9f7ae1ee96d2a8d642cec7f6ceef9cc93062ff1e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 19:52:51 +1000 Subject: [PATCH 0541/3101] autotest: extract ids and message infos per-file, not on aggregate Will allow for better diagnostics when something goes wrong --- Tools/autotest/common.py | 125 ++++++++++++++++++++------------------- 1 file changed, 64 insertions(+), 61 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 517be7703f3..33ddde47b49 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2121,74 +2121,77 @@ def all_log_format_ids(self): structure_lines = [] for f in structure_files: structure_lines.extend(open(f).readlines()) - ids = {} - state_outside = 0 - state_inside = 1 - state = state_outside defines = self.find_format_defines(structure_lines) - linestate_none = 45 - linestate_within = 46 - linestate = linestate_none + ids = {} message_infos = [] - for line in structure_lines: - # print("line: %s" % line) - if type(line) == bytes: - line = line.decode("utf-8") - line = re.sub("//.*", "", line) # trim comments - if re.match(r"\s*$", line): - # blank line - continue - if state == state_outside: - if ("#define LOG_COMMON_STRUCTURES" in line or - re.match("#define LOG_STRUCTURE_FROM_.*", line)): - # self.progress("Moving inside") - state = state_inside - continue - if state == state_inside: - if linestate == linestate_none: - allowed_list = ['LOG_STRUCTURE_FROM_'] - - allowed = False - for a in allowed_list: - if a in line: - allowed = True - if allowed: - continue - m = re.match(r"\s*{(.*)},\s*", line) - if m is not None: - # complete line - # print("Complete line: %s" % str(line)) - message_infos.append(m.group(1)) - continue - m = re.match(r"\s*{(.*)\\", line) - if m is None: - state = state_outside - continue - partial_line = m.group(1) - linestate = linestate_within + for f in structure_files: + self.progress("structure file: %s" % f) + state_outside = 0 + state_inside = 1 + state = state_outside + + linestate_none = 45 + linestate_within = 46 + linestate = linestate_none + for line in open(f).readlines(): + # print("line: %s" % line) + if type(line) == bytes: + line = line.decode("utf-8") + line = re.sub("//.*", "", line) # trim comments + if re.match(r"\s*$", line): + # blank line continue - if linestate == linestate_within: - m = re.match("(.*)}", line) - if m is None: - line = line.rstrip() - newline = re.sub(r"\\$", "", line) - if newline == line: - raise NotAchievedException("Expected backslash at end of line") - line = newline - line = line.rstrip() - # cpp-style string concatenation: - line = re.sub(r'"\s*"', '', line) - partial_line += line - continue - message_infos.append(partial_line + m.group(1)) - linestate = linestate_none + if state == state_outside: + if ("#define LOG_COMMON_STRUCTURES" in line or + re.match("#define LOG_STRUCTURE_FROM_.*", line)): + # self.progress("Moving inside") + state = state_inside continue - raise NotAchievedException("Bad line (%s)") + if state == state_inside: + if linestate == linestate_none: + allowed_list = ['LOG_STRUCTURE_FROM_'] + + allowed = False + for a in allowed_list: + if a in line: + allowed = True + if allowed: + continue + m = re.match(r"\s*{(.*)},\s*", line) + if m is not None: + # complete line + # print("Complete line: %s" % str(line)) + message_infos.append(m.group(1)) + continue + m = re.match(r"\s*{(.*)\\", line) + if m is None: + state = state_outside + continue + partial_line = m.group(1) + linestate = linestate_within + continue + if linestate == linestate_within: + m = re.match("(.*)}", line) + if m is None: + line = line.rstrip() + newline = re.sub(r"\\$", "", line) + if newline == line: + raise NotAchievedException("Expected backslash at end of line") + line = newline + line = line.rstrip() + # cpp-style string concatenation: + line = re.sub(r'"\s*"', '', line) + partial_line += line + continue + message_infos.append(partial_line + m.group(1)) + linestate = linestate_none + continue + raise NotAchievedException("Bad line (%s)") - if linestate != linestate_none: - raise NotAchievedException("Must be linestate-none at end of file") + if linestate != linestate_none: + raise NotAchievedException("Must be linestate-none at end of file") # now look in the vehicle-specific logfile: filepath = os.path.join(self.vehicle_code_dirpath(), "Log.cpp") From 55d0cf4b03dc4f5e71aa0ab3edabf6c54cbf43c4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 20:09:32 +1000 Subject: [PATCH 0542/3101] autotest: add diagnostics for log parse issues --- Tools/autotest/common.py | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 33ddde47b49..88decedc8f2 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2135,8 +2135,12 @@ def all_log_format_ids(self): linestate_none = 45 linestate_within = 46 linestate = linestate_none + debug = False + if f == "/home/pbarker/rc/ardupilot/libraries/AP_HAL_ChibiOS/LogStructure.h": + debug = True for line in open(f).readlines(): - # print("line: %s" % line) + if debug: + print("line: %s" % line) if type(line) == bytes: line = line.decode("utf-8") line = re.sub("//.*", "", line) # trim comments @@ -2146,7 +2150,8 @@ def all_log_format_ids(self): if state == state_outside: if ("#define LOG_COMMON_STRUCTURES" in line or re.match("#define LOG_STRUCTURE_FROM_.*", line)): - # self.progress("Moving inside") + if debug: + self.progress("Moving inside") state = state_inside continue if state == state_inside: @@ -2162,19 +2167,28 @@ def all_log_format_ids(self): m = re.match(r"\s*{(.*)},\s*", line) if m is not None: # complete line - # print("Complete line: %s" % str(line)) + if debug: + print("Complete line: %s" % str(line)) message_infos.append(m.group(1)) continue m = re.match(r"\s*{(.*)\\", line) if m is None: + if debug: + self.progress("Moving outside") state = state_outside continue partial_line = m.group(1) + if debug: + self.progress("partial line") linestate = linestate_within continue if linestate == linestate_within: + if debug: + self.progress("Looking for close-brace") m = re.match("(.*)}", line) if m is None: + if debug: + self.progress("no close-brace") line = line.rstrip() newline = re.sub(r"\\$", "", line) if newline == line: @@ -2182,9 +2196,13 @@ def all_log_format_ids(self): line = newline line = line.rstrip() # cpp-style string concatenation: + if debug: + self.progress("more partial line") line = re.sub(r'"\s*"', '', line) partial_line += line continue + if debug: + self.progress("found close-brace") message_infos.append(partial_line + m.group(1)) linestate = linestate_none continue @@ -2262,10 +2280,12 @@ def all_log_format_ids(self): raise NotAchievedException("Should not be in state_inside at end") for message_info in message_infos: + print("message_info: %s" % str(message_info)) for define in defines: message_info = re.sub(define, defines[define], message_info) m = re.match(r'\s*LOG_\w+\s*,\s*sizeof\([^)]+\)\s*,\s*"(\w+)"\s*,\s*"(\w+)"\s*,\s*"([\w,]+)"\s*,\s*"([^"]+)"\s*,\s*"([^"]+)"\s*(,\s*true)?\s*$', message_info) # noqa if m is None: + print("NO MATCH") continue (name, fmt, labels, units, multipliers) = (m.group(1), m.group(2), m.group(3), m.group(4), m.group(5)) if name in ids: From 7c526d52fedb707c165c21b0243a1b6c88431ef1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 20:10:15 +1000 Subject: [PATCH 0543/3101] autotest: correct streaming-boolean bug for parsing of log structures from code --- Tools/autotest/common.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 88decedc8f2..ce70a9474f1 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -2283,7 +2283,7 @@ def all_log_format_ids(self): print("message_info: %s" % str(message_info)) for define in defines: message_info = re.sub(define, defines[define], message_info) - m = re.match(r'\s*LOG_\w+\s*,\s*sizeof\([^)]+\)\s*,\s*"(\w+)"\s*,\s*"(\w+)"\s*,\s*"([\w,]+)"\s*,\s*"([^"]+)"\s*,\s*"([^"]+)"\s*(,\s*true)?\s*$', message_info) # noqa + m = re.match(r'\s*LOG_\w+\s*,\s*sizeof\([^)]+\)\s*,\s*"(\w+)"\s*,\s*"(\w+)"\s*,\s*"([\w,]+)"\s*,\s*"([^"]+)"\s*,\s*"([^"]+)"\s*(,\s*(true|false))?\s*$', message_info) # noqa if m is None: print("NO MATCH") continue From b80cc9a6106293d68fb1c22b7943168ca961d824 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 5 Apr 2022 20:14:48 +1000 Subject: [PATCH 0544/3101] AP_Logger: remove old, unused log metadata for MON --- libraries/AP_Logger/LogStructure.h | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 8c40896a0f1..68ba9e40872 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -942,20 +942,6 @@ struct PACKED log_VER { // @Field: ModeNum: alias for Mode // @Field: Rsn: reason for entering this mode; enumeration value -// @LoggerMessage: MON -// @Description: Main loop stuck data -// @Field: TimeUS: Time since system startup -// @Field: LDelay: Time main loop has been stuck for -// @Field: Task: Current scheduler task number -// @Field: IErr: Internal error mask; which internal errors have been detected -// @Field: IErrCnt: Internal error count; how many internal errors have been detected -// @Field: IErrLn: Line on which internal error ocurred -// @Field: MavMsg: Id of the last mavlink message processed -// @Field: MavCmd: Id of the last mavlink command processed -// @Field: SemLine: Line number of semaphore most recently taken -// @Field: SPICnt: Number of SPI transactions processed -// @Field: I2CCnt: Number of i2c transactions processed - // @LoggerMessage: MSG // @Description: Textual messages // @Field: TimeUS: Time since system startup From e0adbb978ed13c664bff7cd151cbed77613a14ff Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 6 Apr 2022 18:44:34 +0100 Subject: [PATCH 0545/3101] AP_HAL_ChibiOS: fix dshot timeout bug where the elapsed pulse is longer than the send time dshot timeouts should be no longer than the pulse interval use correct timestamp for dmar send time --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index c7dbd225869..5352a3db6e1 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -255,17 +255,18 @@ void RCOutput::dshot_collect_dma_locks(uint32_t time_out_us) const uint32_t pulse_elapsed_us = now - group.last_dmar_send_us; uint32_t wait_us = 0; if (now < time_out_us) { - wait_us = MAX(time_out_us - now, group.dshot_pulse_send_time_us - pulse_elapsed_us); - } else if (pulse_elapsed_us < group.dshot_pulse_send_time_us) { + wait_us = time_out_us - now; + } + if (pulse_elapsed_us < group.dshot_pulse_send_time_us) { // better to let the burst write in progress complete rather than cancelling mid way through - wait_us = group.dshot_pulse_send_time_us - pulse_elapsed_us; + wait_us = MAX(wait_us, group.dshot_pulse_send_time_us - pulse_elapsed_us); } // waiting for a very short period of time can cause a // timer wrap with ChibiOS timers. Use CH_CFG_ST_TIMEDELTA - // as minimum. Don't allow for a very long delay (over 1200us) + // as minimum. Don't allow for a very long delay (over _dshot_period_us) // to prevent bugs in handling timer wrap - const uint32_t max_delay_us = 1200; + const uint32_t max_delay_us = _dshot_period_us; const uint32_t min_delay_us = 10; // matches our CH_CFG_ST_TIMEDELTA wait_us = constrain_uint32(wait_us, min_delay_us, max_delay_us); mask = chEvtWaitOneTimeout(group.dshot_event_mask, chTimeUS2I(wait_us)); @@ -1566,7 +1567,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) dmaStreamEnable(group.dma); // record when the transaction was started - group.last_dmar_send_us = AP_HAL::micros64(); + group.last_dmar_send_us = AP_HAL::micros(); #endif //#ifndef DISABLE_DSHOT } From 3bdcff263d2591d8763e9183ede643b4d827fb98 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 Apr 2022 21:16:07 +1000 Subject: [PATCH 0546/3101] Plane: release notes for 4.2.0beta5 --- ArduPlane/ReleaseNotes.txt | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index efb065ce2ec..404bb023e02 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,21 @@ +Release 4.2.0beta5 5th April 2022 +--------------------------------- + +This is the 5th beta of the 4.2.0 major release + + - fixed a timer bug that could cause boards using flash storage to watchdog + - added OTG2 USB on QioTekZealotH743 + - increased stack size on log and monitor threads + - fixed rudder control when ARMING_RUDDER != 2 on quadplanes + - fixed accel bias when an IMU is disabled in EKF3 + - improved QSPI support for SPro H7 Extreme + - fixed @SYS file logging + - fixed buzzer on MatekH743, going back to 16 bit timer + - fixed H7 flash storage bug that caused re-init on overflow + - fixed incorrect lock class in UART driver + +Happy flying! + Release 4.2.0beta4 28th March 2022 ---------------------------------- From 08e22095a966ce7f85d7cf4bd950c3e2b62a35e4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Apr 2022 15:09:40 +1000 Subject: [PATCH 0547/3101] Tools: added script to make using a crash_dump.bin easier --- Tools/debug/gdb_crashdump.sh | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100755 Tools/debug/gdb_crashdump.sh diff --git a/Tools/debug/gdb_crashdump.sh b/Tools/debug/gdb_crashdump.sh new file mode 100755 index 00000000000..89f6b4d66c1 --- /dev/null +++ b/Tools/debug/gdb_crashdump.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# script to more easily get a backtrace from an ArduPilot crash_dump.bin + +[ $# -eq 2 ] || { + echo "Usage: gdb_crashdump.sh ELF_FILE CRASH_DUMP" + exit 1 +} + +ELF_FILE=$1 +CRASH_DUMP=$2 + +arm-none-eabi-gdb -nx "$ELF_FILE" -ex "set target-charset ASCII" -ex "target remote | modules/CrashDebug/bins/lin64/CrashDebug --elf $ELF_FILE --dump $CRASH_DUMP" From 2b2cac3f19e041e20ef3548eaf852371ee2f91c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Apr 2022 07:57:45 +1000 Subject: [PATCH 0548/3101] autotest: fixed annoying gdb pagination on reboot --- Tools/autotest/sim_vehicle.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 2c2e227d76b..4e4d677c089 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -640,6 +640,7 @@ def start_vehicle(binary, opts, stuff, spawns=None): gdb_commands_file.write("b %s\n" % (breakpoint,)) if opts.disable_breakpoints: gdb_commands_file.write("disable\n") + gdb_commands_file.write("set pagination off\n") if not opts.gdb_stopped: gdb_commands_file.write("r\n") gdb_commands_file.close() From c1e776fc468af12577bba92d52d17952234e8e86 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:45 +1100 Subject: [PATCH 0549/3101] AC_AttitudeControl: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AC_AttitudeControl/ControlMonitor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/ControlMonitor.cpp b/libraries/AC_AttitudeControl/ControlMonitor.cpp index 37fadc2600c..5efdb1606c5 100644 --- a/libraries/AC_AttitudeControl/ControlMonitor.cpp +++ b/libraries/AC_AttitudeControl/ControlMonitor.cpp @@ -1,6 +1,7 @@ #include "AC_AttitudeControl.h" #include #include +#include /* code to monitor and report on the rate controllers, allowing for @@ -24,15 +25,15 @@ void AC_AttitudeControl::control_monitor_filter_pid(float value, float &rms) */ void AC_AttitudeControl::control_monitor_update(void) { - const AP_Logger::PID_Info &iroll = get_rate_roll_pid().get_pid_info(); + const AP_PIDInfo &iroll = get_rate_roll_pid().get_pid_info(); control_monitor_filter_pid(iroll.P + iroll.FF, _control_monitor.rms_roll_P); control_monitor_filter_pid(iroll.D, _control_monitor.rms_roll_D); - const AP_Logger::PID_Info &ipitch = get_rate_pitch_pid().get_pid_info(); + const AP_PIDInfo &ipitch = get_rate_pitch_pid().get_pid_info(); control_monitor_filter_pid(ipitch.P + ipitch.FF, _control_monitor.rms_pitch_P); control_monitor_filter_pid(ipitch.D, _control_monitor.rms_pitch_D); - const AP_Logger::PID_Info &iyaw = get_rate_yaw_pid().get_pid_info(); + const AP_PIDInfo &iyaw = get_rate_yaw_pid().get_pid_info(); control_monitor_filter_pid(iyaw.P + iyaw.D + iyaw.FF, _control_monitor.rms_yaw); } From 66f0a0f42ba9b515704d170742d80963a21f7e54 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:45 +1100 Subject: [PATCH 0550/3101] AC_AutoTune: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AC_AutoTune/AC_AutoTune.cpp | 4 +++- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 2 ++ libraries/AC_AutoTune/AC_AutoTune_Multi.cpp | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index eb1e13a7fb0..8257be55430 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -1,6 +1,8 @@ #include "AC_AutoTune.h" -#include + +#include #include +#include #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 5267e662694..5cb31d2adb4 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -19,6 +19,8 @@ #include "AC_AutoTune_Heli.h" +#include + #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step #define AUTOTUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index dce6ca01d51..8b9f2982f45 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1,5 +1,7 @@ #include "AC_AutoTune_Multi.h" +#include + /* * autotune support for multicopters * From e5e4dee708afb174a444126bfe52b8000bf3ea1b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:46 +1100 Subject: [PATCH 0551/3101] AC_PID: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AC_PID/AC_PID.h | 7 ++++--- libraries/AC_PID/AC_PID_2D.h | 10 +++++----- libraries/AC_PID/AC_PID_Basic.h | 6 +++--- libraries/AC_PID/AP_PIDInfo.h | 20 ++++++++++++++++++++ 4 files changed, 32 insertions(+), 11 deletions(-) create mode 100644 libraries/AC_PID/AP_PIDInfo.h diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index c3a968dda90..cd291a02fca 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -7,7 +7,6 @@ #include #include #include -#include #include #define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency @@ -15,6 +14,8 @@ #define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency #define AC_PID_RESET_TC 0.16f // Time constant for integrator reset decay to zero +#include "AP_PIDInfo.h" + /// @class AC_PID /// @brief Copter PID control class class AC_PID { @@ -116,7 +117,7 @@ class AC_PID { // return current slew rate of slew limiter. Will return 0 if SMAX is zero float get_slew_rate(void) const { return _slew_limiter.get_slew_rate(); } - const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } + const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -155,5 +156,5 @@ class AC_PID { float _derivative; // derivative value to enable filtering int8_t _slew_limit_scale; - AP_Logger::PID_Info _pid_info; + AP_PIDInfo _pid_info; }; diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index 7f17d534fe9..6e183400480 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include /// @class AC_PID_2D /// @brief Copter PID control class @@ -75,8 +75,8 @@ class AC_PID_2D { void set_integrator(const Vector3f& i) { set_integrator(Vector2f{i.x, i.y}); } void set_integrator(const Vector2f& i); - const AP_Logger::PID_Info& get_pid_info_x(void) const { return _pid_info_x; } - const AP_Logger::PID_Info& get_pid_info_y(void) const { return _pid_info_y; } + const AP_PIDInfo& get_pid_info_x(void) const { return _pid_info_x; } + const AP_PIDInfo& get_pid_info_y(void) const { return _pid_info_y; } // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -100,6 +100,6 @@ class AC_PID_2D { Vector2f _integrator; // integrator value bool _reset_filter; // true when input filter should be reset during next call to update_all - AP_Logger::PID_Info _pid_info_x; - AP_Logger::PID_Info _pid_info_y; + AP_PIDInfo _pid_info_x; + AP_PIDInfo _pid_info_y; }; diff --git a/libraries/AC_PID/AC_PID_Basic.h b/libraries/AC_PID/AC_PID_Basic.h index 65143a26a23..8ab1f6f1005 100644 --- a/libraries/AC_PID/AC_PID_Basic.h +++ b/libraries/AC_PID/AC_PID_Basic.h @@ -5,7 +5,7 @@ #include #include -#include +#include "AP_PIDInfo.h" /// @class AC_PID_Basic /// @brief Copter PID control class @@ -70,7 +70,7 @@ class AC_PID_Basic { void set_integrator(float error, float i); void set_integrator(float i); - const AP_Logger::PID_Info& get_pid_info(void) const WARN_IF_UNUSED { return _pid_info; } + const AP_PIDInfo& get_pid_info(void) const WARN_IF_UNUSED { return _pid_info; } // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -94,5 +94,5 @@ class AC_PID_Basic { float _integrator; // integrator value bool _reset_filter; // true when input filter should be reset during next call to set_input - AP_Logger::PID_Info _pid_info; + AP_PIDInfo _pid_info; }; diff --git a/libraries/AC_PID/AP_PIDInfo.h b/libraries/AC_PID/AP_PIDInfo.h new file mode 100644 index 00000000000..98a05f879d0 --- /dev/null +++ b/libraries/AC_PID/AP_PIDInfo.h @@ -0,0 +1,20 @@ +#pragma once + +// This structure provides information on the internal member data of +// a PID. It provides an abstract way to pass PID information around, +// useful for logging and sending mavlink messages. + +// It is also used to pass PID information into controllers... + +struct AP_PIDInfo { + float target; + float actual; + float error; + float P; + float I; + float D; + float FF; + float Dmod; + float slew_rate; + bool limit; +}; From d73cd7d0e3b9f757b47eaf3720dd8113956fdf03 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:46 +1100 Subject: [PATCH 0552/3101] AC_WPNav: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AC_WPNav/AC_Circle.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 1b6fd33d635..3da13dcef1c 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -3,6 +3,8 @@ #include #include "AC_Circle.h" +#include + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AC_Circle::var_info[] = { From 6a97056736c63546099d09ab38ec8f75ede22081 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:46 +1100 Subject: [PATCH 0553/3101] AP_Camera: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Camera/AP_Camera.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 1acb74c2fe6..75cb44093d7 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -2,9 +2,10 @@ /// @brief Photo or video camera manager, with EEPROM-backed storage of constants. #pragma once +#include +#include #include #include -#include #define AP_CAMERA_TRIGGER_DEFAULT_DURATION 10 // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second) From 8ec0eed74908daa7e3c6e32b0ff3e2e6d006b907 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:46 +1100 Subject: [PATCH 0554/3101] AP_Landing: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Landing/AP_Landing.cpp | 2 +- libraries/AP_Landing/AP_Landing.h | 2 +- libraries/AP_Landing/AP_Landing_Deepstall.cpp | 5 +++-- libraries/AP_Landing/AP_Landing_Deepstall.h | 2 +- libraries/AP_Landing/AP_Landing_Slope.cpp | 1 + 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 588f92977ee..2ceb1d2b37a 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -408,7 +408,7 @@ bool AP_Landing::override_servos(void) { // returns a PID_Info object if there is one available for the selected landing // type, otherwise returns a nullptr, indicating no data to be logged/sent -const AP_Logger::PID_Info* AP_Landing::get_pid_info(void) const +const AP_PIDInfo* AP_Landing::get_pid_info(void) const { switch (type) { #if HAL_LANDING_DEEPSTALL_ENABLED diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 1715bb3b50d..6b7d1621e69 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -109,7 +109,7 @@ class AP_Landing { void set_initial_slope(void) { initial_slope = slope; } bool is_expecting_impact(void) const; void Log(void) const; - const AP_Logger::PID_Info * get_pid_info(void) const; + const AP_PIDInfo * get_pid_info(void) const; // landing altitude offset (meters) float alt_offset; diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 8d771741228..fe49370897c 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -26,6 +26,7 @@ #include #include #include +#include // table of user settable parameters for deepstall const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = { @@ -448,13 +449,13 @@ bool AP_Landing_Deepstall::send_deepstall_message(mavlink_channel_t chan) const return true; } -const AP_Logger::PID_Info& AP_Landing_Deepstall::get_pid_info(void) const +const AP_PIDInfo& AP_Landing_Deepstall::get_pid_info(void) const { return ds_PID.get_pid_info(); } void AP_Landing_Deepstall::Log(void) const { - const AP_Logger::PID_Info& pid_info = ds_PID.get_pid_info(); + const AP_PIDInfo& pid_info = ds_PID.get_pid_info(); struct log_DSTL pkt = { LOG_PACKET_HEADER_INIT(LOG_DSTL_MSG), time_us : AP_HAL::micros64(), diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.h b/libraries/AP_Landing/AP_Landing_Deepstall.h index b8b90686b8b..b668054855d 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.h +++ b/libraries/AP_Landing/AP_Landing_Deepstall.h @@ -100,7 +100,7 @@ class AP_Landing_Deepstall bool send_deepstall_message(mavlink_channel_t chan) const; - const AP_Logger::PID_Info& get_pid_info(void) const; + const AP_PIDInfo& get_pid_info(void) const; //private helpers void build_approach_path(bool use_current_heading); diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 4f624af2e93..657dedb19db 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -23,6 +23,7 @@ #include #include #include +#include void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) { From 0c32eeca2e6763a09d4aa473f2bc7a8fb33f4a47 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:47 +1100 Subject: [PATCH 0555/3101] AP_Logger: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Logger/AP_Logger.h | 17 ++--------------- libraries/AP_Logger/LogFile.cpp | 2 +- 2 files changed, 3 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 4a509347a73..1f826233d09 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -48,6 +48,7 @@ #define HAL_LOGGER_FILE_CONTENTS_ENABLED HAL_LOGGING_FILESYSTEM_ENABLED #endif +#include #include #include #include @@ -326,21 +327,7 @@ class AP_Logger void WriteCritical(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...); void WriteV(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, va_list arg_list, bool is_critical=false, bool is_streaming=false); - // This structure provides information on the internal member data of a PID for logging purposes - struct PID_Info { - float target; - float actual; - float error; - float P; - float I; - float D; - float FF; - float Dmod; - float slew_rate; - bool limit; - }; - - void Write_PID(uint8_t msg_type, const PID_Info &info); + void Write_PID(uint8_t msg_type, const AP_PIDInfo &info); // returns true if logging of a message should be attempted bool should_log(uint32_t mask) const; diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 7c0c5d78cb7..c52410ca477 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -407,7 +407,7 @@ void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, // Write a Yaw PID packet -void AP_Logger::Write_PID(uint8_t msg_type, const PID_Info &info) +void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) { const struct log_PID pkt{ LOG_PACKET_HEADER_INIT(msg_type), From dd589934cc09010788609d14833ed059ae38e068 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:47 +1100 Subject: [PATCH 0556/3101] APM_Control: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/APM_Control/AP_AutoTune.cpp | 2 +- libraries/APM_Control/AP_AutoTune.h | 3 ++- libraries/APM_Control/AP_PitchController.h | 5 ++--- libraries/APM_Control/AP_RollController.h | 5 ++--- libraries/APM_Control/AP_SteerController.h | 6 +++--- libraries/APM_Control/AP_YawController.h | 5 ++--- libraries/APM_Control/AR_AttitudeControl.h | 4 ++-- 7 files changed, 14 insertions(+), 16 deletions(-) diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index b283d47329f..dffe4f0cf6e 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -163,7 +163,7 @@ const char *AP_AutoTune::axis_string(void) const /* one update cycle of the autotuner */ -void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_err_deg) +void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) { if (!running) { return; diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 3030fed6a9b..3e33272b268 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -5,6 +5,7 @@ #include #include #include +#include class AP_AutoTune { @@ -54,7 +55,7 @@ class AP_AutoTune // update called whenever autotune mode is active. This is // called at the main loop rate - void update(AP_Logger::PID_Info &pid_info, float scaler, float angle_err_deg); + void update(AP_PIDInfo &pid_info, float scaler, float angle_err_deg); // are we running? bool running; diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 94553bbe66b..359ca126641 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -3,7 +3,6 @@ #include #include #include "AP_AutoTune.h" -#include #include #include @@ -34,7 +33,7 @@ class AP_PitchController void autotune_start(void); void autotune_restore(void); - const AP_Logger::PID_Info& get_pid_info(void) const + const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } @@ -59,7 +58,7 @@ class AP_PitchController AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1}; float angle_err_deg; - AP_Logger::PID_Info _pid_info; + AP_PIDInfo _pid_info; float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed, bool ground_mode); float _get_coordination_rate_offset(float &aspeed, bool &inverted) const; diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index fda999d861f..81cd84665c5 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -3,7 +3,6 @@ #include #include #include "AP_AutoTune.h" -#include #include #include @@ -34,7 +33,7 @@ class AP_RollController void autotune_start(void); void autotune_restore(void); - const AP_Logger::PID_Info& get_pid_info(void) const + const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } @@ -64,7 +63,7 @@ class AP_RollController AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 0.02, 150, 1}; float angle_err_deg; - AP_Logger::PID_Info _pid_info; + AP_PIDInfo _pid_info; float _get_rate_out(float desired_rate, float scaler, bool disable_integrator, bool ground_mode); }; diff --git a/libraries/APM_Control/AP_SteerController.h b/libraries/APM_Control/AP_SteerController.h index 6ecb9380242..301919f3a5c 100644 --- a/libraries/APM_Control/AP_SteerController.h +++ b/libraries/APM_Control/AP_SteerController.h @@ -2,7 +2,7 @@ #include #include -#include +#include class AP_SteerController { public: @@ -44,7 +44,7 @@ class AP_SteerController { static const struct AP_Param::GroupInfo var_info[]; - const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } + const class AP_PIDInfo& get_pid_info(void) const { return _pid_info; } void set_reverse(bool reverse) { _reverse = reverse; @@ -65,7 +65,7 @@ class AP_SteerController { AP_Float _deratefactor; AP_Float _mindegree; - AP_Logger::PID_Info _pid_info {}; + AP_PIDInfo _pid_info {}; bool _reverse; }; diff --git a/libraries/APM_Control/AP_YawController.h b/libraries/APM_Control/AP_YawController.h index c0039ec707d..d8b2eb7d659 100644 --- a/libraries/APM_Control/AP_YawController.h +++ b/libraries/APM_Control/AP_YawController.h @@ -2,7 +2,6 @@ #include #include -#include #include #include "AP_AutoTune.h" @@ -36,7 +35,7 @@ class AP_YawController _pid_info.I *= 0.995f; } - const AP_Logger::PID_Info& get_pid_info(void) const + const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } @@ -69,5 +68,5 @@ class AP_YawController AP_AutoTune *autotune; bool failed_autotune_alloc; - AP_Logger::PID_Info _pid_info; + AP_PIDInfo _pid_info; }; diff --git a/libraries/APM_Control/AR_AttitudeControl.h b/libraries/APM_Control/AR_AttitudeControl.h index d466f1c4e02..d5dd9923e05 100644 --- a/libraries/APM_Control/AR_AttitudeControl.h +++ b/libraries/APM_Control/AR_AttitudeControl.h @@ -82,7 +82,7 @@ class AR_AttitudeControl { AC_PID& get_steering_rate_pid() { return _steer_rate_pid; } AC_PID& get_pitch_to_throttle_pid() { return _pitch_to_throttle_pid; } AC_PID& get_sailboat_heel_pid() { return _sailboat_heel_pid; } - const AP_Logger::PID_Info& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; } + const AP_PIDInfo& get_throttle_speed_pid_info() const { return _throttle_speed_pid_info; } // get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success bool get_forward_speed(float &speed) const; @@ -143,7 +143,7 @@ class AR_AttitudeControl { uint32_t _stop_last_ms; // system time the vehicle was at a complete stop bool _throttle_limit_low; // throttle output was limited from going too low (used to reduce i-term buildup) bool _throttle_limit_high; // throttle output was limited from going too high (used to reduce i-term buildup) - AP_Logger::PID_Info _throttle_speed_pid_info; // local copy of throttle_speed controller's PID info to allow reporting of unusual FF + AP_PIDInfo _throttle_speed_pid_info; // local copy of throttle_speed controller's PID info to allow reporting of unusual FF // balancebot pitch control uint32_t _balance_last_ms = 0; From df6056576948afc55d6bea95ece381e2627e003b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:47 +1100 Subject: [PATCH 0557/3101] AP_Mission: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Mission/AP_Mission_Commands.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 2bc7bc0cb01..0f5867a87e1 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -7,6 +7,7 @@ #include #include #include +#include bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) { From ce312a375001d3823343ed26b173a7035fb90b17 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:47 +1100 Subject: [PATCH 0558/3101] AP_Scripting: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Scripting/lua_repl.cpp | 2 ++ libraries/AP_Scripting/lua_scripts.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/libraries/AP_Scripting/lua_repl.cpp b/libraries/AP_Scripting/lua_repl.cpp index b5d775431dd..373276ca102 100644 --- a/libraries/AP_Scripting/lua_repl.cpp +++ b/libraries/AP_Scripting/lua_repl.cpp @@ -10,6 +10,8 @@ #include "lua/src/lauxlib.h" #include "lua/src/lualib.h" +#include + #if !defined(LUA_MAXINPUT) #define LUA_MAXINPUT 256 #endif diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index 389429a36a7..ea5d92d4113 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -16,6 +16,7 @@ #include "lua_scripts.h" #include #include "AP_Scripting.h" +#include #include From e1dd53eb1719cf44dc4a28f9a07f4cb690a0e184 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:48 +1100 Subject: [PATCH 0559/3101] AP_TECS: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_TECS/AP_TECS.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index e47122c9d02..6d6ed2c0bd9 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -21,6 +21,7 @@ #include #include #include +#include class AP_Landing; class AP_TECS { From c829b109bffe210f44bc7aa1793aabd4653e9251 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:48 +1100 Subject: [PATCH 0560/3101] AP_Vehicle: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Vehicle/AP_Vehicle.cpp | 2 ++ libraries/AP_Vehicle/AP_Vehicle.h | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b8cc4b7817b..d75d7f36e7d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -4,8 +4,10 @@ #include #include #include +#include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 1fdbad73970..7fc49a93c3e 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -21,6 +21,8 @@ #include "ModeReason.h" // reasons can't be defined in this header due to circular loops +#include +#include #include #include // board configuration library #include @@ -29,7 +31,6 @@ #include #include #include -#include #include // Notify library #include #include From a10ed0e87f2ac1c081e578e121b890d1244a0c4b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:48 +1100 Subject: [PATCH 0561/3101] PID: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/PID/PID.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 38b6f404454..ddc13ff7bca 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -4,7 +4,7 @@ #include #include -#include +#include // for AP_PIDInfo #include #include @@ -98,7 +98,7 @@ class PID { static const struct AP_Param::GroupInfo var_info[]; - const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } + const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } private: AP_Float _kp; @@ -113,7 +113,7 @@ class PID { float _get_pid(float error, uint16_t dt, float scaler); - AP_Logger::PID_Info _pid_info {}; + AP_PIDInfo _pid_info {}; /// Low pass filter cut frequency for derivative calculation. /// From d31937008002389e7e7d098dd54e2e066dfe5b8b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:48 +1100 Subject: [PATCH 0562/3101] RC_Channel: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/RC_Channel/RC_Channel.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 7005da477af..bb0285fe922 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -36,6 +36,7 @@ extern const AP_HAL::HAL& hal; #include #include #include +#include #include #include #include From 9d7cbf86f538cb9e011403d955a30b3a38bc90d2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:49 +1100 Subject: [PATCH 0563/3101] AntennaTracker: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- AntennaTracker/GCS_Mavlink.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 7d176865643..2f6ae171b62 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -128,8 +128,7 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() // Pitch PID if (g.gcs_pid_mask & 1) { - const AP_Logger::PID_Info *pid_info; - pid_info = &g.pidPitch2Srv.get_pid_info(); + const AP_PIDInfo *pid_info = &g.pidPitch2Srv.get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, pid_info->target, pid_info->actual, @@ -146,8 +145,7 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() // Yaw PID if (g.gcs_pid_mask & 2) { - const AP_Logger::PID_Info *pid_info; - pid_info = &g.pidYaw2Srv.get_pid_info(); + const AP_PIDInfo *pid_info = &g.pidYaw2Srv.get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, pid_info->target, pid_info->actual, From 32cfe70a443c870ea40238aeef40693c6ee1652e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:49 +1100 Subject: [PATCH 0564/3101] ArduCopter: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- ArduCopter/GCS_Mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 205dd127743..1078f34e22e 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -263,7 +263,7 @@ void GCS_MAVLINK_Copter::send_pid_tuning() if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { return; } - const AP_Logger::PID_Info *pid_info = nullptr; + const AP_PIDInfo *pid_info = nullptr; switch (axes[i]) { case PID_TUNING_ROLL: pid_info = &copter.attitude_control->get_rate_roll_pid().get_pid_info(); From fd4a00a6541e8fab00c16ff6b1c5fd9b886eb0e4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:49 +1100 Subject: [PATCH 0565/3101] ArduPlane: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- ArduPlane/GCS_Mavlink.cpp | 4 ++-- ArduPlane/GCS_Mavlink.h | 2 +- ArduPlane/quadplane.h | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 07a23adeb39..6a2c762c2b4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -290,7 +290,7 @@ void GCS_MAVLINK_Plane::send_wind() const } // sends a single pid info over the provided channel -void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info, +void GCS_MAVLINK_Plane::send_pid_info(const AP_PIDInfo *pid_info, const uint8_t axis, const float achieved) { if (pid_info == nullptr) { @@ -322,7 +322,7 @@ void GCS_MAVLINK_Plane::send_pid_tuning() const Parameters &g = plane.g; - const AP_Logger::PID_Info *pid_info; + const AP_PIDInfo *pid_info; if (g.gcs_pid_mask & TUNING_BITS_ROLL) { pid_info = &plane.rollController.get_pid_info(); #if HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 95fa3bca64c..8518baccf51 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -41,7 +41,7 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK private: - void send_pid_info(const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved); + void send_pid_info(const AP_PIDInfo *pid_info, const uint8_t axis, const float achieved); void handleMessage(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 33bbc2296b7..9bd0fa9e27c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "qautotune.h" #include "defines.h" From 42383dd6f2969d374d2501c1c8493db94770b07a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:49 +1100 Subject: [PATCH 0566/3101] ArduSub: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- ArduSub/GCS_Mavlink.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 26e10bdfd72..6105d73ddff 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -154,7 +154,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() const Vector3f &gyro = ahrs.get_gyro(); if (g.gcs_pid_mask & 1) { - const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info(); + const AP_PIDInfo &pid_info = attitude_control.get_rate_roll_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL, pid_info.target*0.01f, degrees(gyro.x), @@ -169,7 +169,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() } } if (g.gcs_pid_mask & 2) { - const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info(); + const AP_PIDInfo &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH, pid_info.target*0.01f, degrees(gyro.y), @@ -184,7 +184,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() } } if (g.gcs_pid_mask & 4) { - const AP_Logger::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info(); + const AP_PIDInfo &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW, pid_info.target*0.01f, degrees(gyro.z), @@ -199,7 +199,7 @@ void GCS_MAVLINK_Sub::send_pid_tuning() } } if (g.gcs_pid_mask & 8) { - const AP_Logger::PID_Info &pid_info = sub.pos_control.get_accel_z_pid().get_pid_info(); + const AP_PIDInfo &pid_info = sub.pos_control.get_accel_z_pid().get_pid_info(); mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, pid_info.target*0.01f, -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS), From c7f954c6c7c6ab44e94aaa5843cad7408fb27769 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:50 +1100 Subject: [PATCH 0567/3101] Blimp: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- Blimp/GCS_Mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 32396aebf01..3601c5bf7fc 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -123,7 +123,7 @@ void GCS_MAVLINK_Blimp::send_pid_tuning() if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) { return; } - const AP_Logger::PID_Info *pid_info = nullptr; + const AP_PIDInfo *pid_info = nullptr; switch (axes[i]) { case PID_SEND::VELX: pid_info = &blimp.pid_vel_xy.get_pid_info_x(); From 4db1e6a914237e9bd5540e7bbd2a6238cfc4be37 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:50 +1100 Subject: [PATCH 0568/3101] Rover: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- Rover/GCS_Mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index ff2a1e6fc44..6ca3f6b11b4 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -185,7 +185,7 @@ void GCS_MAVLINK_Rover::send_pid_tuning() Parameters &g = rover.g; ParametersG2 &g2 = rover.g2; - const AP_Logger::PID_Info *pid_info; + const AP_PIDInfo *pid_info; // steering PID if (g.gcs_pid_mask & 1) { From c452002ae4d15ed7f9194bd031f4fa17f31391c6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:29:50 +1100 Subject: [PATCH 0569/3101] AC_PrecLand: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AC_PrecLand/AC_PrecLand.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 945b95bbab1..a60389909f7 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -7,6 +7,7 @@ #include "AC_PrecLand_IRLock.h" #include "AC_PrecLand_SITL_Gazebo.h" #include "AC_PrecLand_SITL.h" +#include #include extern const AP_HAL::HAL& hal; From 6a0250bc8567b0e8bba8794a084f4e94b559c81b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:55:36 +1100 Subject: [PATCH 0570/3101] AP_Winch: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_Winch/AP_Winch.h | 2 +- libraries/AP_Winch/AP_Winch_Backend.cpp | 2 ++ libraries/AP_Winch/AP_Winch_Daiwa.cpp | 2 ++ libraries/AP_Winch/AP_Winch_PWM.cpp | 2 ++ 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h index 0e03f7d0a71..81cbca5f73f 100644 --- a/libraries/AP_Winch/AP_Winch.h +++ b/libraries/AP_Winch/AP_Winch.h @@ -59,7 +59,7 @@ class AP_Winch { float get_rate_max() const { return MAX(config.rate_max, 0.0f); } // send status to ground station - void send_status(const GCS_MAVLINK &channel); + void send_status(const class GCS_MAVLINK &channel); // write log void write_log(); diff --git a/libraries/AP_Winch/AP_Winch_Backend.cpp b/libraries/AP_Winch/AP_Winch_Backend.cpp index 9a01999ff41..61e395de018 100644 --- a/libraries/AP_Winch/AP_Winch_Backend.cpp +++ b/libraries/AP_Winch/AP_Winch_Backend.cpp @@ -1,5 +1,7 @@ #include + #include +#include // setup rc input and output void AP_Winch_Backend::init() diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index a213b389076..99f1921578e 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -1,4 +1,6 @@ #include + +#include #include extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_Winch/AP_Winch_PWM.cpp b/libraries/AP_Winch/AP_Winch_PWM.cpp index 7a26f931de7..afd7d770095 100644 --- a/libraries/AP_Winch/AP_Winch_PWM.cpp +++ b/libraries/AP_Winch/AP_Winch_PWM.cpp @@ -1,4 +1,6 @@ #include "AP_Winch_PWM.h" + +#include #include extern const AP_HAL::HAL& hal; From 224c32edf06faa1f41562a4eb4629fb0ebfbe0e8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:57:14 +1100 Subject: [PATCH 0571/3101] AR_WPNav: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AR_WPNav/AR_PivotTurn.cpp | 1 + libraries/AR_WPNav/AR_WPNav.cpp | 1 + libraries/AR_WPNav/AR_WPNav_OA.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/libraries/AR_WPNav/AR_PivotTurn.cpp b/libraries/AR_WPNav/AR_PivotTurn.cpp index 5c1965b63ca..cc82d9bb826 100644 --- a/libraries/AR_WPNav/AR_PivotTurn.cpp +++ b/libraries/AR_WPNav/AR_PivotTurn.cpp @@ -13,6 +13,7 @@ along with this program. If not, see . */ +#include #include #include #include "AR_PivotTurn.h" diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 1d4bbb5b6e6..8372180efd8 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -13,6 +13,7 @@ along with this program. If not, see . */ +#include #include #include #include "AR_WPNav.h" diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp index 9bbb7ea8c5c..97e7b77c587 100644 --- a/libraries/AR_WPNav/AR_WPNav_OA.cpp +++ b/libraries/AR_WPNav/AR_WPNav_OA.cpp @@ -13,6 +13,7 @@ along with this program. If not, see . */ +#include #include #include #include "AR_WPNav_OA.h" From df91effa4e13da1f2174a189c38a5de955edf715 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 14:58:19 +1100 Subject: [PATCH 0572/3101] AR_Motors: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AR_Motors/AP_MotorsUGV.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index 35c11546448..36d06fc73a8 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -3,6 +3,7 @@ #include #include #include +#include class AP_MotorsUGV { public: From 65b00bcc2ad5b983e657888352f4503f35b0a2a4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 17:13:20 +1100 Subject: [PATCH 0573/3101] AP_NavEKF2: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 0dd728669ed..a7847b9b075 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -4,6 +4,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; From 0b16c4063e5d58a356701e0b9852d8920d17a3e0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 17:13:21 +1100 Subject: [PATCH 0574/3101] AP_NavEKF3: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 50bc3497444..8cfeadeea6a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -4,6 +4,7 @@ #include #include #include +#include /******************************************************** * OPT FLOW AND RANGE FINDER * From a9b90a3b5a8d3413fd356d98958fdbe7a7e9bfa1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 17:14:40 +1100 Subject: [PATCH 0575/3101] AP_HAL: stop libraries including AP_Logger.h in .h files AP_Logger.h is a nexus of includes; while this is being improved over time, there's no reason for the library headers to include AP_Logger.h as the logger itself is access by singleton and the structures are in LogStructure.h This necessitated moving The PID_Info structure out of AP_Logger's namespace. This cleans up a pretty nasty bit - that structure is definitely not simply used for logging, but also used to pass pid information around to controllers! There are a lot of patches in here because AP_Logger.h, acting as a nexus, was providing transitive header file inclusion in many (some unlikely!) places. --- libraries/AP_HAL/examples/DSP_test/DSP_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp b/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp index 92104c40407..3f3fdfda565 100644 --- a/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp +++ b/libraries/AP_HAL/examples/DSP_test/DSP_test.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "GyroFrame.h" #if HAL_WITH_DSP From da21c47f43381ef20c0cff412eee8f03c43ab8b1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Apr 2022 12:49:45 +0900 Subject: [PATCH 0576/3101] Copter: 4.2.0-rc1 release notes --- ArduCopter/ReleaseNotes.txt | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index b871bf31c54..03108134e54 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,18 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Copter 4.2.0-rc1 10-Apr-2022 +Changes from 4.2.0-beta3 +1) Minor Enhancements + a) Log and monitor threads stack size increased + b) SPro H7 Extreme QSPI support improved +2) Bug fixes + a) EKF3 accel bias fixed when an IMU is disabled + b) MatekH743 buzzer fixed by reverting to 16 bit timer + c) STM32 H7 flash storage bug fixed that caused re-init on overflow + d) @SYS file logging fixed + e) Timer bug fixed that could cause a watchdog on boards using flash storage + f) UART driver incorrect lock class fixed +------------------------------------------------------------------ Copter 4.2.0-beta3 30-Mar-2022 Changes from 4.2.0-beta2 1) Minor Enhancements From 1240ed13b1dbc85a8a496929bfe3c8c00469b0af Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Apr 2022 12:50:15 +0900 Subject: [PATCH 0577/3101] Rover: 4.2.0-rc1 release notes --- Rover/release-notes.txt | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index 46d91ed5dd6..1423f0ae207 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,18 @@ Rover Release Notes: ------------------------------------------------------------------ +Rover 4.2.0-rc1 10-Apr-2022 +Changes from 4.2.0-beta3 +1) Minor Enhancements + a) Log and monitor threads stack size increased + b) SPro H7 Extreme QSPI support improved +2) Bug fixes + a) EKF3 accel bias fixed when an IMU is disabled + b) MatekH743 buzzer fixed by reverting to 16 bit timer + c) STM32 H7 flash storage bug fixed that caused re-init on overflow + d) @SYS file logging fixed + e) Timer bug fixed that could cause a watchdog on boards using flash storage + f) UART driver incorrect lock class fixed +------------------------------------------------------------------ Rover 4.2.0-beta3 30-Mar-2022 Changes from 4.2.0-beta2 1) Minor Enhancements From 870527a385c6bddab34998d0253d822140174599 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 19:01:20 +1000 Subject: [PATCH 0578/3101] Tools: allow size-compare_branches.py on AP_Periph --- Tools/scripts/size_compare_branches.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index 1f824dd3ee8..e9b9e788dd4 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -150,6 +150,7 @@ def run(self): "heli" : "arducopter-heli", "blimp" : "blimp", "antennatracker" : "antennatracker", + "AP_Periph" : "AP_Periph", } if self.vehicle in vehicle_map: binary_filename = vehicle_map[self.vehicle] From e2313cccc495dbfe738adc391ab60ac6c5e9245d Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 6 Apr 2022 13:34:27 +1000 Subject: [PATCH 0579/3101] Tools: added CarbonixF405 board ID CarbonixF405 a new board added --- Tools/AP_Bootloader/board_types.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index d786dea38b8..e0fdcb235f5 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -162,6 +162,7 @@ AP_HW_SPRACINGH7 1060 AP_HW_DEVEBOXH7 1061 AP_HW_MatekL431 1062 AP_HW_CUBEORANGEPLUS 1063 +AP_HW_CarbonixF405 1064 AP_HW_CUBEORANGE_PERIPH 1400 AP_HW_CUBEBLACK_PERIPH 1401 From 729466358d32ddb1a524280065eded14a833f91a Mon Sep 17 00:00:00 2001 From: Lokesh Ramina Date: Wed, 6 Apr 2022 13:45:38 +1000 Subject: [PATCH 0580/3101] hwdef: added CarbonixF405 AP_Periph node added hwdef files for CarbonixF405 board --- .../hwdef/CarbonixF405/hwdef-bl.dat | 74 +++++++++ .../hwdef/CarbonixF405/hwdef.dat | 143 ++++++++++++++++++ 2 files changed, 217 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat new file mode 100644 index 00000000000..c9b65a069f0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef-bl.dat @@ -0,0 +1,74 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32F4xx STM32F405xx + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 60 + +# reserve some space for params +APP_START_OFFSET_KB 4 + +# board ID for firmware load +APJ_BOARD_ID 1064 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency set to 0 to use internal clock +OSCILLATOR_HZ 0 + +# assume 1024K flash part +FLASH_SIZE_KB 1024 + +STDOUT_SERIAL SD2 +STDOUT_BAUDRATE 57600 + +# order of UARTs +SERIAL_ORDER OTG1 USART2 + +# a fault LED +PA15 LED_BOOTLOADER OUTPUT HIGH # blue +define HAL_LED_ON 0 + +# USART1 +PA2 USART2_TX USART2 SPEED_HIGH NODMA +PA3 USART2_RX USART2 SPEED_HIGH NODMA + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +define HAL_USE_SERIAL TRUE + +define STM32_SERIAL_USE_USART2 TRUE + +define HAL_NO_GPIO_IRQ + +define SERIAL_BUFFERS_SIZE 32 +define HAL_USE_EMPTY_IO TRUE +define PORT_INT_REQUIRED_STACK 64 + +define DMA_RESERVE_SIZE 0 + +MAIN_STACK 0x800 +PROCESS_STACK 0x800 + +define HAL_DISABLE_LOOP_DELAY + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a small bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 1000 + + +define CAN_APP_NODE_NAME "org.ardupilot.CarbonixF405" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat new file mode 100644 index 00000000000..25c37b9e7fd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixF405/hwdef.dat @@ -0,0 +1,143 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32F4xx STM32F405xx + +# bootloader starts firmware at 60+ 4k +FLASH_RESERVE_START_KB 64 + +#MCU F405 store parameters in pages 2 and 3 +STORAGE_FLASH_PAGE 2 +define HAL_STORAGE_SIZE 15360 + +# board ID for firmware load +APJ_BOARD_ID 1064 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency set to 0 to use internal clock +OSCILLATOR_HZ 0 + +#MCU F405 Flash 1024 +FLASH_SIZE_KB 1024 + +# order of UARTs +SERIAL_ORDER OTG1 USART2 USART3 UART4 + +define HAL_CAN_POOL_SIZE 6000 + +#STDOUT_SERIAL SD1 +#STDOUT_BAUDRATE 57600 + + +# PWM outputs +PA9 TIM1_CH2 TIM1 PWM(1) GPIO(50) +PA8 TIM1_CH1 TIM1 PWM(2) GPIO(51) + +# USART2, ESC telem +PA2 USART2_TX USART2 SPEED_HIGH NODMA +PA3 USART2_RX USART2 SPEED_HIGH NODMA + +# USART3 +PC10 USART3_TX USART3 SPEED_HIGH NODMA +PC11 USART3_RX USART3 SPEED_HIGH NODMA + +# UART4 +PA0 UART4_TX UART4 SPEED_HIGH NODMA +PA1 UART4_RX UART4 SPEED_HIGH NODMA + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# LED, active low +PA15 LED OUTPUT HIGH GPIO(1) + +# spi2 +PB10 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +# CS pins +PC8 BMI088_A_CS CS +PC9 BMI088_G_CS CS + +SPIDEV bmi088_g SPI2 DEVID1 BMI088_G_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI2 DEVID2 BMI088_A_CS MODE3 10*MHZ 10*MHZ + +# one I2C bus +PB6 I2C1_SCL I2C1 +PB7 I2C1_SDA I2C1 + +I2C_ORDER I2C1 + +# allow for reboot command for faster development +define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define HAL_NO_GPIO_IRQ +define SERIAL_BUFFERS_SIZE 512 + +define DMA_RESERVE_SIZE 2048 + +# stack for fast interrupts +define PORT_INT_REQUIRED_STACK 64 + +# MAIN_STACK is stack for ISR handlers +MAIN_STACK 0x300 + +# PROCESS_STACK controls stack for main thread +PROCESS_STACK 0xA00 + +define HAL_DISABLE_LOOP_DELAY + +# enable CAN support +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 + +# ADC inputs +define HAL_USE_ADC TRUE +define STM32_ADC_USE_ADC1 TRUE + +PA5 VSENSE1 ADC1 SCALE(1) +PA6 VSENSE2 ADC1 SCALE(1) +PB0 VSENSE3 ADC1 SCALE(1) +PB1 VSENSE4 ADC1 SCALE(1) + +define HAL_NO_GCS +define HAL_NO_MONITOR_THREAD + +define AP_PARAM_MAX_EMBEDDED_PARAM 512 + +define CAN_APP_NODE_NAME "org.ardupilot.CarbonixF405" + +define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_RC_OUT + +# enable ESC control +define HAL_SUPPORT_RCOUT_SERIAL 1 +define HAL_WITH_ESC_TELEM 1 + +# enable GPS +define HAL_PERIPH_ENABLE_GPS +define HAL_PERIPH_GPS_PORT_DEFAULT 2 +#define HAL_PERIPH_ENABLE_NOTIFY +#define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY + +# default ADSB off by setting 0 baudrate +define HAL_PERIPH_ENABLE_ADSB +define HAL_PERIPH_ADSB_PORT_DEFAULT 3 +define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 + +BARO MS56XX I2C:0:0x76 +COMPASS MMC5XX3 I2C:0:0x30 false ROTATION_NONE + + + + + From 0f230eec5ade6005945253eb3577c2b79dd4a867 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 Apr 2022 12:59:49 +1000 Subject: [PATCH 0581/3101] AP_Scripting: increase default heap size in SITL and on F7/H7 if we have 500k or more memory then use 100k heap for Lua, making setup easier --- libraries/AP_Scripting/AP_Scripting.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 256a66091fa..340dc60f162 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -33,8 +33,8 @@ #endif // !defined(SCRIPTING_STACK_MAX_SIZE) #if !defined(SCRIPTING_HEAP_SIZE) - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX - #define SCRIPTING_HEAP_SIZE (64 * 1024) + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || HAL_MEM_CLASS >= HAL_MEM_CLASS_500 + #define SCRIPTING_HEAP_SIZE (100 * 1024) #else #define SCRIPTING_HEAP_SIZE (43 * 1024) #endif From f4a3a65797dc7dd389c9f9ac100c63123a2163cf Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 22 Feb 2022 15:23:50 +0100 Subject: [PATCH 0582/3101] hwdef: add revo-mini-sd --- .../hwdef/revo-mini-sd/hwdef-bl.dat | 2 + .../hwdef/revo-mini-sd/hwdef.dat | 40 +++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef-bl.dat new file mode 100644 index 00000000000..a0d2d0cfabe --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef-bl.dat @@ -0,0 +1,2 @@ + +include ../revo-mini-i2c/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat new file mode 100644 index 00000000000..eeb063095f0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat @@ -0,0 +1,40 @@ +# Revo-mini target with I²C and SD card support + +AUTOBUILD_TARGETS + +include ../revo-mini-i2c/hwdef.dat + +PA0 UART4_TX UART4 ALT(6) +PA1 UART4_RX UART4 ALT(6) + +PA15 SDCARD_CS CS + +SPIDEV sdcard SPI3 DEVID2 SDCARD_CS MODE0 400*KHZ 25*MHZ + +# disable logging to dataflash +undef HAL_LOGGING_DATAFLASH_ENABLED + +# filesystem setup on sdcard +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# disable SMBUS and fuel battery monitors to save flash +define AP_BATTMON_SMBUS_ENABLE 0 +define AP_BATTMON_FUEL_ENABLE 0 + +# disable parachute and sprayer to save flash +define HAL_PARACHUTE_ENABLED 0 +define HAL_SPRAYER_ENABLED 0 + +# reduce max size of embedded params for apj_tool.py +define AP_PARAM_MAX_EMBEDDED_PARAM 1024 +define HAL_WITH_DSP FALSE + +# save some flash +define HAL_GENERATOR_ENABLED 0 +define AC_OAPATHPLANNER_ENABLED 0 +define PRECISION_LANDING 0 +define HAL_BARO_WIND_COMP_ENABLED 0 +define GRIPPER_ENABLED 0 +define AP_OPTICALFLOW_ENABLED 0 From ef1058e60ceb616484f4648fbd504e5676b2874b Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 22 Feb 2022 15:24:16 +0100 Subject: [PATCH 0583/3101] Tools: add revo-mini-sd bootloader --- Tools/bootloaders/revo-mini-sd_bl.bin | Bin 0 -> 14136 bytes Tools/bootloaders/revo-mini-sd_bl.elf | Bin 0 -> 365196 bytes Tools/bootloaders/revo-mini-sd_bl.hex | 886 ++++++++++++++++++++++++++ 3 files changed, 886 insertions(+) create mode 100755 Tools/bootloaders/revo-mini-sd_bl.bin create mode 100755 Tools/bootloaders/revo-mini-sd_bl.elf create mode 100644 Tools/bootloaders/revo-mini-sd_bl.hex diff --git a/Tools/bootloaders/revo-mini-sd_bl.bin b/Tools/bootloaders/revo-mini-sd_bl.bin new file mode 100755 index 0000000000000000000000000000000000000000..52cc65dd781062eace5a1172a0ca08a4bc6dfbee GIT binary patch literal 14136 zcmc(Ge|%KMx%ZqOo84>zY_dU;O@Q4!pn(Nk2udVYHoM{E$AUqk6@y+kSkyXe&x}38}XyBwwc#!E7t`04 zeHngyA<+d8K1P_d2y_v?jPpOG*MCKwu^az2Iv;!mjsL0L|84om4gXr-{PON7yR9G3|B1H$c>7k?v>6CA93A~?AD30iXO zl8lnwN!*E&N|}(4|I?tq{qn8JIw|Ic!K~f179$5#Mt)Ld<%01_`7Zm+^4y9iowo*y z6+SH%$t%yfCS@u&Rph!`oVNw^Tqw8#LUPSV&f4;{$7lHG#(+OFu=%9jB7+jer22& zdpoDNpLLo-Ou@6x>>w`|U)p^WA7o~c*|g@PQ%SZPQn7K91HO0CvZJ_v4CdXQs11~GQoyhl$a!2k{_)$Z^9Pe3X z*>NeYosli085L8?h)_B7?QbyhUt@`WPg)^J#&$tsiUf}rkQb2GqjTqOE$H-0LMLk` z^5QFt1h2=?MS_$+T^!NfD-iRYfM>g}uP_Ai#2krP=nU}%P0o>Utz?DFicVYHtr9mt zyxed@L5s8C1UN0yz`88#g`=q8P3t7_37cvd3v*yt}Sj9c3}e^JI*j(alv39co0_E_w>il1NQ zSxJ&-r1hB!uHEQ)rkp%OgfxGZr&(+jXNWB#gE2laI;(x3J69q$B44+Ij(SUsYvfvq z?b2CVzIl{uwG>@vyU@!#zgiLGw=_i8hw!}=qFab~B4lW>Ug(H!4DtPlA47ND=~@E@)*=KgnRcStDqgc#Gx$k*|+UJ9l>(ZyAZK z#rJajI~b{y^HR&8x2{;~0H3O9OyC+~odffxTnc0KF>SQ;Vp`h$vAv^MCGpiNON};<-y10h#SbE6!7oCIlIOy-Pdmr8BFS$JG3MKDC$b^Nn(gRwZwz#F z_s4dR&~~TBc(JjaLgtumfYFisf$*y$ooSgy7X79pz73_n9hnnh1lC*t3crfY7H2_M zUvg?s(>eF)2xE81(sur)!HD1{v#0wl?pMY8JhzFzLZ6lmtv#IR)wZFpAI?o`^V6|K zknH~%IWj;})yox!b7W4tZAd&kx#B4)wO_+EIhS2tcDQr6VcW83(cw%t)6}`#`j-UP zy(7?$7BW}8yx=g?%C&F`V`G+l9wE@#HJE2#o^w3%bqMLS9N|6iW_XP9X4%lB!!ze+ z^u9A-W-FHKUz#m*BtKXo?ks1X$}S;tZH#GYC{ry&-WjXBc#!*EIn(t1;A1Qu^^RCd zoI)M?py}VKSs0-_e}9Zp{p1jlpC6&nUk;HQIz>OY^|~|r^voMGdiAYL!Q{{lGRI}U zXe_Bx%(rTq%0%9(IY{KpQGUsGF%l^R{iP$C#!Hb8l&qvqq>PXV5!PAw1AG&ahZT~) zZRpS;+U67SHV+))n~7YBTAy8m_i-GrC64!Enha&c4!sG6jGqyjdV8rT+t8k`hoa7Qlksv_@(HD7F;U{Q68m*bXNUA zx*B|dyrrSU`a|S5N7U$yvAi-OhgIW|_;RZXA{G}9riqI4z zXS_y;#%+=`?%dt2rkzgqw4sENU+W}he<`g?X%q&uD|suq?A9Y%(54>6G2zE9gxnd zMvn%;Rg6`leRgNlo(74TKj#n3#V&GuFpcHK-1ORZ$0egz}yfA0A ztV}nPQFirRlJgS7HaTa7jt)26FNBKYDGOo_xib-K5oZOL2@3`0*7OqQYkKi{=hPEZ zN0`m4@_yD%6lNnU4o4VkPlUCejHIvpFj64&MT&(;q+0kia*uF6Vh}aw=FQ&aoa$#z zH|ACO9VW_i+FzxB=>CAf=JR2VyK;zRw$%7bnfxnZ7O95JXasmX>tOS_Fz+^7CrTN& z!3w)BS*8i6R8;wP%4V)=qINuO;4UX|4A8SCCl~s|y>H*i6pTc$-xEWCDKv~Q%Zl2c z93hTpoLtz{&Mah>4n<5f$J}f(bQ#1e5l&>qkSTM7$t81QVSg!G!ih~zHiR`dHx}`i z&hJxPGgoGbPb;&`T5-3y`t^?#f8#Spk+$19>6|SSZ>&|L=T+p8V*@(&d1rpmDt_2e zEIvgv{S8Eo9;RjX7jkEmT<*{RPV;wbsFs{hXZpBJQnMaleEa{I9sz$8kA#^@a)D37XVKwg*yI>Pwz< zP6}lxOwk&LPH+qoo++B=Fv=>o>*6YMIsA`^z017w)z4YUC1to657(YuO8ajNlx#Sq z>|1P-GPd$_riLxwW#?pua~{cjoDudqjL!5hbC4fcK^U1;fmgz`S{`9Vvqa=Sj<81z zU0GNQ9(|J#tW5XAl+G(FKHy)~a#4L4PFgS?p@1*+d*q}zwf*u{?wm_zv z^AN%1Io8J(Ftgd~r?@o_^CH{GG?9T_>>QE&n9`tBe=?TpX7_#2N4)&7u!ZK%5fkJ8 zw{iZ6zbhYde`E^LJ%{sqnq;=_fZeQr1iHvJ)>J%_y2zzi$(~0t+o^Uo*RTTj@G)gy zR^p^U`%)O-w(+8|y-~T-{!*uTI(mbZe6h+8WRcn5+AK2+k)OV@OSy9uZI!0!ib>?X zZxnW6j#_w;$G(uK5<&PDuFjnP`Ozs1Ni$Ia6z6$aEbs#&B|pr64; zrNqw39?opjh8vaTX3N}4nQ&b1%(*GOl=s%Tqv2zHlMA?WT*VHr*4pjm*3s291M_Fe z7vy4!%VpX%x{?tl11mK7IIHa+U(?PUq)=q+i&}G3;f~nRXE{Lnw{wpk`I(OwCyC?j zi8BM=|EShJU57ruG(IY=_2o(_7*B3H)5yr7F{YV2$jEQT<<7Ov!5#;`C<)$bj>ui3 zQnfErB6EjBxqFx*qhlb<50tPMBdH4u1FIB^Oy($0dmzjeio`TW{=f_h;Wl^r8*dX*74KM{`gF$KqKcGhyW$7@}+oY!AUWsn#H znSHE}yX#n==B{E7Bd=FjLGV8cEdh=Am^I=m-x<$jF-y#D$Et*}-Z!@2Hv!*0wh$IW z*9E@=du%5mJ0w6nCmr*Z)qmARNi{og%fO7-(Z-0Ol$ZTHZ{FoQ$F@M)P>mIFXP!_L(PeAcXxsuRM zxlDV5)uM~r+MukmYrp{)$_yBB8u-?XeY%aFZAqiD%IuO@e=bhT8cSk*@dp}}#dfOk zL;z(wm53R7amR>PYL$+5?t?xWWsl~j5V6qww$ViV^eAoL*q(yi=F${l7wY=EC`|JI z7J5aguz#|l#ko6p-n(z4#Qy09VyncC*Kmp3PUaK&QbVd0HO$nnKrE;FCJR)W-^8q; zIcFmKp!uA|73}j~*7}K8 zSoc=s=AHm?j#1Fij=7lzt~k9iqe zW3N{NrupCSEj7HNl-bSV5sYM4$R)LTIqAIDgnEX}H-{XTS!a`T+tAj-_qyA>tZ?2- zgyZNhZDrd~{bAY;-I3nzn|#*`fM>7U8hHggq^(Vk_hy%JJE#-+Gd00MCeA?)T1W)c zDtIQs4^(n(UIui9t-Q?;PUtP=(0k)lQ#p%Q+ND&kvcx4xc~09Ww!emuYv)0UQYN+! zLu{P(3G}bbK0Sq#r!Zzw-^0jvDoj2fVqs}#9k#h!A~tt3d^$zSV02uVXJ>#Tq^|fwe!Bo5Xr5)&i_YV?T*xU6rv-oGVgIRiodCkD%|f z``V{oE&Znk)Q*pRmUcD&pS7g%^RSK9(ET0ZPK3gw-V)nwp_HOC9aH8m?ww*=)cer| zPF}Kbq0YK!Ve+?R$vYd4Kp&rVW}P@LkFsF=jOG&QKNqyBgxrv9BbrS$h#*z(JML| z{}^F$qB-723R*n=(tWV82zI+2^h6;2RC>}mdZJq5JWh|nvnE-tOZwBe?{erPZ<`dd zb<+8$>vH*UHu6w=!BvBO8}sy1Bm}RN(NpaijC_f1@0aOn+&Mno!`TumnLJgV>>8}m z506LR>!N+K2i$pSD<3<;i-wTRr>|fR5SbaFn({Z5Z#A~l^i3l+-x{A5`;r>ubdN z*6iE|%bxC^G~W==;f!YXnIx;{P7h-hYD^ES(MA=gH6OKI#-p3!eO99vgVe4w1kzWi z(G@C@TNHhHgO$!|i^2#cN_IN;F#Jwe5)!KY4PTb9eHOj?2HLq7`nwT<$ZRK9afehT ztu-|}M|-h{1&M*2+u>oY><)kFPN{9+yR1t|pU3)K%B}O9_!iF7R=AYBdEJsrG0bDF zoT8gY@YT+18^|X9(%JA`8j^B^j;I6{i5jg-uB>K}?gOkl*SDr-3Oo^u;`~VHl^Pv{ zJ*Ep_v~j)Cq1>LVr|T+jPjNfJwQNY-=y7>?lv;xH#GBw92VSOxOcLu^<+-PZ^8dXz z6ZRCfAI#D&CkYpJagyM<*Rvct)HyyB(YU#REcSSx#tJ$Msjg}>DG#VD{DHjNu}=#c zF&(?kw_^ojm6CO1i*r)gUi(fTGi$C#KW|=;zL zwjNgJpE{A=p5QCBA*t?C*(9U~eVCIT;^y!nW{$Qj8}q?m%FWgEYRX7|H}Lo};35yc zikpbe3_FuY><_dY(TF;cIhX~l?^c)t@OLP#ytU+=KC+lt1wSWv{qJ@#FR_aaVz}c+ zPJV#pnO+lYbR*89yu#-33X{$(k|S5woia80Y|;a*!(5H~UE=oeOqQ9Ou!qy;a$#!U zO)i*vE@3C@!bh-E**X)}GPRWXUNyS*HS65|P%QFAE13kd(SL0~GiMrh3P#Skj?2k0 zC-`TmfE5dC?@Ekmo?Jn#{%C9xdSp)aDBZeIV*Qa9FsI)`p#7$6 zQ~HWY<#S=L>WCVBP}L0lj^zgoB(+?PdQ{Eu@3!!mu_@(=H4EplVD?;Mru^Ln8b3n# zDS{tXoDMTcjbk z`Ka}gs%zKWe7PgPoThq_G9cw$Fsb(=DyL`=L{GypJL_d|REFhp-he zy>VkJPD*FFE2pRql3Q7aE|3bJ-b0;gR93ZalaI^|22(1kJ?y~lMb#dx-a`$wCXcS` z-{)b^1HQmKxLg-VL!Bnn`A|(o9o)A#ao-|2hZrG#gUM_6? zPG|bc$zr;I6ItzD3jZBZ;cYm_;0(XC&l%rp$0zqBFcByJLXae0T4B*ak-Atq$2%s!_K(DD->O=+{)oCFTZh-qhY%GQ&TR zJ!^%-gq&&ZPQgd|8O^M)fI0%&UZFB(rHj9nw?`s7wUMyRCk#0rvSE)da@lHd$GIWJ zLnW$4jVdSo%-M%MpVqaZ?gAAzk`an8)Cj;AYSMPw8WPx7#^>)|sIWyZcf9009unep zgqpuavc=zmNA%t+t{3Hy^49$JTuAxzaR&ERSVe4CLR}Yh4xJx9>VzS{mZZ2( zbTnd5GIW*Xkq#;!dOv%vV+izC^-zc^0odmAAQMh&-J4U_36#dr%J>0la=l z@USJiBo%v{3dXnArI=U6sn9<8iG$M81{f;?&p7z6;*1=>yEH-r)aWI3zZ2GAyp-9e zM&DMc&xWqey!)i`aA7NP201qazjR#p?X2S<`VSxKR+^?Td|Nv;gv zU_JA#o$!&w+oL`)E~cS2$%IWY*}YaW4iRrwXy88$OVRo+>DEfQ;i2<2sqV8Iga>w^ zeKmUfr~&puTaBSh;~tGr8xnr;gx?H(S^^toDFWIcbT87o)5_8+8l7n;)7)qBGEHyI zu*w{}*I5{Z7vC}-eYUF#cis+!zefs}5FNd{o(PnNKM?Ah>iU^Ps2y<=-|sZu!JJe` zD#?E{NMncG;5d#La9T1iEt!|bEb?am*oj-#dl1&1J5eb!#29vSfyM6Z zI-#T=yRZJwN|ss7JE4$sn*NlVHT{n4JEU(c*PrZB78f#BHb!LOA|dZ6i*?v78>C21x5U>~1~ z{2Fq7oGF&g3{%7TT2ro^%7pG*e9aKhtT1C0{-Iha?{|6Ev+lO_n9V_o|98x1e}LP{ z*x8~bKsGLgxP9ub$vWuUtRy@+vmv^mwBy^$frhUxX*iLxZj3%A16_T z+;&V5Je%q(hj?R~x4?&)bcPWMYSPh{7h(mzv)V+zeg>G9k?Viyp?gfBxKe^-i|&NbS1I)5Em#KFNWOm4Lw(;+B?qn7h?T=0Ce-I{lfRVGMR#(JG&L)L)g%*=u#2VE}ck|>gxN~dut7X zvF1A8b+RF4G)(y@#!g{`!qB*vj96Mo3@qlq@#6ZnTBYkf@ZHfs49@yS<#q#fBTm_Mkujn-g$aK<^`tGYKOyQsLzzT*W(NUKl7S}uKIFj?Ha5r4nBnM%=91|Ka~+u z_TQ9&ugveIoVqm0=y6L`9=aA#UfTnIkzVu}rIVhVPKRw*&>JoWFQ^?qYvrc;lzxYi7f&j>?_f>&PN7!c^C5I9N1M{4>EFd! z#IsVqxK?T|jcI4bOU`O@<0Yr1KXmT(mF=mPI!5KoWb``gp3ZFPjMV6ybRyG)ll-iN zy_r@<`MgKng>{u$_Bc<+_)8le7cR2ghV8$HCtxsS5m}r$ajw8V5uan=iywS()o=7& zFMZ~zV7~sk37&ak?p6$`bK%A0U}NhsZuDEn^*gf+7&m50YE}?y*(6Y3^1>pQ zQ;QKX)Z`No*VLVzg#?f2qN;UfE8)SawnGdlN{;2Hd+8IF#QLT9w@rSsJqL$0M zh{vO=$MQ9ro?SQxbY9t6HXdC%c92hpY;}c3Ic3VABn|f=sU1Ie{`K?)@aMt$rFGqddIKv@;n*QL zvjdcfyt|=e*m0vS%wy&f>scwcPCMLiccZgDJcs*G$u2YYe8RCk)M9yhybTZosB!#3 zK=l2>-tp@Jae%%*Ue9eCe(>-)Z2~(}eGU-Co}|_SqS!iB&~6*P>oCP<)m0I1{YT1$ zBe>a#kFu@4Rk}a+*vL?Y-pEyqREKc{;_*Q!! zjAY0h-WyS>KUlsy$bu?n0<$|#mHI5{+;i1){lyt!x>^ho<6DURrC0NAN#@;hOO1Tz?E47ty$+F{0gT;gu(H`KQVYcuE{3wTe zkCZCDmGcsdru8k{wc&Lm?ktY=>72*<^cgNMUBim&;x`!?$gy^NHe){;kJ5X;P#+I3 zJQMDg^qtw$(FdGkIA4yZ8)ubnPq)ulvct#K;ATGPs^e;?)pq$!dgn4pJTDcs*ZPdp zGo`j#W6AkioWIWr-!AB+XUy^FN!$&8ex|SsEfKsXz&_ONV~UP@Tycu7Bt=&ZY}#6G zL*iD{5C0aAw{RJ|zh8G7Z2jM>8r;=T>p;psl?h zv5VChdAib8x3=WpO4@4gD~UvKCYg|v1#&V{ABVnxgS7b8x4$4Omlrl4Ve7g*&N?bT zDlfe?A*r%AAt$s;QbV)Tqp+`>EU*ysvDlGmHE! z^nDG9pI<(YB;+E@mnhx}*Xnz9x zdz$FDG554)z`^0%Zo^5!_H0sGkk!ozS^cxsr&TI>arx;Jw3QDcbHt_M818`E>WWIf z26^39@+rLEl+V+`@V&S1tlQhp7qyxuhm4-%cwKtjbKGYt+390zS|;WLc)b`Mq@9ljd{PxP4sh zUZ1Nr4gPzIuM1r7zP`-lb6iq0^se|T(jj#VJrPpav;lcuQ$4=xXH?|YXJAHrsImij z3|APNSY&`0{f;Vsq#TbunfZowD81Pn>oX^zt!@4H&H z88zCTg!O>^J?WL#b?E|Xv@hOM^ygJVX2LJD`C|9#%@cByI$}st=;V~zc zYTrG0wQH)YmEkq2HPx}nxVmmlZFSu}2y_%Ei;n{F1Ex)B6$Z&M)ZjOGvNw|r{Wwz{G|3uPm4rP z$1R?@?iMLiPG?8Mc$+MbFgczUiaFGIICADro~-rR_qGhw4sS`AxF zp4^Hm=^p+{Pd4sCo?@=_zkna)K}u_Cg=h;bQSlzUx{m&pj!vJbrWup}Ew8RC+}dQX!;1(eOQ~6~>Jy+X&kZDNRKa zQX!=1jna6u0Dj{MzL>6Q=ey48np)srS7EG2?fT|Y0V%YKR0yeaJl`{=!ng@#o0?0X zLkg`T6+)^ODP4teGs-rj9#Uu(sSr|$R+Tp=+Dx(N0y?ZHfzcF;7PGJ&F}*o@8F4D& zgNXHre}Q-!;^#)GG*c}3fLmXuYnK{zjT1Rd8H>(T$D+|O_}#}SjvC#7eU1p4iX4$C zFxa~LNCu1dXbN7nks#FrPQ-gE8+}bo=8hn@>dut~i^OHimZ4e#UTohw0WG=;Exvip z8erhRKJb>0;u?8EdXsPt;7^jU1@PaKFvTCko1#91*?3=+fiN8*6=4{8A0sRTOz{}{ z!LPo?w~5BSbsN{ycpd!Py}0k8_X8B?mh1$6@s@SpT;ryMew3;0pE;LekhOl~6 z)M^;kL8I+7BAEzlKr53mpe5o+%`CnfL|G1<;JH*jIYP2!{l9OXU+>%GoBz-{VqdtZ zVDU{iFDbmG*y$=+E`kPz_{9((1s(1GE1=6pqU)SWbnCB6@F*QRLIWLt9^wW{5{Wpk zmpThDPQJ-+87vuMthYgHr%uvkS%!o24 z%M#@*_S7MwcZ^{yf@Bg(c&x;uFy>)y@O2;fl#EDJkMx&}6-mc0>0s~Rd3m|dgF2|6 zC|_i~vizB0Xxp%F%5OT%uc%Kt@#l^wYD;f9705q{fJ8tdAQ6xVNCYGT5&?;TL_i`S z5s(N-1SA3y0f~S_Kq4R!kO)WwBmxoviGV~vA|Mfv2uK7Z0uljzhEcL9E}JNs8D{L0f7>nBemx;x$LOv*m%fQ=@o+Yva&DWX}s+H$5jFQZ)3T;h#nrcszVVGsZW@H1i-arX#tDj*LMbJYPIL zGmN=UVtikohMfw}9QKuc^P3S{%c0N{fu?`iH#dD5euQ~9oJ-x5AJ{#Gxq0}@uAVj? z{y`V}xTlS=k_H}rq{k58;a7UJz|qE(iYNT87GCs(KdV^_Ogg)1wARe?*Z`_|e~+!6 zg|CmWu;mg9KhT{YVBueZnh`;727J|K2%Kr-%(PE$VBu-qyW8FBeEn>06VVD;_{MH^-^~%N+uq<7ElZnvC}geY;oR=j@|3B}JG0~N+j#gNU9om2P4|Wz z_1+L4?+sQXSB+eCP_M{dThv$`@-`}N7B0Rx%3B?@H?d~Ir%7e{Oba-@k-wMT@Rm;K`yR$1? z%bCZu$hp#Wt8?Xz%NH!Vapi(r7pz=R;}6#<3n~}Xc;de|)6ly2K&~+n4qcOiyTUm+XBOh+v_gn?r;`T&yEqk8` zvGAqtUzR)8?rD5>D$hL>Os$Qf7Q^gET~C$&q8GEWIA(0;u5z{B5!^M6?P6YQ?VRBD zz|KIwz>WY%A0O$?tKU|U9byG6oOvJhdUcn&Mcv5?&Yqz1Ro&{&-0_(OUmoR8&yIM* zZyK0>3h4(%rjNvLBBh$JUg%7}gx_26`y-^^AW?a9{CA`oYjeEb9mv0rvII?a6nC_< zt%v3N!rklHP9^u%c9x6G_K-nbftX4Pyd6>DOiIcjg4uc^`GYXyFqL zBiPxK>p%X|6jm)scE=cl*RWN~HSA^Kfn9x0-Y`|mz0kH0&#%SvT-(fWj(IA^ZE2Ue zT{9cD#ANMVjqO8I&bFfWU8c?{r>)jox*qDrDq&&a&D{g*tzoNbEzp};UbJ(2&rNO9 zpz-}SHvHL^SjkhZ)TRwRrU1*|*}~n|T+71tF2!AlHXrSR9{rZCO&7>+Mwb@YQcpQ| zm)_2Gmfzm`YJg^1nnOXmsmIr#^bZ#Xw{frHE`o+%bqxsQVXVI3Hyxun_xS~0=nIF| zYopJ#c`Mj}zO}g(uLf=oUK98g+BC6a;Y+brt?Rh{rR?}Yw=a#FrY+*R7k_IsT=z3cj^*vghwH)T=LGbnIY5qF)bx ziSJZ*s1aVk$9~nuFlOgto_%}HY2fQfHY!JY2iTmyMA%I1NP4ONu+*c!Z+9!@lPxa{ z2&*i=xje988hQ`%(OeeZ&^7b)bLtPK@vZNk`;kJuzOTzDG%Q6M3}-eN zMz#^=?_MIUpT5Y#PhX(i-(O^d8UwXJ>vezDvHl}ck6L!}qU4rAVO33g-cdFu;+|mG zIyInq49~G}TDLZKeW0za1p3EaFl;&7_I@NiJ|>J8SR3;$%GLI2TUq#ggypa6c=1KD z`G~N&_eE_x3(rKYFD{e&M9A$Ba`yqv9xQJb}^5lox_Fv6tuzw)XG{2BWsK9u{=ks!TaEi zYl*EtEd0iWXlK7mIa67*E&BPZfqT9f8@THW8Z$Juc0a&b zZ9@uHdR}y5KQFKaQjgkZ3=Uyb-k!Yl>27rt4|oFkfqhdp`n6V51=i|X?yhZ`+0lhr zb9WTGW}6*)W?}c*f^lkUU|uD_bCcRl>?aXZVYIU)>aKqxVq&rVXl6KCh@FgC0(yg% z8|{2ES{LT*z&6-A+3xTy>h;8VnOii2enxHA46LfA@T47ud#L(WasM&S*Ugjrk$=0e&v|GjE7%qWbWE2RF`y=G?->+f&7pgB@Gwy8!6ZHWr>f6 z-)<~{9g?$S+*?}ro!8bG4nzN}-RJGTmIy1!3c#MXyX_mE-2x6XPJ1+xS6Ecfy!z7h z$myPF=gZAN6@3j+n=7)YrOnK(ovdvbf?ed$Ijf=tvTarMzOxSBeg480-&rSqhvPRF zze8J9Pwotxn{_L*=Y{-dxw6%t*YfiDx?8+0lLVEEUlrW`_%-1-y?LT{q?b>ymGP@B zfv5c$hchnlRkL!QtY;CvTnU_S}DS zIds5Ae?~1owk2mytZ<1>`M8cY7jfL%UG^q>;9{E^Py#Kkvbsok>cWzdBMjWlPc21ZmxJ3otp z!w$6T%G3V*W>4Uw{gVQZF~gZU7VUhA$_k&!?pOBpetmeKvMpu*+<$*0FLZ z`qx(dWWiGP%zf$x|6f|Lc7yk)R-CK8jBMcU?lu|+N`-4JQZ0=Inp-r_>C@D=7^t>OuUn@Qau9X>QE^%&u68H(p|I`d2a2X+HVM+TYBtGf!b2ipsv#bj`}T;xrHw@y8EIvSjmf{ zT6;PhaObKp=PdmA#f_1Ziu=Aqy^)%c}t|MPz?uFcY&#OOJuS;cgV~zQ`MvWhZv6% z_MRc#tp`pe7pW)J^82byo`couB3fNjF@MHh;TMpKi#T^8&hVZ+(b1?Qe;1Y>pYuFcV>JBZaNei(d=UcKj^YIQ}d#l!7rkrjw zk1T1J70LDeEp7!Z%5#Kflh@P}p`A51hbGr+xxuVxXKfF8I~}9_&5Lf#=hu2-+Dn?q zcRalERqj=5nfJgcUUaB(LzP-}sH(I|t*)IyK8W>!4LER0op<1rVcw)54=;`=UT^LF z7$wjl`fLcysyQA^4x|UN>ai-JuQy-XU6aV)aA_n?3{79w`mo0~GIn1`3G=l3g3NQ^ zl;*jwl7}B_48H>Gv?&k%GsFixQ}Yww)uofydojMB4ps-lzvJQGpk7SIBxDu|8N(Ga zgRp;Kntj~m=|cVHt+X~?yszi6wkt)K%_d99rD%_%G4UH7ES8fy2z|T0}v=h znH@d00=Mnhr}xmiE!z^AWxc@yrX&h2#-8#>I6PszWvtcZJw$AK94L98@u-p$%C40%6cVN~K=Xl#TjC?h`PAxsH zcztJ;QCs~u&DP}}#jd|@YD>~lUNEsm!Q1E7I##eBqiDMFbhYC7wA#Dq&9;}%uV2T) zJL_IP?_2jw>$XUskQeN3eI+uzkoJ&KCtp6#R?TSX16{jUjcnO-{y;S^*mATw1WEVz z@SI|QB{H?p9oUDSY-}kF?WtBnpH;h1&%SC*i|=d2ztz94W9>^fRqUx&yq{Gw?;*68 ztgP!;@e)Y~iz0L% zM*F4~_BE>E6z&dKj_~l@2+!AA6r8lvUn;2B(N<8g9d|n8Bj-d<7FD9gRw`9kq9_!ixsr$Qr)|TGuIXy+KwGK(0=$tTmr zZ+yk@x<o`n2BlMqRGf)68mXi_M2VU9n$t*Ol|)uWfX8qdAsdpalkF+!+fDE&OP9 zy}ijiUMc848mrfNA@+L;^X~F_oy0FW2MuY3jWy+Nc#I zwG=eceAJstYw&Iqr1L^)CH8I1)3a?YxRr7Q7X;6>eM|4rZ|gNwIBWDZSs^9l3eE{; zRbq`^)6@A*6Sb+Zp(4jR<460nfW4)l#!}ArvM|3uBjuk_ZKtE2=+PGnYUbCNurEbB zk3}PGR4=IyjRD(?NtFdP>q9%xAFPu{W4q`>%~`1bN|bwtR`P&%O>B4V>3js|c&e-6 zy(qh83)Z*o{%ttf)BZ_(dxMU5GD5Bl=K=r)lV(_AFLAWdQCj?QuV1j6@kuB+<^L@s-sbpnX88$gY`R znSz^$Q38+HxQce3!+A_^KySsk(otR*uctRnt2b8ofm#+k?g*9!HI(Ky(;IILI~7p5JwQ&5qDEzYXK^*TI6|(qKk#N9g8YTJ{bxqR&;< zHf4vNas5k-tCEK7>D4vcaaK=^Z{}GaI*1yUvY7m5l_gD{fEw~g(kHUqt^NhUH-lz1jcyu@|!Xy3E7eOSd=kc8fyN^geERjrxi^+eul| z*&v*3jEie{SncvIeLnQ2YqLM&sH4nVbtFF1s%wr!Y<2$$zU=3%IA34*5$0xo&4DQ~ ze|^$cSyS&fw>ZkOUASeLWGNWhd?3q$Q|DhBL9dVDN@qwKyX2dg_b?s?+gjAYS_>a+ znwo|AP&-8(VmNA;%FY~woHiA*;Nh-fG}GNUXKMJZZP>BT5YPksxpa*BgAu+L_Z^Ya zoQ3T7r&uxbcxzQ}b?w{x`3p+1Jsrz&WSotEjOtR0PZE_}^1 zML)f#53aawH6!uwcr))3|LZz#|(;grbXV~j}ut2y4Kp56{u|* zg&no0^IVkWpToT~?hY8FK-}EtvN9{(RK>II7nh#(cd$e;QFoLD@l74^KM! zapcE!NL#!!KaOnKjkIt0+CbyLTD+w{5<68!X5u~Y^#Q{RT1}owIQ8~){w_Md@kE3b zdV?nriK)g@FWeI}jkXn^Y|}l{gv|5N?T7+Bo#R|fKRS)4Xf%X|&Bq4%obSBdeMh(e4REv;w@x z;2nNk+1ITc=dSA_ZWeB0?u%`EVS6pocJzT);MN#3iSjngBx^pM&G|9xIM~tt95tdm zuQ03Zpv&6G-5qUdA(X9T%gm=!p9=f&h_H`((a3P9>na%4ggF@Z=@kD%+>UOzxBSX{q20! z4M&IJY~TQIc1yvGXlF(AocByH+Ie-6ZQtB%vf=F{}${?_NKBNwx9e% z@!6C6JD_h?KJ%qi6gK3n%F|3A=Bz5~U~6)gh~KiDB^I0!fc5R(pi-vCwG;|1=-$h<3X|6c2%zNub*pGHz+ik~rVNa#K z$xzYVMrTOei^uNG(5A6Cql`m>4MuyBrNTPZTE4|^{lHprJSWZdWsaK@K)UFf#SmJg0Zic7dh>0)8;X~upZDb}&yZuniq?tAeJSNGul z)EhV&$(r`e{+G{eObH*|7J3G;!y6;oy0O7$&X`U8%d`=lM{~-oW!rE*{;=)W@auzo zQrOLTrCPJoc=5wDjGfc3*&7Vg-B^Wx5}g^|U0S_Zso1j^v$>h%|AqNn+n}!H1-QRx z9a2%YV!J=Pbuexu;bCP#^@0`RckzN1-;O>f=a#w`*0Au0-Tf*)3-+sc3%C1t6OFKI z4tTx6l`Cd;XpTMAMKzd7$GNwt(uTG?(^XV+!W3)QlgLvU8~Dp0?J*_MWudns_RtAD z(~)vQ6^#~*1^A|@x^hpb6!U#gu(ruqL354TPV?87&U`Aah1rC{13FTqp=^aAK<`A+ z&by+|tw2;C_biV(KpUyAZnbI^~^b7yN2&+L_b}*uwk*sGATSH4%oLO95 zW%oKNGix%#c4K!d;TY)p5IyvE^i)$%_h8-hc}v4~bx)-ky|$O2)g_jSGdLR_C;(@oms z6~{xFA!ArI-ri3sFr1m>um$P@DP8L?%sf5Gjvnzt5qmcJ$ZSS*<6Y{xM{6E# zF1ps(UU$9mJPWYF<63UmthLYQ)A-oZRkMyna|YgWWt8Dmrb4`slW|0(2LY7IGp6`?snS_=s4!iK+xp09C)J_tG+eFYnveO^a! z+2-_+e??9ipYTfexsK{mwaAOemU3p2z4Y{3rKRxo0r*kb(*&Os~JWA5w?eV2ytrVUfUx@XB&hdFOR<1Cf@j0}>2NC@qtSR@D zm@G#=!kBWJj7JP-HmbRS6QTUT!qE09U8eq`OFO&lLGNirUAF#DxCuDdk{eL) z&WZO5>=R;+0he0fQo3S!&A`wX!E%26>n_~PGxx!W-8=-hm@3ZL7W5nK*3~oqbUXTu znPN_F##%Yc7iP0C{>><%eq&A5eCRjU)MB01KHknt4@HWzYMX%haA59s2i>9Jn>RWM z@vl34r{D6C4p-URZ4vB!{iaS{VK`IffVCQ+RMT$ibL0qS-*J=U2m?yTLKQXr(f6PE zvko85W`kV8TY|CPQ{T5o>D{NuNOg+yxpxmDA*n6&o*(f=JH1UYL{x!@y8a;`@+A=2 zKIQYaNPN8S>W(a!sS$A+aH{|GDL z!&84@>eoy;8Z~d@_anrj=;ZP?etSA+U&=Qaj%>txK;y*?Q+qncU3yNl!MA#eBWz4L z7xH%0&apL@cG%~XcI=*WE;z)b9jV3&X|2C=>h>vTlrk0bBZvq4w~ zEao#)#0XQdKH=VTL@Guk8_x=oj#oWs=`x-3VE$FF*UtP1cjPB<6MI!)K634E5U7`OD`Ig4Ehr>IItIBh>PvGAt4{pWon^i~Zsmw8TvvX`39*WIwi zzoK=p`cWins^iF~s&a%*ERXl>L5?H0rDq9p^!>s^Jp++bk$a$Lg}Uzil9x`JV$x~R zr;sD*q-Yg#B&|oirgi7%y+ra#bXHsSijN~-?n7ip^m5ONouOO0estmIlP7-t1Nu7T z%ruM%HdyKKd8n-(j7K(@4e58Be*NJs*sIy#7_a+8aK$u?hMJ2WAYvAXc!9`+6g2=M z(*z<@A>q2n$G+S4s7Y_eDpqG2vb`Dc(1(<>%iY4 zy8CaZSk6;tY&Mp=$!{U7G~9{fjb6dq{Q^7}1edg>hE;rTL|T75c~i3jt(XaXlh8_c zmNfTDSEy$urMA*)(ZL*V;4(Z-_7l z_aIiP?(mhGcQQ3zb|_Mp>%xjy*r4v>tFD2c18x(U%3VaqCUtd+@=$jVWQ{^c8tP1bnC%tzUHPSm~ zPv-}S4S#jKqzRT7z9zsvbg+hxKNKt#8cX9EODl1vtx}i8qN269-_r0cTJsjsinsKHXMHYL)0$3Mn3CXOMc_{zH(2M_Q5AL?tpd`eP6@G`^#5tF~21R zwgi4oezOpwY~ag*UoD;4I8AtLH#|nWLLuz}y z#oc@ThNTbHYvXsil3N_XL-@M%Q1DQVt87D!Qn@2BAAkk-;?@Qc1B~+UrdRRKRLVT_ zt6i0f=QunRqbW!Ca&@Ki5wjG(OKYc8gi3=Cg*F8L6gm{N)|6IO)TmVt)s$9Qaeq(p zdc(kqfm2;IzO&I(OR4yZv?IEj-Uuo0S_WRVYXzQ*`;}*}NX3l!D5|vQaC|8gB#f2z zl6d})$j^=E@h;k48_&Oi{Ey@LSCQWv&+kY6t$6;|$m45bq4SqfdgsQy_^aaGoGHEt z&dKy$uc@o2v-%>wfs2qtPvJL-cS-^$V(sB4jHrTD!ysXTk-R`Is56=(6f(m9?T zRXFjFI$p)_WzB->N~do-O`<=)JoHl z*y#cLd;C^jZ_*p0ou@=g(VkZs>yLY(Ri_WmUUmAg_!jrzIUNYnTZXG<0dCA$?t+>d z@zt(tX;m1vMpdq*O8j@h()m>jmfna&y+E1h1%7XJt+bZgL%xo>T6-{S73=WT?mN%c z^=qElQCFC?#JOY+&IauEIYE5S*C^VLJAlt2W(kD4wX*%6|* z;~l{v6+1#{VVlz3if@y{7q~OHgCslZM$vuNW9r2t3szCR1xxx@ELgP#HT&a?PP*z= zyMo!}b3!+27ms8i7V;Rsc*I%0Bj`lli7%U-6}hd>^3)K0sh-p7J8MXD1=GtNI2)&^ zhJvnED=hoN3fh3Z5G~M$bhR#64$J-pix#XDzqc-2Svv(aSARlh{`g&pKG$br+cuTZ zw^XT%Vs~@&EzP8ZDUZ%X-g*NpTI*-woE#e|Z{Y5a`||Maf4Qs@ zm)#24mR{Mt-zlpl^wvOcO+xQu-zlpn^g_0!SN7iTl(i@HE`{Eu3B9-W%J!+SH!P^h z38nkAj9J$ASipeV;Sl$+dd^1l&yR#s1{J48nh$ka>sI%Eyvjo zl%aeps1{HZQR?X|!o6_<7uRKW?)5LItb%-Ixnl)tuh>4t3kp_2wSYRQX-87Z9V=0` za{H90K*1`g7Eo27^m4~4l&wNNP_PQB1ysyx z*Nori@N2>Ev-s_U->16Co5tLHDk!2T!q71>d7Z@KqaYrg5MK@I6%leND`UTmW~@+!;lq0uv`rM73DHxNvPE zH>x*RJZAoUNZ@~c;9EYDb!dzojpv=ne;Uu{BLA;=p5!m#o1#-l1Mq!ODpFr0GtznR zK0z9ZJjro-;BQ4`%}V;MS+smH{Vu}&_EE%pC_X?kS7pWIC#_y|$E=#=tJbgq{+W|z zmkk)^$45eQCeNNWY5D-jDO4GM&Q#owBl(z*eK{Ckry?cF)R+i+7v+?-Bk71IP(INf zI*W3yu=CkWHy}NQbU#ulWNcWU%}Oa#@WJXRGdj){WDR-Dj{9BCxR$3VhCCzVCc}Ly z0vcwGGsS|6CIeS|^s}CRSQ^rN=t|>uSYkeN+N2wxR8i3gO*L!r9AvC}MxREzD<#WvR@E>|YXPF}8S$ddT(G`~R_w+(3VcF%YBV zQz9S{kO)WwBmxoviGV~vA|Mfv2uK7Z0uljo0hiW@J4klp5wkn>rQ`(@f{Hf@S)Luwy!gAN32c^eo4X_%XhWhx!~8>Urt6 zQ@$0N^r4OiD1Q}H=!NM&M_&6IF`}q$9?`dj^4VtONA)GXc7YhzqluQuxtaJeSx~6k z?$t+v(ED0}%MYAI$fJs;-UOVi6P*)D(1kMHI$2*rg45e22Qv{(gi$=B8 zWe`ZZD~)Vgzr^D~Fl_{9GSlut9zYq;7vsYu`~rme+kJnDyyX5)X=kAu)}%|^jd zrvZ7ER9(gRF(>gg{Vp_&A9smHGxakQsQ2w(pva>A3(w?EJW}WZ#t!`0$%&aTm_k)j zM2=V}ZWhl=38C@5LLwIzBJeGm2W9rJkX_g-L^8U_O#yrjU5bideh8 zpC~4$#@ZDxd7Mhf^839dy)91Eekmc--Ye7F;DpQvBtz}Bs|gF_0$~g7>qN1EEA#N9 zJ^=3~Ps|+*LDQSamn2u{AA`_;P6aca7HYFz4Cf@zab{A1ejN3ZZ*Z2Nz@~S@1<4Ct z)C9YJib`$~M7nOnNKCF4M3#Pmh^0>ITTW8dN|wA*MUwOzde#;yuFbC}) z$E;6xOu^D@|BUC-aJ$xFqk3&IYNn$M6W)F%)}vYJtJW-G_H*3xCbZ0TY((Gfo!s*W zQ1cx&(%H>(X%gSWQv0FVDLPLjr5Pr^7(XdVJeiQmtVczb`4^gpnU!dsk{KnyGdF^* zWv&C(2A296BvaD4Is?B61s!86Lvbhc8nzf+wfIZ06elnq~eQ51#oZ)F_!x;;CltN87c`eh@Ka*25Ikyb8@J zW4U?+YWnCY6S%n)#g5~|_QfD)j@Mx_rHp%M`e`inE-IYN)f`9~Nj&uyv@B%?w^xC% zXdyh4Z^t8r9$>fted=q(xSrb|z^_I7F`mi0h$RGRl(Ez>*|~+=pA#Z4kw|l|2=$Dm z7L%oA-2R>rIgF>B=HVv^F_zi~olmLdx&Iat9VC$kk5G!)6047RyakPD%Ph!bxl;$5 zH$z4bq7kV>GQOlTy%>+wp@PtKABt0l2}0NNfoSS*LD=;EXhLeCAnf`_Xh7;HL8R;d zAfiYRS^7{kKDAg7IZVAohDNJd&2V z6NF8tS&}+Q5OM3Jf=JiDAnRp<$kKa=2nYh!Z=?24QGXhJ|1kvc<&#I4U1 zgr*;&l39X?Tc0fmo30Zoa|98$K2H$wzF#khxb+(Z0qaYs$@A587{Xdv-sjD*@A?=!lWp@io*IfT5!0r{4O>-S1$sY*HuDMdF`|AXiuDRYOYQ3Pc zG#Aak)D0P*!CsE$+D>OO4D6;P*WZfl&-sI?@#@)plrI!2xX~15tLnb-ARZ)BB*rTbsJHS z3Mxx?%_5y!1(l<_t|CuAE-1I|@)Pw_LFMVL5J~<_Q2Dy6g35j_sG+*+3F_=lK^5w* zl~lG%P{q3I5K&JGszi5vK$5=@)CAo%kzBMpV-&jL(_Me0vZn-9r@IKb)Tae?K+mpO z%u@Gcj3rpi*{eW2lR1S5tE+{a{%j1fB-aSi@k^nfCA$ujj=dR6AgLw0KB68xm$8BB z*<P-6}!6Xj~=5VHcNlaSclAvEOQ)24K zmx7+iG%6D|sLOF7%|lFDoTx{MnzUTpa2fq28cqEWKUnW zAGB?9+OT!9 z%z-e;GyepCD@+}YPC4eNgHWvA1h5_R-9hC0QGUDT_=WygP)Xlkv4%OG%6r{T?b_ys z*wZS#iD^0=sX>r>yPgde2nB;NUH=L`0Fex0dQwdl<=;g8joGVu^FLNxbO=YuVU%ps zSeEX%#hih>z6I4B)&0cDOTUeXC4K31rRiRD&#_Dp2K^^w@isvi@#;yPS*=o&jikl& zDS*m%Y}0>BDo3E-j>pxZ=mXPNc?FS!=4(2^f|5xf^rYH5(AURR+PiE?OBM@)UIgv> zmY5iA0_plwF@j$Avh*5gKplF+%h6wpmC&o4JE?ljaw?&BwmfDWh;~>sYK<5V)N8Sv zm=v&cLG^zem6f4R@~1&f^)R=auvd2FQze6%U(EE}*U?SIqnN&eEFY7~n1|2;r!T?f zS%!XsD5Q1DTe*4(5tC>*vb^Q+s*sa1J(szm*H=R)n zDQmP5{5oFDhQEe_`hQ7f79Uom$nk1mRN z>eZO1Z0IRn9tcn6!zMq93d$Q?JqJGuPZ3u=32v{ah+`ndy`CbDDGW}pr-);w#}%H+ zg{l5X;!k+03L=UQmhum-EYd4Bw>%f}WIzV${9}>&kZ9|2H+_(tB)JDF`otfz0 zLOz)K*9F0|NW*I(lZ_^vggP<4)7kL5!OI8le2k~Ac@KgQfJxp+hAJT_yfKoEr6$E^ zhkB0Bg8dt5V$9&i=_r9T>{s>LpTP%`{n3DGBvRr0xZ2BAvb?)dq>SN4J4$2S9o;9< zgE4#%X}B38bC8D2aievR<9lIBxf=HnFg9!@xX+;YX{2G5AexYAL@FEu+0mXvZ^7R!4I7Z)JcfiVL~jKtSaWytexRL7qd#6xj# z^(0zaaMBG5f5s3Vd=geoT9@fhuf;=M%wy{^aWJ4Dh}63-uYrZtNc2~7pIiLWYb->N z6dY$Ym>x}xNgh0g>~>QgmNwy`4C3lClo!0?CLXM9!Glj%)T<~{(BsAve-@(DdoODV zMrzE2mRPMHLMtAOw}cI+V#0DGv|o;=D4=n>JyDR(3Q3s8Ed<4wjm;;*EPn7a66E7< zf|zp7__q|M!!dBgVJBnb#q2-lCO`05YM-bj5FR!N1zwDxTpC5g_oMV$_-8GO zgfrti>HhmNbEGM4J3VRHNmX1+vgh zWqq@P4w+Pp;7dS=RiViOOzACLt{qL zR^Esb<(?GUU{0d(v8Fx_DL>h{Cx`TXiWdD1$zMgR!L&i9;|OF&@&nU*imCQhphhEI z^Ec>F?#-jv&G$erKpOi6REK~I;1bI$4H;}R*OA-L@!90|b9@e(6QW)64$+*B-sa>X zupydrM_+2r{m{4$Ni^q(zq2{Jp-HI~;Oo7PtV=YfR>WEcL#r1lW~p{4Sy~AC2BfjcsCD0EmJ%IDS05qW z%Y^QnUfusr=w2pt-v=G{BKfNzI`C!GXaH4@6tlK$ELl4a`Xi*V9_V@GGHd9Uz!%n* zQ~z%j*6fg?y@T%>-7gO8tztZJY;N78$MqepR?JbTSxEa|rBIXCtLEW^no6ODUWi8^ zQKKsdP@_Yj79out{v8dBDaK&~ryb$=FX5e7`*iFX!to2U$?=as#e+gI#^J(z8i#L! zY$6sMzp#iL{{^T|k;b0=AIDtG@=7l(&l8p}_FBF#VR@dI0ZCX6O-N$M%^N{O&IfV= zv0!=LII_GLR3*|_BYOFN9$vz7;Ge)+8J$EYLQdP`R55Qy7t*R+Ay%F-;;*Mly#|z5*myxLAsa%?wcy4U6UvF24rV0TYj;pjevX>= zg7U7x5)5j*AXePIgopsGV|ewVSD=}%z{~u3C0(641gi@VQtp>1=JP1`4&}-z_bi0@ zM9Q5)PW=T(j>d!f5OUYfLS1zgawpyK7Sh?9&0IqKYLa~z2xuQD+3HMjvGxw-%&4z5 zQf?Mnp*@A%wHq)94QF_^N-MAD1Xc=Aqy7eEY!(fOV))e$g710(0>id~^*L1k9qGDX zfS64}*F%VEkY|O62U>HGt|I0fAqVPOq~SL~b{FRB&!8^0m9nvGz+?2ZE>KDifcP3Cj#6?N zx6Qi-9jBDCMA7HiCEGtCwE!7P z$weSOfpJR7lR*3)iYO&dm0zp` zNiO!qigx7}D|b^#?28pmyW%UAZ86>Sm5TZUG{sco{1vfu@x_WkgFf+L$38F<->=L? z-uOFW(A^ilUs*}{_%|z?kT)G8#yY6eC#3JBJbgc6zAH`eb5oH2Y1(P<_0RF>$3asc zlbI{|WrEBswx%5z;>d z7_}q{>01%fe;-A95=mGQ(kIH2Od)+MLi+DQ(w0Oaed_`4`507A5{2}w29O;o->y4Z;H?A%gyP5%jl62>M&# zZ;NKf69+2Xa3K@3W~HFN1wsFzLSht2&;~&%W{Z655Dywi(DE=eXSi*H&F?@aGl)jm zhGfvZVwuHw*oF#1%k*KTvJDf2o=HJ}+i*eHGAZb9D-?u1lY;)XQG!U%q@cg8NDx_> zL(zC!u^{3>f7@vFdKiWEapnMqP9_EYZR3Rq%N$15uNH)sNkMA}f=E{+Z;i}tti7J(Gg|wrWA7XHwALwnz|JnVF<* zu^@6XDd=xoA_#XT1^sO`g2>CH+Y;MSLF8vrpw+fq5JNL5=xi71^sRJ3d*KADd=zefuQV~lY;)Xb%ILQoNp7g zUQk(@lY;)X4H>zxm!ml;=x^I7%6yuWg8sG#GRB~+OmiXVZ~IZ!co59syq(6~ri_I| znVfy8+JmAR)14Ibw`~@braLLat*_FjJ72v^Y_Z;!K^Ei^ngcNfY%$l>d;Ks7z?0 z?!+o((+LGzrs;XG)F2JA=hYVINwrxK^fzI7O2VGbtO)wkcFd9}=x;^PpC~Pfg8o(n z{fW|t{6t|?#m_7vke@*%*=xiGu!C1pTSSZtQ;qnMI?H zS}Evny`QU%AdEMmES4*OxzZF4$+OG8^3i?|S z^ru6d4aY~ax0|agp~Xn5EaGDo{TQ;8#PFb?e+>xzMQEqr=6NkZi1$u|J!Jz zMN3Bu^>kSOuBHH>mRw?9G`(oS|{fEaH5K7V=?n&RlT%?j+8C zsm4W_OkWGNd>6fE_uGQ*f!&u*GBmzvT=!p$*V_M53+T|2fo2$x+;dQezOcxh>c&+! zlyJ5e{pQW8fp#lTZJe#LePAmH!FqlfXBpV5U{Pex^IDu`Vh6yY(4P0LILj=w!Uy7h zIqxvo8(?@nlK9KL2Atms!!%6zo2ubaGJI>CO`d^a#9=+V;w+Q6<)!eRXJ4FUMvNX7 z5Q!CrNuO0{6^3!SFAVR1(d`Mt12#kGBhaYt$Ta;~QxBi%7zaYt$j+#^D%X{g4K)s9ekV9jaw0>h1z#2a@ou4X@V1 zw-=$Avn{}b*QqAlQO0~rY?aY}1L*ZFu?%cCSiQa_mWk~JtJk;0GBbLD`;Bj3gW*?^ z#Gml34V~xg0HpYRs%Zr}Dtt?9mAwOYuWyNEVl80x`j%K`b`-2$-xABp=!r4TC~zZu z+X|z9=`~EFxgSJ03zOl8RKpi!m_{?PRTd$`iP20fEQgnmW@2HST|SzLWfs*@_nRKK z!RY6`hG{exLXa~zjpm=IhFjoB(PLt(Y%18jJtmfk)so@FXeO4KZ6U*n(M&9>&?*ck zdfWq}7ZZjD5Fj)R?}xeG(TuP!#-Vu5!IKRjRXvc}d$|;i)84^4=o*N;+FQ&9l!K;e z(%Q?5Pm|dwplNdYTLt;wLqTs7_?Y;Zr;#YeyoDKQaQC%h402sHG6xJ|F!Nm{S&1>P ztNm%r565Ddh9t(kt`4Iy{~_oWq}Z4@$HqLJHP0i7q5eZCK*FDGBbLL5r>aS#E7GJa0`<76GQ!Bh+vM9;ZoH!KP~Qf zVylc^S$iE%EE79Ph7*n_7FKE6mlBRAmX*?;gsFgnCvVYrWY(Y(U2Qy9Kt#9alWV|xw1h)cttqHUao$goGX{5u_y=4P;$L1+fj z5GpD76F4q7oA#NZe+98Joodk&7x0X)HOOZ*AUCh=qVsd%){FN&#DAOVjixB2xs%j z@O0Hg-9BKwV)t1D^(e%Wxl-1=baYox<=HgW^dTMLZ<-CziZAw2iYu)a5s* zro&{I_Bvv#tb`0FpeB}q`N(hrYGRq#R5F}6xe?1Os$T*1D=@nM3d7qWh_#js2USxA zJ_QkmiH)`Pa>K;JT6?)+VqvYl+%U1sjGk8*{u7M8n=m}Ug;i%f2H~wRml)27;lG7p zISNHIEZ1FDBo1}sQ1BHBwtuT&PyhwLxq>-{4Vn#RQ!kSZpf#9w*N=L6@lB8h1#PGo zFE99S+wPXQR0cM~K&lGVky#_TAU}A%77~ihgN+5;rFjyVZfY(rMDjcPwQml(? z35^aRmzMqkm2Pn#o!;`S`!(J;7DB>h=80!9v;>$?!Y*=%3bGvPMo|z^A&MfmiV-})BjSBzRlHFXkEn?;M)5*S)DUkI z?|7gw9{CxKiO2i>J~cDDoBaN7{_i`VPxaLIsi&TLs=BJWpRTKt<6`P#6<&{0u0a&l z!`i9S`&daHzkuA&;3U0|mEXTf^a*WD9ci4z6z-?&fNehpvt|d%M&U*zn~W%T3W9sl$6735b+9}==Fq> za!{!8-;ySJ?NnJB|3mnG14q4fs``3}o(;G++_2-R*OMy64+xioa4}q=5S!N`_2c{L zRnL2Ll?rzv1Wz)Q*JlO2u2MsIe;$X39h8j<{u3n?{4ap-{~BAM8z%+)ZH7#emNqOl}}vVanm87}n`xI2iaqxT*oTZ)&}2;P;13 zAe;c36P;p?esO<#^CZwO>8o_*&)8e+t)~griGH7AVu#jm zZI4!OAIgl*eAH{owUrS3?{tdh_mtI}Gqg95*+XRuL;Ho&F z3EoByXwC5<|jJu3739Z2P&TuMfRiQr%VdvP+CQk<9~$a1UP$=aGx;oVRi4>c6$`5=#xk6#%j&kfbZ zGON&K_8R2Bz%(jn^G0Ag?9+%Tm8XZ+%vcKA#i}#zVlvE+@o}MOvb2iO`E|TSZA;L{ z9FXc_#PL(e*cnrw7nnM0$z+V1PVJh5XW&<5+|CNoMr+Z$C5Wco0-NZzjOo@X&elz;-BD=)AcA zX-J~;W&w-@3!S$&z-&3kSs?}IBgI?lu& zowpB|^3y_!FfF8`3sgF9IzpaDR61`Oz~@Mg&YOyl)sw(t8&-~`9OSHyZMtJJ zjzpK6pVgsfa#(sMhoxt7Sb8Rh{Vs|SdymY=pQUGVSh<-Tx!D`PM;CDe3h`ygSzks0{2bK8q7qdzxQV*~wHw4pT(ue^+YPRemdX#X@k*W6K+ zcaWajedK!j9|Jwwl3~RREeC(fJIk4ZIuc&GWToMq7qVgX7~vyUuQ$B&)0}zfM`VV^ zP3fHoh(|cR%eyuG06-94V|d@En-C*u*e^g>v@JOsvK;#(1j}-$=*+6aL2)laXwqAx z94s9&yC}Kn-;PY-)Y3iT!mMq5Xk=s<7iquB$ zL}K0@D$U}pGwkocYBr@FMOf$@rfs%5uLDG`2qT~^LqG2ZQ?a>ujMzeSq+*KjkE(3jBMHxFuiL6<8ust1BDS8CJ%1YNGusKr~=3|)SrQ7aI1>Cvd&TZ!bZ(V){?iPWyu zV5+wgiEYzhhPM)FU8liJZzYntUV|=gB~rRUgZbXdMx=D3M%_S2=_ZX9c*m|Z?3*=M zWVl(BzHQ&CobRNRR~YtfDnmaj$Ohb>xQ{_iw&ehejtp;enzbh27DV0_8TJpP)t;jp zZQS-AwT>ls3+v=l={F!KxS9Ul+J8EwyezFNz{(oKdm}@Bq{i;1WFCW?RTXDayI&*R z_ga<1iPTZnUaw7M2#ly~XEa_TJDP!A;!RL#e(YtlGNlj>m-!_+^CbAxW;`a;?W%k^SSz>$d>VqMzztwXC6@><1UeKBuY|RT zayICjz&#KnRx9v0Zvbro+8&mo;zp^n`o6pSFnzTGXKMx4Lk_-JXay)zal~AmqsqD0 zh$wIag0F)czz|X3H$cCJg95yK3%Uuxc?C!l1**$gI6qY3E`)wjo_{4HO0Q@%)G^9q zvwjYucEhDtG#TnxP5T7+V@esUeJ`OtSRw9&p((LWipG zgGi9y9#l^TkKaMZ@!kwomAp44_#DQSV1r~X?@(38Dz^xP`Ea^?>U8-$48lEdwe!Hi z$M|4;@Nt;qFM4xa^%`ir4P0xID}^DSfW}e))>D*+vTDiv&U(o5jbf@*J(=2?Dnbo5 zB1${{RgVW@!)EdJnQHJ(kV?@D#^8o=O#+A$S!)jhVK&@w^pUd2Lm}FTwW|#kQKfu@ znHMgoe+A912jPkrNHUbCT+XDf_z)o%Bl?AKM%85q9tM41%jMz11q1a`f`Mvo$U@qV zQAH6{jT3;F&((PGx*4=FI-f`mx8&zJCS%mu%)oQuB_QCX2iam7jh1XZSIJ!6ft0ty zHAtIyUfRv;z<+{kWRv(IqAFgfxq7?&6bO(?`8RNU1}F6q4a|u0bw9g-NwMxXUCOk7 zg$FO@{(zWe{GL6C-pqRNt04?fU2I26^Mf%cpzNGMsmP2 zohcb|HyN|89w=@UQ+CtMO!Z1h^<#{AWf7!$4nx(IRIjY=lj?Dxws3;_XJAykNUArL zRa^t&LL_nsQ@tI4i_K2!O_ebvRnI7<>`@=GquCgK4Km$j`~G6LOTuE>QS2xpi8O;&Mh2?aGRIpv-~H*_4gnI1XZt<;~bb z*@sx$D4UylkQj=ir1^2awJ}GDw1UD<P%x_|6wIm#1+!{m!K`AnoRl}KR=v~V`Y2NtVgLidDf$~*XYDq$TzM=6=3RRv!GEw)7JGg`IEE8*ltcplK%3r%OQZSc^A z_jC5T5~NmK_`WmY^BXs=fsnqF;6v=|!q1V~cP9Lq`0(Hkm5$KDd`-h0ru{iPjCb40*_#vp4J&7pU59|E-HxkOjr#?g>>l{Nf=%`@ke|27(yT7n^#TSUY%)pB&)Z}a z6o)7-`Yz%gjm5keaofsSPafX2yn^*K7RVP94Av7@8ySAydRl~3e8b9FPkS+1!FpQA zXnE^tESfI#&RI{#kg;Graq60%x1MgHCVt*}`W`45%Ue%Wq}I<{Pb`Q!!=m-%H>Dmz zSVXKRztwpaz~%d=-rKBS{TDKe_2f@;IBQ)qPtVYAxO`Xj_ZGmdH@W89 zqV?qOUo#jC5wV{9102p>*D(*w(4UKWkO14I_2hS_x%+_wF}x!dt*6-XlAU0-Xgy&! zPx=F1HiMyrtqE_z3yLnX+!yg)?~g?7EV{%hVHL(KaMatPOEt3nE6^JhU8a%a*E8&L zjeLJ6!>-UM?w^KsTy&*INxz*kuhJ-mdFntvu}WS>0t5VOD5Xb-W&P0%yIP}Ke+qHC=ZPS2?KF;s69ZPQ^5{#Q)>I*nR+3-J;{o^fAVYBjH5REfT*sMf0P^)3WW+iff+6)smE0G_g&1%AC zC5i(zazhD0NuVafgw4vZ6i~Ba!e%8J0Mufbuvv++Kw}IOHY-sr5OjgfN>m2~U0|~k z)dN8n*sMeiK+pv?D^W{~HmeDnm8ca6y1-^7YLCHYHDR+7bjDz_ny^_3rp92iny^_3 zX2f8#ny^_3X2xK%ny^_3x?-?dP1vji^JBDGvGIkVZXl!to0VY;Vz60F*sKJL3{PxU z6E-V{n`0OfGGVjQ&t~}v>Ub$_R`zveIh&PjwJc||vh|haY*y*iN@=scLGGN*T8jo& zma|#KpR-xnsg>nyR`Me?cE`!Q&H6cLUcqJ^jvmU-+pGkJpSM|wY(HOYsh*9ruBgL5Jlaq!W&`GLM6P{K8Ck| zN}9ePXZMiQRrM#n!rrH3W44R&>UJUAz$1@aVyIte;&u>jhRg0imr7YCFVKn65<6%>Dt_-k*#`0;azpes@1DV z9`prBK8A5^xS`1+#sm*9NQ*YVdpzD!@;8D)GYqg zE~KE&R*~I^I{f1tHM9p*d?m7mbL5}D2xh(*Cf>HI7w0J8&xywycg+e=4~6JML_88W zHJ?Bh5!JaWQVWqI2nsSCA{*mt1({&^H<_y-?}VJpdWKrK^TxiADrKUMQzf?{xw`pK{2H+Iz*YY!mp9n#IX%{yi8`!W zHts?2-EjY2@@(B|g>pZHto|j>kV47tLe!lSefUrF1;Z)GYPE_CkLFUf?gGWe_+_Cy zdi8+CBxU?Mg#1Yap(Cg9sIbM&22@w6+C2xL1V*u@W05?qZZhnV3JGX*!>Jxlk8PML zA+>zyb7sKb;2JTG{-N7|3dMKJIdDyjpwK;FZ=|uIK9$#t9SHaZoH1e}LI*B679oyG zu&au|;QyXMMspM8Kg2RHQgEtgf0zvoPt}n+fSpVScqhQMkcIVk7#PMJmIT97));Qp zgMYc>*j>O7PaX3#mNQxSGdo#+JFIaJ=8Dgr0fPCaiF)6Ri8_W_5^hIHbb?p)aL!^= z9T+Y~GOfR0GS>}xTe_WNTMVad1voi`efI&4f+ zXK*J+3~eq=?|cB|Yr|CzCfDBRF~na z%z;D}Z^|?YUu{ZPU`#HU*KFm3^XBzViThLN7J5bvB(m6nPTvb<7W%#NC4BFbAUpJo z4rYCup@Vg32c}ONq4O%Nj_2svRAKGICkcOGMdf3kB@c@WAoN@~d5qi(&s}i)@&SW& z@B8IP9zBda&g5)Mx?q22CR;_WWz1yD$hD4{AA?R8|1h>6KL^oyZy@rS$PQhwuQPKC z3Xv}UPSJifV5Tz<#h~Rhq-`6AqwCC>+#kYhpvk?5TI2WDFmU!UxxGJ>v%mPSCbvH# zwTOKEmZJAHx&2wGO(Lk*NDQ z>8GI`%Pl#lL0vRl1~A2-E*hczG^mS4X``D1n?<90jp*W*Oa}E(4{Ty5Xm1Do!7 zp(s0J8Q65sPwzns&;y(9O){FM2R7YX_1FalHr?;joXDCpqKn&-t02pq zeI}EN;yLQL8=P$b5x#l4r=&Ue0&KLki)U7G&{B_ox{H+a9*~TGx)}eI4=ghN>B80) zrDXim#rP-F!$Lg!FZlPr#3jZH#yrK=E%4nX!Dw(heGG0#8{A?B#+Qm!t%oixn1Q@?GIWX0r((V6p-U@o2FlQ- zl{W)r=n@-K3a1{rwDM*kh7ubMD{lq@H5pdk32I^LzfoJK+a9@U(4~u^%QGQE4_&$#y5#Vq3|+byx+Ks;moA1biS*E= zi=oROLSxo4Hd4oa3D*Rt2;|Jb;@`ucJYuQgV(5~sQ_Q_kOPn1?X)4s_a$$Oe=3Z!G znanMS(A*16mKE!EgyvpongB<};Y0hBvSRxYntP#Hr5_-Hc$ns1XigdPn1mW@p_G_= zA?(-$hT=I$r3_CPL!DuT7p3!qK{N0^Q7${{oMF`w@MxYF;mT!H?hLD$rTH%qzJg0# zh_^|6#CJ@&P}*5m^mh=Qz8)bJm`s<3!$2l1Fl>3}C|=~Dd$tuya|5{sgpVTM69CiT znF3ewCcwe)bi<{lLKNdgc!6vqbdO3-gl_2r5pq0OmNU*`fHUAZ4bB*F5_ta%3hx0= zz$sgbrzh6lg=ZiD&uBdMTkv$zd0k@-V_vUC%*)|)Ue{PH%l(5c< z{E})guO19SmqwxFb${?FUL>#Ub9t>ncom%Fbsu=9z)4=$!?PAn^2&G-Uh;Vz21O;W z=Y!>3#$jGL+hrS^QMmxTImy!+q=%E;y6 zRwpa%w;+54mpu;B8xYyp4fWO0mUSByI1|(nm_9cQE`2gUCp?_sXH-uDGbhg0s4mq~ zqem!F*L!2`wUj0de?HKpZj;k)|NZQ2f)lG3q$h*I*U%rJ*5<76Vre zy78@)y6i3oG>p9sBM#?_AV#y9Zdn=CDbwoUp$Byy09mV3rq$Uei+FHaoieS?$snwO z>#q(BC#_Cet8*(jZh=D+!7?MLgF1c#Q~cJHT1W!A3Z%LZ-kx---N#co0t4 z!f(wftc4}Wmk%dd{?3ds%XN}G$F>;%$hs}liA*I4t zQYuNKn2Jchh1+C*7<}m7b6of8<6LsFy>RbwZF`TILKZsFZP-i`>*PCWZ zK|w{G_){O4(ZTdYQ5tM0&AIzU0is#VC}#3y912#?=yeu#jM?)p3ca^6v@(s~o7N9N z?QIO5tiAmj!wgUgjo~)vyB!X{)TUhFvE1~hncb#E!+XTJxe#;_y5_s%Onx;$q3}AW z(2N#Qhd*G-H$A;gi0HpGp+}&^!&(vCg&uYhzcM`;s6-0G;Z)CWAZ8#xvLa|CgZ_jF zFTxGvcUTZ7lIYdoK?yhV1Qyr>W{kuUAPj>m6qofD<)8s?#($x>LVJMv6<6dlPzuG> z2@#Xv@XOB47Z-bT^&2z#5I9F|%Tc9wV@ZZjZ9!E-8{kt&zQgd{0}KlQK~8bb+V5po%LE@7l$0Jp<)t3-IPFz zFzCI1yqo>+-`2kOZ#qOwg~PA+{@q1MH=0G*{q(=zzyB_gzB^jE^O zf{CT_r7yRPHACHOhCX5>mj2n`ITP;xT>87H;dYchJ-ww*^j}K92Xe2X4*ZpIvb8$T z49D?c#*IMfcjBE_m;QO?Ko;~bz=Ar_52h07`R9W)i7u^A{+-datnzEnI4HZgRl)w9#c^m2ZhXr zccHUahnN-?4EAQ$;d=eTt%d+2`U)t8Tm3v_{Q(ZY-uEfaT~bR;iw7dq67*)4=)Yw4 zBZ%9hMaoNp(ZJ4A9@tb^9*da9#r&2%$sXnJD*Oe$q@qrt7#4v=zp?yXmGcG4M$7sR z;$&*nmrC9pB=;-8S#6-Y5oRDhjWzI+fQ|8B-a9 zR2t!=ILT*jXCb_)tWP09>JF*)UCe*J?vNrv`_-Lph&d1rzv?1z6hlDnbrKK9-MpD6 zE00x`KX&b^=daXEqB4oG}k026<8`cRW;Kr z?gSoB8m(gxCsO1IAS{QIH@s=O(p(CBA)J)y1yG_;reUm#!uAa)3E1IxPwk>~=21;6{oKnb7-v;W_;P@?E`Wk?@;dzsQcsv?vnu!uUVj5ov_Xx-^f#DCWJW;^%CY(zodaG(?z- zsFK&A+{;&Uy;Vvz5!69+Eu55TT9@f8;ChtwPnt+{ygqDa03RTqh+npq@0NZ<;%MrzIkKJJr1 zukqBhmfhSdg*K1!hBWdwO98(!fx;evpx?l?upf9;`nh)j-+>#GH=3=@ER^>x3^(>x z)X-KIN8gGXxdN0z>3$6fU%}xwrtq?V7X|IHo`;}X`g>~{QK4`d=|7)mahNbgG-L2X z8`XKw7p&3r5o(pXMY7w2LT&|v>RFB|ud{ngyHqE{uZ5HBZkO!N0^A2qTIea@AH{Zc zKSbEP;DLeH*r+tC6Tr3{PMX!l@LWj1W_2q(x4`8d1{5Q;z0@eC3S&7gqq;>o#ZOq{ zSlrz;>~&lDg-&%h@e5t6^gG?1Us6)r{wSu<9T;=_J7SFIw=|$OesvkoZ)gBh`4r(Z zDs*R`XH~|Dm(kC%73z@v_=Te!3 zN>gQP;$*}Oua*G=tBpg7CWjPRVS44rdP9?=hm&qZWVadX72E%WyW|cwlUIV(%1wuu zxER5am0FstL$o+PDC-a{jt{nRL=WQ|6&7uw2^`S_YTC(C>evB^PLPwxckKx@4L})9 z(_jK8V>l%mjOSzwC!vAS!QB~f>~|%zZ}HopX8uMsGp1(#MolF!I?hHiQCC%PIKkp~ zt?{@YAheUv0B=+k(`$5ALu~Y`q{UB~;}Gyw1L@<){#A#tOnwN(7yU>V!X0E|;76_W zqT0P{;zI>1;U%bl4oIqppF^gwqN%Pa?9D)I}Y${hB(7Ep@ zcPbZ5(7AtB&Q#9Xr*q#1VBMy3-&fAGMrchJnHZazAD?tFJ~`dJCUV^I#~r@>NPO!B zvu!+slE@7tc0m;mByL0ie_xG5O~?%-{5m)N3ZYdM}JYD)sH?#|5B`1GWzFZ^iNWg z(LcZc>PPSYef6VvU^S<)>xGX#7CwFlBqM=77CtVAdV1lb-*4fg-+$qwcSs#~cGkm* zK86!{w2~Hr;Y2Nj!-+nI6Bk15+`>n1QFbz;$<$pR!-aT|reGDg35j~vf_h0xJYfAkFVR|?**6MKVPX-5L zZ5jI6S@>x8S@>x8UHBNr=pk>tj2?#jEqo03Tlg66x9~CC zZ{cIO-@?amzlD$CehVMN{T4ok`z?G7_gnZF?ziwU+;8DyxZlFZaKD9*;eHDr!+jS% zhWjpj4EJ657{=(~9Vkp$_!#cH@G;zX;bXYp!pCsGg^%IB3m?P(w(v2G(ZjE$l(GB+ zqlfgff~>>+Dfa!dEjHsgCz1M_)2uad4(0n>(yRw@P9pWs(e1U^_8+y5CO0RM`g-AG zoRdg>z3?&4(L-M^e2jAvssBcX{78)p9|KMz^)Y&Q5@>q#(8uWEJwP&g=wtMdK#v~! z7(FD?qlZ35508Py96e;4bZnjhTJkGgatj}2B$12JudrCq^qTgZ~|HAr?hm0cc>W3quVx<8Sjl`xbzYAaoC0)l~rB!}Bei zG4MVhe2;Y$FDMm5HJN2)c;l+#3W+2>Dwn`jJqa)jo}qBYK<+efBO~#G3>a#R=}%%L zoF`>W0#Cc}T?cLNB_A(fGgQ0ja$;=u=Lj(dRzvFj4CjTpQ4`I`X$Y@;5QI+LgI-LAyEB z5Wnirkig^MP<2dTni;CzA1tI;GbkLZzN#1JD4{M2$Eq(8PM!!aC}Sgxk6I#p83Z4l z6K)J-5j+hRlNY9^_Aw)e!{P%X6r@zh(Y{1*@+t$8Wgy$3GKR)C)r3Kf$=NgD~~d>EH*Vg713j)Gjx=sov( z`2tz3YFQvJK*%mQT_9_8fk-wV2dmD^W?eGFP~Jn~ba8D~70lsYAhg3(tUy{!$K#Hr z>OAF~8^!%TA7BlFSHWdB0q91T0i)st8sHPL^6w!t8{Iiz+ze+_oCP$Nd^;%c8fCo$ zp6unI){!!tTl8ED&a267RIm+V_B2jACHoC3WJ3QzU7+ap$A0KwhO?chpQ<7*Ta!CL zs>MtG4jcx5pMHo?IlOO(wG6T34%~}me-0=8&{Oa{Nj)mq4%RY4mJyy%)N-lu&>qCFYtb25u-f$^fyB8)ZBR!eel{ zMl9@8BX%LB3WZ|dj>Oe=c<}AY+5&18r;r&{O}KoCn0w%=h5%eK3VlCZ6^{Edo`&a9 zxH?80&dun8(Reh&)$a?i8_q34$bRq)2elS%B)c7^7UFhs>SOnLABjn)J8}|&?xiC9 z6s$gv@auim5%4cW#6#h#4gy#W&q}!LegHPcqTEM$Qtc*RCnb6+el8hr0priXbQhe|kSF1J0?ru4jc?vy zS^&el0bT?7k8tt~V~E9zq+6>Zf6S-*Iaof0lRh-ufDQr9s9_%};_@;TB1u15iSRUB z<+~84>y%M_SFvFbo857!J4#vE-+)r}A}&+G+X+|o6u?pNEQYHi-*E2sa}LlMaHD-t zK86UzCa>$40k>4n)OLn_u$NsAAQMt*Iwj%w!JHrj4s$iYdg0V9fr)s?jP zW~N!X#t2I*=fsCOC%%<);=`O1-^w}hVSe*!<(&91zj=-4H?Ii4dGYZr-@JgD?m#L= zJHK4n_;STkviPi!FIS+58WZ`M3t*}w*nSaIu|L%2xWyJRVVtInV)z7(ncE*JUE-7S zpMk82u}QxH8=W=5ne-e|q_f(cNl$@BXN`A^XVI_N2EXRzJ_)@GFFy(0#XbI_gH5SmSUpXQvmDPk-t$^(I=T-tWr7D=bzMR2>Zl z9CNnpxYgwQGg1w`h7aJ@VXAyHf~c+?>Fd5$82yP8!5nvnE zfkd}ijON?cN7VHXTBrBH97-kd;5Mum`FcLQWDWc=8_CBbqn+K9d>cTd!R67fhIbV~ zV}zyT7}f%)<{X~vVf>d6;v7}FoqlcxQ&NWj+Z)L=It4i1zZf!EfbZSU{Ou(m?wv|7 zML^Qy_?SCYK+2m)ji(71;Bf|~J6%B5C~1dmN4c||7pTJl)S)X%zu^rcnBx*`Ow=;g zyeM;EsKl|<=s-cXGMv*Cp6+0id<`^89)TuXmAlv(ig#OGkVllzkAqgI%91d1X?u@R z%u()0gg72GagVNIS!_muQJymqA#^#xMfMbcWb6wlT5Qnb zA%s0g>~JKx)P2CY4;_E7aE0Oix;042 z|9xx)K%Ms@1?+au0jLiau32oj&x_~=0I2zbsM!htJztb`+PxIhc_rA2IGx^|yeY2+ z+W@8n3y+0>*F?Znd>%%~n-XWHHx6&F?pslIHQ41V3-ITX=TR8*y^pER2mV&v)oyPY z!G}fcN)~u~6MQ7KWQkGA+r=1Va$zRR*s#k;M=|p};_@dEqt145mL>C@8c6|1o1%h| zT1~Z&HN9^TWT(E+C`sg`p4F&|$WNWac*mIzMaNS|vWSj1oo`U2$<)mZTd8F>q%I*^ zWtIha1h%A};(c19!&*~43|nVLYY^6+n#{11OlixVsneL=$(nO&>OGd!1|2pd^(i?| zF_U$OH#7AOIX7y~u2c`vCevlk=cksC^F}lBGBoQ>v&OsI%>*m?3~VAswQn^`Sczw< z^wmt`XQtlMOZ8Kc!Jx#14dVqS}E3eFsg!YOj6DrW*7z^RwW zI8Buh7^xE|WV%M-)FCXly;TVxsy+_u!ry*i%c)wi+db2iW4uzU$#}Mz+6)dO#nYeN z&1MO2l$~m!$Sr1k5pI-|+QqPQ%yM?awW+rlcCJ~>;+c_JNmb4>>yHEKO1(wS^UVy8 zoa#=U!muBkWo#WwQylGdFVJX3>T62bs?qw?#f*2M8R!0J8&h=*yU2`h1lpWBnqe2~ z^tPq$IEGPW5+hFg>QI)AmuzB~r?WUyqCw*`!plp-SZh zp&b4qn-AV9wwQcH?hrB;zB{Z(2f!nOOe1y*N~_zeaMJiz;hl#xy~=16A64D}Xya8y z!vLnY7efX{_Xn`O7(=q&!2phzKyJP2=x~6jw}@;t(Xjx&S3)pIq!oGV$W|LIK}g)Y z1c`dX6WdXsDeo4tjSx`h%|gDsdI7E8z6==|yau)z-W!ZF%I8bfOfSQb2A4{AVSz8y z9R{!DNf`rYBQ14+yA`YhM7iBq)AhAj6o4(SM3P@0}-J;BG%4rME**`Ulb_!LDXQLMyq}mFj^W6<#5K3 z^Psl6C&JxyJ_9@I0t952QN!Cs9}w`GNh;5ut;T1a+XwJj(SYdna5YCk!P^Jrl*w`t zhuT(%sdF+b7js($V;+E5bKr&+F_mpKI%>@-?3=J{ELpKIw#a5B$VRbpL2YsOC~AQl z2&Z~}oRhYtiqcMkq_vb565hdiMfeC-rxp~^S$HZJ^;A*hT15LvL6K8)iWqf#uB%lN zmEVAdAB)k_xU)+|{@{xO2)86()v)WITFU7J(u;0%jEt95WSIkj2`kLp$H;Blf> zJvHD@-;$9l06&BZ@4<~a2|&#mbr%2Tj=G3{7>r4v2yNqNRIi*xcU4wfbPndQ@OS54 zXnTED3y1$uJ`rTQ9+yzg4AbUXNWZp+`j~&%^Y}>eY4b1C|Mcop8KOUE?$$S5Pn-ZWWK z>~NKVH$B7EfS81V6882Mz)hbR+sf_G#5BD$7d7DVkjGW`|a!wXCdK*+3o z$l81eh)3s%m*$DZWG`OM>_qJi`QSSX!MTv1FoY>TUkE8^oR589p69CqPeI{454Z6W zeap~2>rAm;iw_EXi{Oynzr{KWJ!?@%^=L3g9)Z8LXg}vY0Ov{u%uYwCv(ti`Q#3C< zfC0`Q5xwZ3G+)Pj=U;q#IwV6C;trQD7cCT!bbQq6q9X*PoUaLfn3lT{TYQ{Rbe#CJ z&O_++idG6&ZH)a#(K-QjvDL?~Fp5qVQ14s=HHuD6Q?mvqK#MLqT`B>V=XOzp>G97I z6t%y?wa7#zDdq(PE{V1Vl4Z*P@&AsJek}fAF|;bi5$!bdN(dIkZ^AqwbV-R3%+Rf> z`fLCr!4nPQIHLVrd~LN8oIQc#ysIB)(Il=WCyr<*iY9qPd;C|b=0b4#2_DfN-=S)5 z0UD5a+y}xD?L>76sZ=4h5$#Vx z%KQY6XpeuTYMue=PWY6GBih-N7Z^bm)Go#m?G#!XQAgsh7)P{IPMI9hF8eI0vcuV$ z%f4rUmz@D7yX;hyb;t;KM0+ugXrG7wyZ{`)&WxAs#rjny2e6m@9U4Z~EQ-7a6RP`6a@lSH3P~+)zTGIQ-h1CSQ z90;CNHW=n^1&^}`N(948_b|+9!6m2{U^|@%4n_)aoH~?iFiL>$41@|ngMhfh1K5Km z0ZHc#f@T3JhX=3+Edp`}um@wTDk_%^#=AwRb`JMr4JHVS;f!SJ69w1~4`2@_32>YV z4Cxf$JKWGAm@FWd`V;|4=Y6I=RY1!5j$pcg+yU&t-d2xJeWrUJQ|EIim?bQ^)MpE@ zoyQo`B_NmjTmio0uvF#=$fbUOfLz`W6p%~(AOX1p*n@71dnu=R0DEw#dnfYe@BsGU zFyS(s{cvA`g#v76GwaYI0gl5XZG*!F_|9D{{KW#|4i8`tju4P^cmR8_L_o^n0qnt% z0tPrdfIT=`K-Q_D=w$+G9fKMDp@2H)R!TcoK)u5Q*n{H)G&nqfJvd%Ki&MiJutGp< zj0dm>CkSYFK1GWQR+c^t?#|3-$ZfDnLNKvr22)*A$`=D$Wq1I4u&(q4GOElW6u4e< z+L^B?ojNt5)1<9B_w~F;{bLk+WNT!m}wn#MW zWF2CQI7g7}WOx93aIPT7$y`9r^91=$h6k_*=L?EEnRP7j3j`&d%n3vn3Q9SdF3P-E z&;TdX#yfqfpsbVO0qntLf@+=2QgU7{sLsjk$FM5})jJs;z#jZWP=k}<0qjAKpcW_d z7|}I?TAd6JU=OYp)b3VZWLu@(v1dlwp5DB0IPQnHy0N=XsK(tqaWG!MPyUn@+ za?1TU4q)fOC<#2Vah#@p*cu5Qz#b24WG8q4dpx3%gF1;D7smnYS0P@Qk;~(89Kc?S zhLpGnts?yodBsMUcIDnn+ z6h?x3R>pAv`>CiLc7g}6$8i8Vk(1y7>~S2xPUI(evUdC^{eBZq@BsEW4q$&ALP`=m zfIW@_*qJ~w!LMQQb*iQcD3usQ{c!+0IR_+;r4$^%PK~p8{;|lSHZvOV0QUHq%HlrG z;abE+q2d7c6RGrGOaKS4^QmnlcmR7G2e9+WYbT~sX&k`L$F-B-0qk)cz|L1R6zbhf zZ=15lLQ0sTqV`$1de`AXC)I-ous4!Q(}ZtxLNyJKPinrG3FZOp{9+KbU&FO>eHN0( znR0tdeIU4?&1rVUx%194c-D7Yz<8+v8YUTjGLqZfN z5^;S#;$B90DI%BcAj`5uUWBz&piMt0ByQcP9B4j^P<+H7{vp(I7r z0QLqU>mMX-Bgt1;w$6}sBla*H)IidJd_;$`?j)%?&$SC&wSU2_P+Js=_zS|X2rpLU zA?stn!PA(rQ&jAXiu^R4dlLAB$tnwSE$eI%s&y1@XckLT$Y;qZA)Ayn0<3i{Icg{l z#Dj6`X5=W56ev8%pl(5q;$}hNK?Ze4m=s4z0X2DLd9s}Y(w~bM92DGh}PwFgc`zfS^^;p;70usJdZ$z zF?uVat40-5gOd12AcfkfYRKHco(DnAG^i@sB2<&gut^w=Uo*YGN9C~uj2njJRzN~i zrG#W32jym@bPZfH{PO*={ z{t%EJ6Aoo)u6F9V=jIuxYxUT3a}8Lqm8P%^$+N_T<;R-ETJu|Fw4IaVk=xLU)+8)f zYL<}o(-FKe<9AZOhhweC*Gy5tz+GlI znZ>8?GeA$k)qFvDk3_?WXLdhojqEr)ns}_<(kmQcOe`EoIPqpAX7WJ7i64ST4j-Jz zhp`-pF_F`oY))^Q$lGRfdecM~p>nj~L{4wAIlXCgVUy0&kVr#*lTN8olUQ*vrtB<{ zB4cGA_AihQL0)FT4dZ^%JK=c%ZYY^Cxy3QK0-semoXW9OjDgh%Cy|@s84cG#w$4&W z7|MQuG0poKvsH!m#Z)Ni8%VWxfLiDqND1!(g?)pxRZ<)wL(4+{Fds?Gh2w81JAzuU zskfNG6cxIi39vIDHMEBbSD1A~Q zyhsB1-uZF}zcil!OTQf)YBLj67B}g67wogF7Jn zr}+e!gGuCgiwVqCp>vo3b3m%ShY93!KuY*CCXlZ`q&Pxm0{I*~4&jgH5@==e-$CYe z{cg^PeL?2_Fn#XVH=obDj z*M4FW;Cw{+Ur)Zwop$-Ztnsw;8qd&=5H)P~XaO7-30*3bbA*DfiNPdAm~+({ei{Y# z0L+Sb|&K>0I@`c*hrI@Z?FcYua78jQhk(R$=ijSt8dwpQSk>e^_MqzO3^ zJNs+BkFp@O$Eeh~h_wmM$1174J<9qc(9>|S8Ks!ZtPpermb?^ExwdgwAp#d$gdo0I z7cD(kq&K?mi&DVT;m|OZ9#M0SA z9AhG^8{2rbrckL@!OKd`7fJr=@YK#%YHna@{a@~kZ2xaNBU`sXOS>(75(MXVNyhBe z!i-nD-_FQ(pP^06=alBcQO!_o@;Jnok<5_Z4LL*`D9lTQm$%^b_6Hh&mdBpq4hWy9 z+(U4eqm!r1HNq$BJyvmOF7h4TppotEM|6rtPIMj^f24sQWvo**h+3XL#xk6{Be>h`uUtWu}2!0RHa(qW6dm&iF#qb1(O49n^imU<5{ ztXhYyh)&_1tI=q+*GA0-Y0mZDTjZ?OVH>^2hz4tPhT*R#9Aa8a6&r?3^NrcpL+jjz z4Uyl$zqggEX4ou#9Q;MQpo&^hPI}0?0^McNF{lFSir_79hphGWSl?4d%9XJO;n_~g zV5p^fa+I~86#tHt4Q7xp1ss%UZbvmzf#s);2j%w&dJHbL z7+_Evx-PiX0RYS4ITS8U{>%nYJ_dRXuHtNf%}{y3Lg-utkJ?R{t!AilD}wd_oP)18 znF|3991o@7wh0V?Oeq z;_QwLVkY6z49N@yWe(76IOxIxWGyHe0uPlc>4JyVq2#>0hhjxvJhyFr=&Y_;z$#W{ zN)1+=n=wc#NR>JA0Z6Tticw{Duwo373Q=XwV#r`vcUa}j$~xo@G&r@=R6_@2Qj_X5 zQ*2;^*EVr^toWg}%XE%ZQX_9f{pkja>bVqFb7@8g4W`(Y$jlPBx>d&^fh$z(dJta( z;TgDrtSI*)vf>3A8EUNxRdM|56%gcNl6r^QdlZPN5cmhQqpaYJlDoho1;f}0ySQDq#RVa+I^3PE*p4tUw}CKYHq4B)L_S z6;;;KTh$1OS3j#3;Z?bCYN*4jbokJboGqY+>Qn~f)vQhhEMi!fS5xx0Vb$;@Z|CbV z)o>|2&&O2b9n8|>v2A!;f(?NC#)j9#hD4Idi>S9VWHc91%)?2G4XS1B;l~0Dp=x6! z;}5r@m%w+BgUR*F6ay@_9Uc11Q4AgTIFtL8l#tQD+zk0W?IkKi(aoEyO@7gRQ> z>%yF#h{g<;A@f9d#t|^m@$ei2Cr$MXM&<=tz(`Q7ujbXc04(Rj86D3e(j{PF8|N(4 z4A1_$89~>>O<>hxsN%(>lmk?m^wYmZ@J=}Cr$2?~uW)GkOpXPO0&0ZLbP+ta9>Par z+xP?STFxpw6#;yK{zvWwdeU4}mfWkXHV$pRN1J~r?|`h_s;$E6E;~Tf+-foVT`*Yq z2v5F#tHo@-M_X+&6*jkW%;vzJH7j+A2$gqgdmQp^%}(E<)8TYMn_GxkN6SJf`;;R1 zm!$_I*nXdvRN@f)g?7@Ek^kx27urc)Vxi`}DbB%56FnZcF|g z0hoPOhq|=t6pX?9dU$(|^I@13`W|`pVOmiwwMw)qg zhJJlErL(sHPH_ByG0%)6nf+@V;r|PNn|pDXVM64@GGW5N$ocRWIqo;g-34DUY!~-C6)-9QUXI=GHL{KP zm^x@-nAY`-ve|{Ta=M+10&7L01hiIMSSv(!Kx@T?wL;_+(^_$1tq}PEtrZv63Q;_u zwc^5BAxZ|cR$N#sM5%z*iVJIn`5O?>T5(~mFf1F;T5(~m5Y+~>R$N#sOQB(1Kx@T? zwZgE5fYyo&YlWyKpta({S|Mr;Xsx)gR+vJ2Kx@T?wZgE@fYyo&YlUcPKx@T?wZar; z1hiIMSSt+c3TUmkuvUoX2eejPSSv)`0j(7m)(TTt5YSq2VXZK1Q9x_Og|$MoB%rn8 z!dhVpO9NUfF02)Xt>_+7;#!FF07Ro z!CJJkt?4*oziO``?DCq&aVS`?H}=H41D39>{pG+G*PKU;SnjgAeD zU<&(cv?AEbu>CYz9sGr8rbg?72(_N2(S~3s(f%534E86QtWqu6hYC=S$UM6ur%Q4*-hh+@Aj zq7+cG5ygI6L<4|Yj41ZoBFX}dF{0RSi>MX|x?sO8qBVcpO_S+(A0D>;q zZ;Pmzv8q6rhce*I{ z+hWMfVw~0!#eQ1^UBxSr68767m|x68Tcg--i>Mn2DPg}Yq6Nh`ttX28wg?s(C0RV} z>?rozI!He}X$rH@PvnsMV%|}s^bMGTt(tNlix!+dgoHU;h zM#7FfYCVDINe7jF^qocnf;ANRy++yMDf`biJj0a4Ym2+)O))&BQC;!mxsYjUR9`$7&o$4| zsG)cYk!_~=9Bc`y>mW0v!=@HbWmrO^8NnB5(_T`enZ>gaRH{K&a3>Wj(`bIMi>Ta8 zZ-&TjOjBZJ)28(t%Ffgi;-fb2X{qfW5^BV|sKf6o{(|(&EI9XXi0&UMz{Fkhf8fss zPuW3+e{h=7%4Qh;VUZYLX2XnKTIbKf@>axr3n}^YbosmYpm_ZG0&MpHh8!Tkakmp3 zD8P62L09h|Bp~k0Cg>KBbUl=Xzc9qaQ||GUwn#z-xbIWI;R3SmCWb5)P;1zWiH@-5 zBa;0!3M}?y33ayL13mV%_76b@W3Q&-sF+ckikJTbl!(n&m7;0kd9(#C1I0*R1QnDK zxhs7KQZ(rq0tyQ3uJj`iIApwgmr?!v~Z zczd)cB61QaVYSA2A2Hlbc*j<2V>=n*K0vTefO2^@k6JIlbVpYJoG8F@UuDQi0&MqL zm`dto0U^VG4r0E6T>bVXSI-DBrA(HJY%XJ{R-|Gv^jFWSFwC- z+^48fz1*j$R^hJr3yPOJRxzp=p*;%oR0h3VBjlQY;2wo1K|8qG6-aZnYU5oR0X!6L zaCSTx*Q&S;;{G7agd4KraIAS-r=(1d1mSQvW9Tg2(nTuzF=XMhW9W1yWvNn1T>`59hQVmVAuXhe*d{1#1p;XS{in21FR`M05Ke(( zCb(4(XZVd?wgh8dABb7B5vra9HyR>8XkzG{K(~^Db#@<2WZ}F4^hY>U2s9*P)Ho{k zau}m`9BzQnXA44K7NMa|d@G~~2z|Lef}(5zWfUBRzC4aXj{sT-XEbmJeDz?Ix#5pT zeXE{tiec|Q%#cTm0#MHC#Ft#S#__xrj|uI1;A`NTRxihQp2RsQC-cHZjcXWdmz2&+ z2>mTwGsW>|uIJauouLnbTT1ab+B_Mu;aXgPYvK6`+!%6=0Cdsh1>4bAv6nznEL|mM$n6l$!)Ecg1&@d zBN_fvJS+>IocCmK_0rm2W$w1d)DJ)&FNLwSjs-MC?bWOS7mI%) zQ~!V1d-pgwimHFOYr3bWd!}dhzS-GqHoM6t+1&P)O|rR=Y%YW(BxG{|0t5&lK#*HV zB18!gAY#O*AORv-NkBvp#J~>~MKLIsh#=u2f*?;oI;T#Zx}Q2#U43fmTq15LXL}mcrd~nlHVvVDS%=eWn8d|degmqQ`jk%r ztp773wB1Wtur|#U`Bwv{dUt_HvPi2qM$7q(f0E5`-9KJMc=rzkJA^SWLxC@an;}M< zPLamE3-FzA5k8l(soRCmWs*({*w5T>Dhk@(PCx824;Gqf4>Ejl_gO4>`yxWmgV!T) zC=~UImOrteb;7Jon4K3gt80rgtCKztW-Vl0cNm#1pM}L4oH3g+hJOh-)qC(+7~$J7 zxHQP-n{z%;s%OqtJ{B8?JO(awb2lMxbS2n9Iu`Vnm@4#laJok79h|iTgu8M7QAi^4 zxhJ-6xOw+sm&4+n(R-*{OMl(j&#O*s%WTp-WLw7HAM!U>)*>XyTDJM)zkdqeJXyp! zm8#8s4<7f>C5GxKyBc0B3E&?me#>3506?bzjthNEjTgU{zKP8;L4Z;mVI?%tybIAY zm!sTjO7%A}Uis%v2x4B)or-{ydxhG@R<)iehHhHxS!&J1`HUW$uT|p2fK5 zjbm>A4)|?2NG8-hs=EZDw!K8ZDKAmDMiGr;c;_(2`UvlrL*DC0(cd^5uYBM%@AZ?& zdokdJaKd}p2k}l>J3(7|8B;h-Udi8uj)Go+5|r&{nNE~+F*b<#4lzgBJ3s@?ftWXl zStK-in3Xk=*9^fR3^e5w#dOIt>~hL_y5uKpgNE!ZW5pOY<<~3}lveM!MYxfy@OQ^C zg%0*nM#GaIa&ScU0F!-layzV2TY?o7QaK>37{Y9E(D(@mj)Au zxm!Rv+?@*l2$x?>G`sbn9O?c8zoXpqff?=AActez2jL&dr#k1@1+lS?JOZZIAmHQeSLXXCgRtySX1to3?V+)yn<8iAH)Cl-3Sa{xbkL z?0}6p>np1K7l0ZvyTRAGR)=lQ+y_4R(p$oIdnbdp)$7Mo9(VApulf;{w~sNe^J6NH zyLPzbT}M@31Jm5?$4z+Lo3pN0kq!}3kjdzhKH zQI)foL!#vPWY$YiaYo8WvP|+$iOd8!K*N6u;E6`=7SNKtg#Xz{GE5@z(**fU`;vrF z_D|;W>&p5whEVx7>u%*PNur9rK=cn(`Om?`$dn;n>mHpSEAuFe{X-2onQDgpNJC!5 zQY>t(do@&A!6mTuV+~bSES-xo?^WfikU&l538rwL4r|H`B=i#vHD_q$()y`}#$@W4 z!u=X*%d`=CKtr9GM;Y%y4RvKkGiMKJXlg~z9HhQam2X0N-4(M5{Y;l=Zp93&N3Dl7 zv>uL zvvUKaGVD{}PZA*^R|Fy07QcugL`cZ3L5OA8I~hWRwAcfMkg{nk<72S>j8>Gumx4ev z;0o)@rrVlAlZ}N!X>B)k7s`yoI>EY1Lsn)754T>eAty72Vb^HL%RI=i9U3alTr49+ zLzS7S8yRo6 z4r|N2!_=?WP-kX3&@CETmRZVp-_+1@qii6Hd#h=(g3D{H5lNTdS?c_cOiKQZ46OiL zqmok3R%SoD&S(uenKvM}tT7t$GFP&zj!jB^!NwV(77bMzWxFA_tk$GC6)BXLT3009 zKQOI_$?rkxK?tAey><=48}PYa(mGU;oQ=FQ+z`849KF^^KtY8MmG zx;Dc+TFIFy0JypQ7!6nz>u_=T3Jo|F>u_)Ru^R9|hM?m#RGPUH%+2F9R9OLw0p>~# z)MUz04)X*J)m5xVQYUJlp<+FfTCIVmiuK@kk_MV9)+4EtH87@PJ!~SJqJg%G^|-Qp zjRra^*5iKkQ#H_4u^v~Je?kLOE7s%6^3ycXU9lclmY=SH*+>*wI735o0U-}(YG?r< zWa2Ch^#DRH)@o=OAY|ig4J`+Re5}*Zip+SHalM9CW)`9u%ncfXLAMPM-KtaB&5gmw zA2fws-3Aq_9-aaCjK{#Chdi<8%!bASH(&rbs!96n#10^~!_`z~F^7VS$sY?c1txO6 z{L#Qp148jfuqUYO8)(>6TlTzX&|FfW1I0dho~FCflp za+*s{Ta;OUy^pb0G7e654<;AV*!-~&i&u9Er@w*AJDQWxV7-eD+&BcoRQ=sP06lmv zNLB9x8R$EgfGJUl*RViR|1|l9T%LT1wD@C_@9z_)hY;aiaxkY z^49=Z^|Bh8)~?6q+4&w{cK-}?KZP5{u0pZ|e}wn><<%gTp7IVT{)Zfgu&4Y26lxEv z@u4L52&I$c=0J&pGlsK4pCYGyB>8z#+LH*2_Zf!mNsKK&h!nPGJ6N=m4gN;_VJt#k z#b+lY_<0Jx9E6XbrB3+GNv=d~1wAm1))#b)g}kd$kRp%G?`(ewu}aFo-Nfg>R42uwMhfU=v$DX#QTZCq%)iOElh%@ zepx9a`7I2f4$c45V3Mdp80&T9N~8v;MZ(5(Ad`uAG!yo$u`{t=OZ-g|G+LfO_f1@c zPi+QZFM!VZopKDm@3)K|7Edq7UWRVL<=7645&nJkctq0{o8reV`Zvf$ZY=CYt5YO-dhLkM{&Q zXal=k+ZiV?*PMD5sU|J|rJ8*s!ep%tD_!g{_8ua9D_v*O&&?lL=GqE>>QN#-gYN5& z7(~CrpTEPo%(ybrTVVhY<~h-LQ1#O|pz$p5hQ|XMX&lfXWO*(j9MB--WOzU$jRP8l zJdX!7(m0?&sMO;DjWiBu5UTY4#V{PuAXMXV)t1Hq4Q8RvL;D(O9ME7`gU16JX&lfX z)a3Di222T-{T@)W#{(K^9ME7`o5uqhX&lfX)amhnMj8h+2zANwE{y{kOkt|W0~%=@ z&|p}%#{(K^9MB*%Tb6oh9ME72b3Go=NaKJ8!+Ja(&`9He2BBph4``%uK!eb7Sp=qW zK!Yi)@OVH2_75d&rN;vrX&lfX%^nYEq;Wul&=w=h0~%=@&|nKX#66Jkn=)slXqgKZXMxRGKa^i~ z4gbAjmgc20ur>82s7eT|^k8esMeq0cExZR?Q}a2;c>J>6gRQCavut|Wn(|<4>OKTH z9$$reur);}ouREM54NT*0j1~B)|9tb4fzV7I*+!dJlL8d)a*4g9&AnRMGBqXRp_7| zY)w54Y?pT#!(eM_9C%LkXlu%Yt*M0&-Q6B-O?j|2bql!6_GoL$gRLp1zQE(@Q4hAJ zSjfXX+M4oUYl_e!kG7^f*qUOAdNQ;%<-yj}_Yr@Yw*_VQU~7uwf4S$8Cu~hUig+uG zENx9?U~7tOVEJMt9vRr0BA>E55YZ^(=|5F=0*a6*6I)YdTY$F8E>sw$M)tSJcLuhm zsu3e#kXJd{jVt5Z2&3#YlnHO{kuJ=0-UvaJ8Klf3Bjhkm#d<26364gYn5HTd(^O@D zg))*U6Vp^>Vw$Qfjmoo)?9-skz%l4ZG9IodOOXO;csMbqg8ZJxDSG(^UKMw2UlGQ)OV9suGko z$+I*~m4RuhqXFcsmG~_`4L=q90fIAsmZqsPFimxV&}<>i_JF2>ozlp%qh?^5>Utsh z21&jblCWe(wv#ErG!-xW%vlfPxAJ+!qS0lYT9&4%GHX@iD?;%GDbh%qPADqa(E5kH z3S8!brm7)3(!Cfo)hn6VQB@pNMs+8CvSS3Ws=Lt-vSS5ss%e@k+aiEhP197_HUUbj zX__kAAwXp{O;csZ2~bl#2AR)x3Q%W=O}y-QvlNLU^~tUQ_SG~^m7O9aM)g>xevklG zHBD1xrwQOxPhm*60ABSJ=6Hqxe(JLXsH~=Gs_bk5YN~0PDmzC2q|O7!*+a}P>eLsw z7cg~B!`X#G;-|hy0IT|^4CxWTPko61UbVwoSt@{^`r!ijWj{gyKlLL8K5WzQC1YW1HXT(aw`?m*eP^E6GBT`wVq z%Fku0pRA%dw^W{{sj}x*apZQZ{4r#>QBzv^cgW;Z0$EmG2Ggena;!W}Q)M>^a6?@rng1Hx~)97OtP0&y@{~d zRxM0ZWj|ZvbR*7$9ScBu4lgywXM}iaeRmZz&;|#ZyTxa z21qufARQz1^8m>f6l9B$IzS`^ToU{RS)$poEd`#Aw=?id8Z5F;q>n69II%=QOp1#| z5t!qW<&f|Tj1%Kv^r<$-ZgQ4tp9?xpMCV*24sEcrZ=%D4vFQ z1!}~DX(~dNN7GauOj8kZ&?nJw9!yhhLp+<2)1@9vQ*pKI(KM9@(^OP246h1Z3O@o_ z-pPbunu?Q|KcwaX(^Rzom2UE2nktQf?rmafFioY4c_(6eFimwfI9VP|Q+Y5=Mac1J zn#zM|Dngz|(^M#cs6(Y5O;dR=O?4l*m3uTz<-s%+6R7kOOc@rm1lKtY~zoRGOypcEfH9&^B4+EH1d6EAc}o-NOWSD6uzyJC#4d z!UjL5YpQVqr$DtCZIiQpf#14~4d4vZRFsK>9srS=k%9RTynn7QRZAQNy=(BPII40) zHOn~oEZ9p@M-O00jfQu)U_8b=fLJgVG}dTn*iOvKQYM}>>xj7wim^JWf%!_B`|wp} z!|}wdZ)EB)u>(xwi|DiJ&z8M=RB*Is3z8kev#>x9p$epWhMt6l?4PYR>N%gCV;Gmf z8I8v>$1hnnUZVM@f_@xEeaRXciTaY&&ZxH`+RYO6eGsZ&TK2{;>fghtzl^2&OP%WD zi1w&Ny@^rxS@!BM>Ub1EXLw&M)qOhEw-N0viOPG^)K@M0wc&=d4JA;$Hv!WmW&Ns7 zb?_mW(&41UU)5pIx9N5QWI;`4CB59G1QRL6R~)yh2zu8#uyGdR>x z2@_mr+53kX&Zi~8;n1jcg6ni%{{_l_z)6DZbdH6c6TYmvULtCOszNDG~0b+CnAMYP|- zHOr@qCt1@u$Y7m)2H}HnqbAj1DGRy;e{2#DwoF>$8a)Rge+0=N z$aBoa2)-4VvB4LMjh|u7Ja5`7n_z~VP1E~DRJCs7=gkpJ#hQ<(aFX}u&GF3paKJ<0 zbl!hsN*f;v#367adQrdXH>RxGPXppqB4=))R)`9BL`F~93|F=l_MW)_3 zmT`BO_Ku-iSgPJi?EdKb+hGo7-1k8EHXJ{>{EF!fW1bI1Cxz2YuhmRX1>zJqY0Y1Q zpW=_S=53}qr^0W|3qZ07&KUI>lisM>zjS-OnvnL|1ZErIbSkeUq{m(d#I9@>uW~XQBc}^XDg^ z+Y8rP0^nEh{1R?FE4_me`6HG7C6lj&rQ+WJ#qZ%dx)2yt{8YcGrZEsS*p3 zHeAQ-NDMYrn+L=lDDydB&U$cWv8tzG7$CqO{TL*ujK5I8K9aC+LuE7op?bfKI33>w z_#?m4Hjr*J>QUX2Q16bw^(z#qz2{&Yw(y52$H5R)-%6OL12K9Ferl1iYPjJHX}toN zxqxQEwO$9{TzJkVa4Uct;Mol~>iYn;HKVBf>0x$`NF-0iJLS<2A>?}?`3~be1>l$P zJVt=impTlZEr1C)59Wk@E5WVumiHlv4v=Rvv$!IW+%^FGJODL_RS7qoA+34*%mma8 z*E$@)QScm2U>tx8;W-zsr5k`V5(+RqYNSdICA;+i?m_VP;6|?j@L%veN8lI$$wh|o zPXe%4X3PLE1#UPQv~s(9J)kq-T0alq8}NJ;4#$H?&;%vHSP746RY^(aj|hAnPLe4> zC1v0wnep(9CBS6Pg6A~2;fyTFTnXq4aFWbJ@camlf200{DqM#$DgMYbzc`Vs$M7Bf z9zrsUq5Q$Mx~R#~@C+qT1z-(4tKr%nL_7}bH&7Rpyr0sNSF3_X*GmXafL) z3kq=C5Tt!EGTf_ikV3rF&riN>j34>~KeRm-I>8Tp#t-d?g?9U)Z~LL+Vxc_pu2-WNX&O_y zUUd~xxh`#`C2kQ{UA{)zpZMiRHF1lsDGRTh7W`c&ru^ohUch>1%~HF8Ey<1 ziUNI+%BO{4_X4IiS7o^cswil5(dH`A%_Ns|)g&^Veh`*Hu>0^RGlDY;j5r86;TYGE z2I)w;>g#oa2c3xbUa$xtYLrxOy{Z-#lX-yRC&0zOx;HE__m1}vcJ_Jva7s5D888Q&wL*RyIrxXu)=uTLS51esX9wmr z^()Da@Z>#dny1{DMujPB*km!Il01KwRZByDKO4>_G-l9dr@A|ux!N-{+{?v$!8Bh;h5b&{W^nREHc%s%x|N<3;8XV4-@c0L+!5Tr2AF z=NDsD3D+RZcShS9HBQ5q2A#G2>*ix-_e(3Fs_dDjalzpY=z}6YbDizXT(d zT_Br6%>q!{3t(W(ABE18Sc$OtHvtq=mh)d%cuC4Tf9JQ;$2))Lho&s&^Q?fFvYgMe z0%FQ?erU>4Zt6|)^*u$g+|&!GL;BdE942+p1U4Zh)k^_Cnm6SB7Df3D+&GHt1-OID z*b8CT`WJXAmf%$v+(P!sa}ZU2pp6-@@R}U`Ltu|b@KJE1_W;-p&$V#lc*Pw44rQzE zKs<@kOA$T#bf6vs&ClUlVcpbt3!XROMpL^_Err~M8%IFY+M5tv@dxFG`4RIls0XlcrW(L-8c;*QY%pBwd;oLcVTWuz62LL=90j+CbA>J29J_i} z!(RhQNE>{uydiQUD9(gynTcd?2YgEP)4K zml}&WaR!a>9r)ixzAr}n*6#w?1d6qA(g?SKzv=-NcA8VqHTaD{thpT6un~w!t^_7* z1Y%NafC(Fc7?(e>MmT_^UVxK7X@t8$qBbL@`n*alLbjw4h&5S?un~w!?Pr=nBM{?q zP=$>^Ool(PMtBG5{ZEi)8-uxxbUKm>t6VRFjS2W>r_c^Q(inqp97O+$Xw&eB&Pl<0DYIZGbQV%GsKb6mo~&37m^TxKnRw~z}noQ(fG0CWV1XP%6|98iO^do&=NjAz(pCpa0; zu-$HOGG5{-H#iwD5n<(nF>x|pVk-B-p#X6*UZN^DI2kW-6K-%aem4_Mr2Uiedja8V zjNoMa0|4JN-QZ;WHxP(+%J4}x-WYgrGM;71u%Kin$N?JO6TlOV%+&}ddkM$Mc!o)& zr{T$x(e;d$-6P%1^J}l=^78@ty2{}$RIPqjWA+Izz z8BeIRG&mVgsIoLT8Q+8iYMkI?Jj0rt;AA`@yo_oCPn?V=G{y-|#uI9Df|K!tI-THT zJfSXUG;@ZN@r0(91}EdEA-(R>;AA{YG`BQ38Bb_|6P%1+4$2-UI2peV&`RA|a5DZ} zKpULkWIR*Ys_U4=kvhi7c-CEpJo(;DkT@AX68V&1<*|Cme~pZ z^ht5+@O2aXWfVbbzA$e9k?K7HSmt7WBiU>Ri_77RD#|A{$-WDVW)@KOR)c7gWL5R( zlqX4UehBir8O6At)1C3y1pI1A)Hq&CqSB!Z#xBK%f+V8#C+Fkb80q$RV`TJ}NOxh) zrAQN`Zx&Qp3!#i!}#X|!}(#6(4Lx;-Q<)3=h7-g_WV5kAX|aTwWf<9CP&OASqD1@lZ;Bb zHK&$-ZS=@GL;#vgVqEhy5ad2p{sh9))_wBJ%tbFAEScVT4oDroFzhNh)ER^Igv*Nu zOBUsS4_d?J#e*eF^CyzZ<;8;~{>6jtKT-gWsXZ1UrS9ttIbMKD*Fz~wP86WV<;8;~ z{>6itRc9I{{>6jty--j})(KTph8GW(oGUCn~9flSE5_^Y$ z<4n_}B&mA9$6EV*1B?y5vYO`BMS`LItLe)K*(#j#pG9v{k0(SN+&mVS z#ei1zo{Egbp=|4Oqk0 zow5EDydgyIKx1^}B@Yf>c_7SaT(?(gO5I3lNl z3oS4J+^d)h|H>ah@S_k5=u_C6$MIh5%K52Rqn6b(D1x%h;+mA)6ql1*C@76Pkji6C!Gx;);VBaOy+zo8lcZo^fOz!Br(z}RB@kh!Q z^xs+FKEqF8Ff+j6@-F!5;$cAu9M@MY=qK^JM7E2oIF4>dO(frKKwf@@5yN~ClWIT( zg!v%GoeNAjhKR|m0VW(nM` z&rHPJ+<;WRmN4f+qDuLQO>PA?EFUqc%Yg~YM~u4zn6P|rf$5uY{F7d~86@iN1~9)R zk@_~7Q%(?T?gdtQDLv9ph)xpZwWC`j@k|EYS z0IVcKk0e8MnoyVwF&=-=0EI3&7)cBYk{Nsff;kx4!95%h_}G1kQO=mY?8P|2jCUVp z__~DMjey}msNQSC_FI=|XZuY8<8g2$BF)z&rcp|q2E<8lA|<|wC_`3*LnzXv!w@YU zrrb!8YYw+B8ERF6SvM*rF=kVDmQ`Aq?F7$j!o*f34q{?^f%qX@VWMbj0H@Vh=p%k% zqUZuP>;#Bs*xkT{oq!l;E-+yy_$`>e2**EZ9eUKt2K=0yFz+JsV6rgR04uFSkF*YG zjWnUKb%^o!6KkEnBZYVURQgPR7{b~P!O*E}iWz!1sdC8vCYfMoBqp&HnnY~ROtf;S zNUaY6$*|!fb>}BC94rvT9H^g9{7ET)EWrWteT@6JOLqYL}-9Whsb3_?LiBWCg z_otx#FKMFsgR6N6<=6*0 z#UBh_Lp`I+Q7-{|!RTM&x0%`ab}au1_kGaiUSyhkl)V)MEt1OTp+Jq4&b&vBVJfPp zgsL*ACQ?~igai+T8$%6dk2;j4R74z zU5aMojZp|{A@C}g{1#3$xRcdEEWf(~-58D<+$pM_97Y2i0jJ03DN01aLLlbB)vYts z8YLRsSpZLilhV%s|N0(u-^a1Dr8T&-lxT2Y2FVxU>Kzp7C2WG}A--OT26rby_P~i) z#Kwc8nptz7R_@P1Dx;8CvyBx-9hx2)g+!+bg`D6VL5>?rm=cI4Ah0%~x?I55&($Okd$8kRCx1rg(Qv6O)pnLZZN9i057 ztwrdI(o5MRq?h6&pmzB4=TIz+SE9$=1vZlrL-jJpx|fbuW7tcFg4bX;-TlU^ZcYye z12F?m_niq!l(y4>I2lf=mK@EVL15XX%zuK96p7g6a$v)%B__2Jn6OC1xNCq3i*(Jg z&>i9UCmr))kSM-jP}5XuHF7LngIM!&V5Mu&BVB{&G@-C#665hF)-i8C7O%YEA9*su-B&MpNeY(rw48eqaU zYzNa;IQ|uMt~N4H;G8eAQJCWSN=Pb~9;sZ$N)rkzml%&fvC5r`6z2G;=qcW>L0$gV z%US*xq_?W`kY<|{{e?*^dYh9~bl@0EeX z996=p-uv*-HG8i#%}Qiz1zd|H^W(7`MxOzE5!^UQWUu7wX23VWbq3ob=AJt6`-wRd z48sOU-VJ1|0aAN`i8X+`516n4egvNP!11p$)Ughb_x!KzE;7bb>4F_0NAZXKK;0D8xh8#-Ku;GWL0#=hPGD%JZd1Xmvl)X^(3OyH`I1h5I7jd0y0J{iRFgPujX z_$fvqRWB&6g_HPq!1HZ5W5($S+JW|*c?LWW0(C#!c(w=3Cg3I_{77$qwXAv?kk29X zS-9$U0B^$cC%E#9L3NK_LHL_?pEUS4ovSf?RpDo!YEt!ou^LW5jl)&b{>eyqhQZAu zaW#nLN3xGK^d!b1**uUO!Z>7b96T%Fj2Q{k@4msvzX{KJpw5P?Vg5&fmEw=&9~RW5 z09z4!DV!AGtMKfEEAIeVe+8KRuW@+44OeO$%-%$2zV9523Nz^NX5-*HQNwiRKV4%i zdVpn=O~J2zj{{@i@b+d z1ZMc;8RZuMVPA%yTj5e%fhhm2GLd@&q-iAg!0*3;a~^=-U1`IX_ge}>mbScY*z#r+ zLLG(;Ti%4M%(f9o0JgjdIhoqwfMCm;kSCYl!In3H(ll*(+py(Ls50}lVIYJpZ$dSh zTL{6HH=()=ZF$?Ue|ffl3}Aqcj-N!XL2EpOabC(yDC zZF%Fix;*Q0IX;MDVPVVrae)2{%j_z&y1E{@2M&K!YEjukaVa8+rHG#4kY8yjE3DhzfM}pbj@S;B=!b1_k{00USpfLa-5)Z9$y6ogejPT!I6vBfmi zk%@Mps`@9x=K1{cy@2Qv;}ec?HmcAvc1;Zh#-&J`M^GA?m;n0DR#5&YwqxbWOkDMI zCYbeaM;0m27H!my>@DaQxV&c-LgYm*gvuuPlQ-bU=Fd;zQeF5_hO;#ZQzB<0hKm7} zgej5RfybxzLlFU{L;e5OQ3a5=p|82q7;`QzA*25+PKYrYVsmOo1! zO;aLCm=Yn>nWia`But4A>PquGOcKY8nZndGO^GC7N`ztEX_^vA!juT1*=e4QNy3x} zQ<$5kDUl>hi7>1uO;aLCm=YnhEd2(k)HyLTL$(l`&9yP3cd0(eWlVS4m0sBOl ztAbFmm?^Ig=c}J)?iwee_jXAUV2_akVv+z>?lc0E1#of}$)*V4WzSlT&AF)pl;-Lf za*zO(xio=k0@N6038Crc*+{3HiFxm-+3v}Z47nBLJtt43ksCp>c>>hs#xZ1p0A0BY z2`my|S?+fPmI$ymH<`fU0&L5@Kw!B5H|O3UaEt)==MEuooB#(5^Dsguo3AoI%tl2Q zQ$DS39sEi2U3|e>*&5wcq5ff?4otOReg*RCbpXfP@SFn=KL@V< zG(3yo8E`Q?$G~G$JwH9T_E0rrw*@D zHgLHg62oGF1@tMbo4Wg0H@9Tfpoi$WE_)*?pk6Ae72)+W1Uu|Bw3X_qr&S(xT{lCp z{aepgwGcyoe!}m{gGO#y(s?yT52kyrP@?acft9ZsZ;9U6Zsc` za9={kocXGZdKh(1!#Pv~mU}OQ7HG(Ezei}HhCCzlB*DW}f?P%MeIK4c@%>YPtWZ7@ zimwU3v(>>&_8@w{B!!tw(senLIhe@^SuSTX2QwKVeaEtcnT(LyRE@v_aGZ~>8mou4znT%zxb2*ban8_H{;BqE&Fq08#aygSZn8~P1G`pP19L!`4 zYjZi1Ihe@^b-J9%9L!{dy5wZ1gPDveOm#VvIhe^9*6ng8b1;(;nk}b19n54*VXn)W z%)v~?upXB)8SbeQXqn5I%)v~?u;p_8)4@!}6jr#L$sEjN3|r}PCUY>85n3&WK^@Fw zOks`7nasgV#xQ-yvV)n7&<2+?nS+^(lp9^nWDaIBhHZ8^lR22l2>COagPDw<3kPeK zl`fVm|1hNm+YL9iTycZtip%AS8!T5`E?3;>a>b1;S8(~AX#SNhmn&|tTyeQv!DV<7 zR_AiLg3Is((#sXxg(r|+uDG$~iW^(5xUuDmi{%P?W~a;LiW^(5;J!NHIoIWK1^3kn zB+C_CS0_M^%jF8Ls}pFM%jF8LtIKl)EH^S-uHc3`lV{re<%+i(qeVT7k8&nLzGv5C zihE9ZY}DK@7=lls7=XE-64gqA%5@HhyTa(w!=pd251LOU#d zfbp+T>OBPJg!W3%RwUZc_u;8b!}AG5!fQQHCU#PP4!zb&cac^3J$q>B44@@+{~_-#ow_g&=M5*}C@zsdb5{1F_KcTnDvGODguHOx`K31ND!F3YpW93O4D<`Cnl_znmoRH`8Sa}i# z*9n!n+^0yw;5wm7m&eMJdtqcA3Fu?xNf=ybm_AmXgu!(}`dE1q$I1!mW93O0TqmTD zl_z0vosd3Oo`k`5Li$*F635C3O?7#!Jh@Mm*CaSO@K|{g2G?1lxh{{DCt+}%kUmzP zgu(R=Q0imlNf=yT0BEHgD^KEB`C>r&Sa}i#*O|grTy=*Bi<5y^z^!aa)?J!BD?lws z94j~B$1=?i85@M)SUHVMC5ezYhXf%wRz8s-M5Ng|Clo?F9xG4cSox2Dm%s~xK$sdk z0{KWSC}W&dsSSPO5d=EaH*Dw|%(N_LZRi^WEQk7r4Sj=<<51tQp>GiKV8)Oj^bOiH zOx4++QtoVsN_PfY)jrQ>hbvq`v?<_sD53Lxe!O7CzQAY4Wwd>v21=ciFaqt(8me@@ z!B`h*sK)Rd241YpXPK^pjWzdNrb`CzYa#&TjcZz^7=||Ayn#6;jy9c5UO;j@Yqmz2-QgM zu%Ym<#_Jp^JT?>_hBY`;cx)&GJT?@bGf}8!hYAmN=_IVpp~8b*I)OSJDm>Vx z6R6AK(jD7$lEPGn3JVG6SyF5R(bCn?NzsPNcOco^2>P~owm@DN(& zP~owm@DN(=aOsY&C@HLPsPNcOco??Qp~7QB;UToz;nLlP!oxDGaj5XvP}sPNcOco??Xp~7QB;UToe@VIohq3~=m(INFgeE(9P9J{s}k><`F zCTeQ>>RNj{X4k(0R=uV3(B0KK+XIF|Pxan_Mla*fDxebl8BY8qX8AVwZ$*GnF`SVm zWL4!1c<7uA1otXPf6I7k2J`SW3!UJT2%+Ha8iq*t`@@$Dg8M&NJ>W~J4_H0m!>H9r zNcOmG>neCtyYb`j$8fE*zF)>VT+uXY=Dije7RY=Z(qKcT4Zokv$o(=Iyp1=`%S;)I zHz*moUnX-Kw#F~?XB+%MzF{W6~1FXPGmGM?No0v$^9~( z+%MyGgPZ5c{W9Jfq?Pkrq*da{{W6~1FXO!n{^g$BFXPGmGM?No0vJ%d;^ zp4>0v4MJIJJ-J`Tllx^nxnIVU`(-@2U&fRBWjwiG#*_PHya~w3P;VjlGWy!|6F^LS?g zKf@aX%9$QEXN_5&0Y0-mxnIVU`(-@2U&fRBWjwiG#(M%h=X-L$j3@WYco%{5LQn3O z@$~&N*&6FK1h#2$kk=RhET|1qf6cr9(-%UYYD~{!+wOuC03!ExEn)OCT(BlGnQY466LZRYvmb z`4*1W@)OpbVkB>=-3`BGSid3pt>$C|Rgm;kaFwVQoYk&+9AVBMRK=;lRo?|by+uvT zKz(6E55`y=K(HIgZ-b-;n)rkQ>L)&GVgdC_AJtVrz2>7P6;Rar$!l@}l|x!Y9YQ-u zIz@m9>e1B{6zYK8JIv{#!nNvX94Y7?Lh^T*V+!Fq{pN_Z$S_ zz_nNr-6sp839E2`0j>_WWID@P`L!oA%1vrDqO6f9GlD4JkSOPY{F99G6Hw?VRp>2P z6?Fl@mQ}qQf+$~>D0hMUc1BqdM)4SBlk&nSw2LblJ71!_3i1~jr7etNGRi6HKzoqN zuY)LSBuedx=vi=eRv6_kEXxvgR~Uun1SOTF5@jLC4`GyNK%vX>8lxPfwuVvI`y|RV ziLx2wpJtTXf+&N6?|JP-&Y)neU?1kx2Wy2Czs()2I2=AR3%`1nX+v>1%WNHnD#PE5 zYyaa}CjR!cXxwstO?BCu(a`lsPW3*D-{hqLe?`w_@VrdV7vcE}Jzs|BeR{5jCynge z*TGW_kC8eOnNL(8!Vu<18w{`B}K+u?ffBi&%8_9>?X*EM022 zkko`q zvyjwDNNNI=MhPW#5|WxJdQHjw%Kb8u$~{K(6PgMpju*}{l22;L%H71Ur&QSuAaru` zaHul*OI7wD7}Vry2tBPS8;mp)A(@l~yNcvpGK@68QsxW*zS>rLm-w-{fu6t_!rop= zC9lk}uf-0Qo+0e7=O2Ps&k*)4wX879FgZimx0+8PsLVzu}S%FY$`IC4pPS_X%@7mf^Te6{u`SLzOhO9Z)_^~ z#wHbeW0N}3?H`W9I?vXWnWz=)RDiM*_3gYM zp=s9TI0Q+9lo-{)6gm{R0Sr(hy`pM;~ zjBml!$~DGMP96Z<(}16V8+aeGq@MDa)qethhqzhf_*4~f?W>_V!42Fkbalkd0X!S7 zZYh9H3p+`88W`Nhz@L%og&e6)0L4*o1GflHxZMbNH(Y%^Q+PqR4ZKXkn;7SDggpj_ zb?-f>0P>a!s4+@esG4IK?HVa6sZ{UZfYn94W+aRHKS2KlP8W4YGmBb#5+--J{DqA2 zl`(%y;D8U{DR8>@JEi!m0k0%Z3b9i%{5im%Ax;XhQ!;!P;623sfEnL8lo|gG;Ae@G z!t5MPT<&BP15T=mdw9A~^hu#$I-J?BIg@OTl0tECtKQ-850>^jY6J_Vdh+*^=ID_x zBJ1xQ11y!m83L!3_m&FkIt~VIW>Y;*&~Q_BlY#gljmX z9jIEOP6xPJP#r+k6LlxR+XU4K)DWUx1^6Nyw&OU#;GWUln2az7pk5ka5@@yuDPoL) z#?nNs#n=mn&6GKmQ2UU+w+%lFaE|E!a>TzIcNgZ8|Cfup_;hPo8^P9}bd&U37JHWc zd;?M%?yQ6Vhsbj&o-mBFdOd1uR0*-CAjc=dIbsrQRKpfhEQTvn!*UfFGdD1I_<%B1 zIskl{j7(-)#&nQUwPEHaXQpH(W#&~NvGE2UNuMBxgog_zRiVsWCZj18NStgWVpj%p zRtcD*7xF^S2ZAvw z{NyE^yefU>A)?As(=yBoQuHHLHzUt=ARbVmq%1zZ#s^Ue6*G~+ zwjcnLhF@RNJaoixpdZJK&a3FmL(XTwvD{|Mjt3Wm>@lIl>uX*`ti~ql9;Rp%nRy&maZ$zzHHxXh!54F!Xy`(EPEa7I&im z!FW|zwLv2o#!?3PU2~aqQ*^DvS=UqZXXb(-ibKaS6zYt!_hZT3g1}8`IU2}aEs&i+ zn7V;5Ey&rpr1XjMMl?91i6&Io~ zkRSBBc@<2SGB+YsaZO1<>M=YYivVFtxf{yGF3R0N%+^Kw5yjV_a0K@4AoYG9l=Ay+ zIoR)}Q5?mvFEJW}u8@pMv>|=45W*qOACHiJP#z)@ZJ6$#TA~e)NVE|;fVJJMy*Tvk z_xfnQMrk24I?@$GIhu@1fw5t3f-X$%zD&JxW(}cMq~(jis7%#4z|h9VWxO(rB;&Y{ zl7pr*OtDaYrZPTO>1d$|I$2+KOpK3wJyg-eg&qUp8kYbNvOy>ZG83U}4Lb-nP5X}i zgQD5MHw4APV0vuaqSmo#7efK@Q3JU!a;>;7l?+EB1OoGmi!}@RWtUIIx;zk0pgg%@ zTx5!IVbnA}iZebl=_+J~TRgNfmDNKtr{zeA*5678XpP6uN?9~zER^Fi1om*K!Bs|k zBSjt#By<{wP@w`Fj=So3S7Am6L?dDS5vPa=-h7nQADOj5hl(i^10w^fPLGE{k?~Nk z`=lOYgCk?CA=c~q=vA@Y4AHdg)iKq|&sY;O>x&0}NK0R4k%#N#{k}aS5Y^3rs2&;Z zg+4osYNpQU7}?S-^q@zrhQm7(A5>$5Vq#2()ryuVnh{&0p$a)UH4bLn=i123p7D zC`-tN;$c2DieS$^sIRC9v@K}`Ea9URK|0-jIzG*en5X~}tI`+>_4m>8BQ7+of;hp~ zPt0qe2!s*=T-j~701Krb1%MkJ(?k?hgfK2L$+)=YFD^}jrJ0nBar-RIv@|ITI(K={ zxhvudLS+kOerXm6r#HG$b;R!u3$7zF{?_#ZI^of~YHW^bx^^14LRu zIQrEwGMH^f1+&fQaGXaAKPJQmxx}tgbSU`zTEf!!u|;-JUn)p`$caE%@<&m7R1AC^ zJJ(`yZ3s7C1d{|6z2Yrzt^QT$^;>^yvG{0am+r2)TJQMca^AvNE>g4t zSraAkaiVzX8K^dxs0YUTs=vKk7h6CF`z}G!+<6J>T6SI{d!{m^KWtwG@12TY zO!-3_udI^sC59azRIxo3tdfF0QrI|+Ptn{*VPQm^%07x->12zw6WLE;eFr-#6^KZE zlzS4Dr8#mdNkGv`Mc7+;aq`Ss(64vI3wFb#H%Vy~s9=P7L)!a;$I^F35-STsgEr9Tw- ziYlxMvKUHH9@KASSq7~j&iu0~{@x^597&b=+=s`RMe zBmoTD9_ypxD7w3kiA`Qb@2i4o?8AEV|NolR@5}L#_aD4wjZR&9f1p0{4g-CFLuYU} zmVAWl@CQR(M{6wXBe1&;@fnA`XlOJgQeYcUHb60&2o8%b%3{NVuUNx@>v!P7az2>o zn9t_;oFEe&I%+t37MAi%Ab4v!I?zh7Ul>uqv7!%>?K5Kgtmg}qx~B(=c#$oJ(Ox_r zVg=-+6b>H0fa3yfo=R_VU~Ee;GI&`B+!QR(qhj%fpkKj3;;x|R1d0~*(YZ91-Pp?l zKf8Pv8;S!T7wE#M1HDk6790Lda=O+Tx~Ug%0*LPeqVE*%1A^CoqPuDh&<@@Wq&}o- z9MofRiEtz+Qny;|Nypj;^___!G?CX&^<)FJFc%HPq!^8l_lMCYDvR{b@*n_5IJDQ_ zUaE|js;GZd^<5HtOo>}q8WioT1`2``D3N&g51C1d>cyWa!8#ENF zDAuWiN|Km*jx7y~4P<13WIrqO8&k+BZCH8!yE|51VQ;QbdBUZ*^85~<`RKNVzELQZ z-}MLdBZz7uf+(;1eQiV^9+<`$;QbP~G_B|m2;RyT6yy4kbwP%)h=apu1w0gwkUA2;!h46?Ku0;`);v4eYHJ=yi@d z{q~lbt?+)<2eI=|h!Cjo6QkW1Qn&9?q4*@ll!bbd(f} zj?%cK2;Ra<&n&DQc*+hamcJstxZxqUP@-V{5Lc-9b|owU7X20)H9P``mL|iR;9y5y zJXn%qI2U+Q$_$M6>9DUdI;@cth8BtO}(HacvD4TwUwJ)P$ z?@)Xk2Y5LAeH^zKL#gfKM#QsLBqf{UCE?n4WZ@+(HUT31Q1B1ZzX~Gyqu$YmP330~ z`v6gurzm?o^%ge0A0s&RCZm|GF)kc{!S({{A7ejMCdX?Tj-7!JHL)P3i{7}(@QuB3 z)eXZ9#xO5^&tXJ!ip@nF1VtvMQPKu)RgvMsxkkQK4Y~Oxo8ectE}5RP;^);+TI?Z4 zA_9TE#K_q$ZX^~B*8G7i`NOf<57aIKT>z$uqo-#n zTf+9|$PdnU2t&~_qP57j_2S(lHY{=EBT^H(x^j93^Czm3$P`u@_H@4;LSE20;$e}H z@{eChg<2^V#qn3$eb;XIWDX~Fvk=08yh@`2Bl{wKl`vm4YFUbXk@zcZAaa4YE(yeS zsdx}ny!9Fv;KN#D$FB}FW}b{G7E`bs-On3l-0#%~OI9(> zD>A5~FMPtNQPruSeg#;nUVRgRa1J_b;joN!l30-no3l|dgSB#d1y5J=s7%d zVxpYIk9vOCvg5=0=Ra`t z=E5G@w>h~i3EK;6w_p)p7RCGR=&#>=sZtSU zOzQ&=$c2KBq7}P(zCh$bS!4=3U?D+}hj8bMX>#AIumSLIyrqfODYPR3Qn(EfrWD}LXL9a)J@IqK>Zy{#`98t^ft!Ur7UU{s@+<8UdvyzHtRaKVM`83Q#UC{Yv5r z5MNY>y@P3$eaLK59&LZWP4Klq(DLEwFuu|DJ4Qi$icd~Zo?^_<5+64G{#)-^^z@O^ zb1by-_*ZQ~B}#8(NegEvqcV~qea>5lb%GWs+NQzxZ4n`<<)1&zax8PAEyz$E9Nj35 zNT$lL#|Bb?466EIrRqXSH$cnP{*D*njbc3GMS>LP05~?VE#NnW^e?|%`5{KwF?<}R znxI|l1J!qMAQYL|hCqvH473;?z1D)5Zw!M~uvB9pcew5ks|b5~TusK-`0@%+4_p6p z3SX|rG)2y4c}=(juI8-k^bJN_=VK@Mj=0JsV_BOSn7u@%%j!)%n7j*MU=R) zvyz%mFv0kx$3{S8rOtA6hh4Z(4&rA7vKpr&!~Q(0nAks15)=%XaaM~dU1 z`=)~K8z}5?p&1vy$V=ZPYh37N!=8^y>htn0SG-b`%^7*4pbzX6-XutL)?)YpDBmQ6 zQ#8jwd2s~_H;C!5iYsIK1@!k|_yr3poNX+ibR+XfV`PxlL?f|Z5#n_HEH>-8|GU2gF*cAmgJ_BCjQ^{z9fUGM@|#0(gWz z%$Z*O;;XtKt3~q}4tE71x)6i=wroQTg%W&pV;5TY1NMvloyHD3V9?m1_#&dXvBN5k z$h@pCe5K2v!TlEV!IGds3j;3RaFkkQx?24ys60~P{);;{+=mP?j+TFCfE7zo9N-H_ zuLz$NE4&+bD4nzzz_OQN4ghCx|YfOIW~wCNem%dlL4jzJnI`PsR83 zMC$r)b%QOH;!!B_;p0_Yp{71y90grC6`cOUxdGX8(%Nx4vc-W?utj0CMsLV^!CnUr z$VIE2Vlh_BeEAg{i^VU$;&Qg!kB{{9_revyV6P13S$uq0;HaAgW2E19V3sKtp;c;` zR}&e6zA*GFytY3U{xMpds_(PD^fz1m*z*1>Y#otuxbB>pi+&&eF?5dpa^oLFO`<^l z>wjB&{Y=2TVo)(!ayxyq;Kopr(T~cNKPaR{`hCaoQQatL!NPWByw3!4t4P-9+!`WE z;sdi(kNC2W>N>?!bY*;su8Q_G2-?2=3VZd@;a>QP6_pVg?(AmxHX{%O;o7zqI1UD> zI$wQfUkZhakCPACwfcs`5}f)Wi=%Y<skdR;b+frsYx4|i+hmU=5mVui(c@shSN1{uGXfYoF(c%cvO~5}W zB!`F5jBP%#GL{8M*Z3lk+u=;yycjab7<+*tgM`r*Vi~ChxK21^kPl`h!Y-hgAn3(> zkb*^s<_@NDfjv%$dQSI=z%j@bCdLqXM1XXSW*}SOVp;W!RY0wQE6yq-K2i!Bws2t1 zhYd1hmydEd%lPD77w)33TA<>J_JGa7AvPan;$0GCD0p5D#<%M<_}Bxd+l;;kbpRjC zOW(#Mm(>V4T@&)LO}xwa_>#u*!DRYIUjXbfO~l74@rKPCWD(OX0va?OA9FMvAM@ZW zV=XFzzWD;NBIqOcNV_Pb1&9f7Chjf>`vtZbBw)Oz=0nqLsEp7|2AVwkM8(;8pr^rE z#)e0XUGy<4NI1xm%`q@^LCCF8E^Kra`b|+!45*Z`H1HyY?Jk6e{!0fQqo_*-_~I`0 zLJ2O8fdfZA63Au3z+E%`U)mtS%WyG%S=g$^y&ggwA5E~!4C342PzpZwi+33x&%?#r zTN#%D_hmTKSZl>9V|Na601j;UH~<%q3lm(x?H7_*_jnqpX9V5D$8+M1_3@2J@Y8VM z&Bq0Bao!0OKOjmNYdcAGzK#&ojD9j(xL}-|`1l@jMoYmPCo- zc`OhujS>~-5!Npp?6jYQ5Xu&QKC5K0@0EvQM~YwQQ%W8yn6V^!%6p`rxvbn_rN=J;77rsF8CNLWE5uf zwFp$4Wkz8rvK%FfXBoE*qe=fH&Dz7qU*IG+^t=VvH#dyIP%w@X#TgffmPCn)>*77| zNk}Gp_<*q^zbWX$5$ArAXB37a`6y95`2x|BC{c0pmxI?%DM}9?y>Pm`^j)U|qGOx% zR||SEAEY3CJZ(nkLC(_QN(b=4v~)lr)x|-oI)Dk!)-m{C!tshp8ZY~$OB-t+Hnu0y|N2*LIB~ef zdk9S+R#CO#yt{+Ne5~4BqaK*1zQk=nO(-IcmbZ_;0>Tmw_07j$E9qUv$2;Qf;UkTV z0n0~Lyghu3(pWymYAhdZ8q3Exjpd_9WBFLBv3wk%v3wk*v3#&eid8UVEibuT%m*pr zRg250fj?PDLM_cTE&$?UxOk0baZ#N^7ww37USH>IFTxrL{D0uz)u7%Fhul7a$0}WU ztMT~XP>(B$>#+_Q8!mYarIa$x1Y(;YLw(LQZUo|+aPfLG_5gJkT(sU?V-?al87@wo zT?Z6hlsIn?b50S~H8umY1un+IHFf~iTa4DN@kY7f`~wdCmXG(~*duy&(f7U%h_>4! zK>b#u`S`tf!x9%LdlB$`O~=PRO~=R2G?tHt;dE;Bk#(fMnzrq_9uBJ;dk{d!8YuYN zBmNHY-zhZXcJWDO38OQV3Wc0~&=Q5qCjDm_>HVpGV=^2y<nRJIX6w&NY5ylrxAM zD`&8EB?avU@lrUHlMm)0UQWe7V=nNA!6l6LU3~V4f7~uU8K=L2SWVU-WJ~zaDJ?bQ6{8)jT=&u;T}F%%t!{}6pI52R!^biI+yQ*5gijK_&dZ; zuJK$l*|*?|a%qDwEyRUQj)b(gGM)nY3w{E8z9jzkU3|VG{*GOIl6Aru7gmq1k;)pV zOmL{^;dnG_FCVSq4dt6sB&~(B4RiZYqp+iCh9QyNAk97ajWuJWMI+n;(!Sh-kO=pL z@z()p6lBKHnX!lBZJquzqhU89qmi(H1_sO#|7kenjE`rumyh3zcNrgl)L1^4Uf#?QGs`y^@8Hf?UwcYBkjrL@hxNm^=!^g$m$LxH48u^HwM@IP&5gmLCgUBEvB5GB{%2Y(G=pgF%`>lQUIs4w+ zn+Mbx{Qu{+D|?^4*Is+=_u6Z}z9z`6G`<6XwkAoF)J?sx@z946eP6Kpk~A&(xb|4m zG#>!QqX6Up4U&c0n`t9WhKCQ0r^MRw6By4JD-Dy3D6cK$gYbH3&K>&jCuD>9sPIkh1I{;-vqXz(0U^z|3n9IvH zxV;tN42-^i2^<(8w>o*gCF#^jh_n6~Kwjqrp#4ICQ|o*|GLBItX4vO3xi|)}k%>Jw zERh0nbi#6q`iv9OmQ8GyAxO#uqcI0yGJv+l#)HPmkq2$7jfW)@%j?I5T&YJgNkCRV zRq9zr+}Y!c5YNrb#Wb|2^}`ohQF+`u#T?jL^{kA z7YxA)g#pN=P8VlI%gOm{=O{T2h7*tDO6q=`X!(kIF{ikPwB{1>y+~6vq?KXq&tWq= z153OMQ8zmQ*=bz@my-cbKue3JWnoL;hbh8S_j4DACw2)_4k+h0z*g zL@SpGkB-~2gJA+3(TXr#$0!2d>jUOx5aZtu$=-)_&R;$Z0>g z3JeZQ#L+3H*l_@NS7F&zc`3P=lE0w zr`suLYJ`I^U5;*On(2yY z%oJ-Y4R&eSMH_g;o&v}!BU40l&QB;cfY4}~ZIqmE73bW!?}t207$qBa6`ZGl0DZ&7uLhz#w!T0{}ZT#tuLV1Z5Q5vzV~_ z(sYvIRF7Ajn-00bGcc-O%6rSCsIf3iJnm%k8A*gh%CC%Ka-sr+!tVl0fk9%;o^_>( z@{2xEev#d^I2q%lFD}Zg%q@=c&XHVW$H05xN;j6+$B88-om)#DLuL380DRDRfpr6E z%ibgRG+cjfkZ2IeAzti}$WwqUzDIdS;Pf0IH%^-sPTB{?IRYeyPJu3~aQbfne-8sd zg9cMFke3gKNXqNo)^E|ASkdPc-3^=v0MPZ6(R@(sy}NBXiaZCyI^vx@x)N7tNA9rP z0LPmED0LcS6tp9y7g66k#VdIdfZ^mB7|$AOL^jiR&g7;zwqWud13_%aUc-UBJJO;2>w3Dl1gz8HL)B4HV z^A1A9pjd8%k0IJZqCWv3Yi$@Om+`nF{D|Xw8zUMCGD&bT9mlBo`s!9-5uA49I>wm( z$ANW+U`BGnDDPf4+z)W8bFudTTpk2u)%`?~&G?~mJ3@5W=Mcv5%!)~QOA9M1{q$vd zIsv0h2`!FScBL-p5%o2o;HJ}Q?rtPb@90=#3@4u0F=vd3q=mTVxxpNjLpf5r$fTIY zVt`2&ZRZ#d8ZD6rZErFjG!8``F*luXyHZf15yXfs+u*nzPzw30$E%?T#?QTiyMcxY z77Zh#Ju+A&{!yZQe+nFGvK^IAj#p(R} zl|2y0bk~!vMamwCW2810$r*a(>150A?I83u6EO`kS&3!3Jl&n;f3o2}$b(|jR7KYF z_~)duC`x4jn@vu3nmvykenmKJp+Pnh-rf{x9X8H3EwRZkL_`5uw)2-FSpbra-z`v| z1@Vj!i6kSptDS@ONR;2LIhKF6r4DordxfpV~+y#)Cn zxV7JLj9O>H`ysHN7R)FqO4E_x^i-~66r9>q;+b^41+}B5pZ1?h*r?sIGh!SjOWWft zjiSuP$eZb);d{*VgcM90sgxM&C>W;E#lwzw6sRu=A5kp6;~3?t>lg)RPQDDP-c*x^ zFcN^=j{g`?UKAdpv@)dqIk}%5(jw6Ot3x`3jCha97aF$?$d}O`QLY7KjpL)e_rmEW zfIAr4tRR{10gxRuo&=aYqwT2ipm8knupx8Jf*Cr9ho#}5SSM>1A>wkw2aT$T589>~ z4;nR*2W_>+gT|c5gSNTGgT|7`!^TCEt}6@@4Q8p^2MH>1R*FX}jnl>2LWAf7hY3s9 zAg-s;DPh`aYy~KaJpi@jVQ3aC10MunIHhqFKzTiWBol)N zzA_#DSezBpq}XZH0yt@>Sr0H(fVL$7ln#w$kq2$384ns8BM(d7H5pa8Ah*)6d=u;iiP#n)QnLBo=dharD-5!?;a@y-cED(-pGW%()b7;qFItBI73CM`oR-l|aP z(PbXMZ8*gq`CbgjCgC-~iJZI0B_3T?=OSBna2yF<-vuCxY0Q{T>joOtVr`{S1282* z+pXde^`kK@ms%^Sl?i4$jVgetm$aE^+T&h zG2{Ls3=`wIaU4;F6*egci(qP5!a&C)oezMO1_AZ`Y1wK%uIN7i%T)k` zpm9jt`>u>?y6?r=&bm7a>SpGxNY8DOv=$n7inaaIFzz<4 zH0}YQ?OxeW%a@HijR&oJF2#QooK_md$?00|fRl-+g@(a!(qO2)JOkBoCIh$7xDQ~e z2W|Ho4;l|e9xfLcVWaS^Zur)IhG!s0J!?ZUOo@1{r(vRRcZ08Yf=5HY%!k5a&mqw$ z)Y{u&5Kx*l&IKSt_xICsjd73J%BBn6a+0zh0ZbJpEt@fU6J|KG$upFeNPI1;VUQ9E zhhz*FE!V;NN#jc6PJodqYx-$tWF^oRU;{woG|7rVOF~q-*QmJzrgaEc=m9Fbg~G|w0Gb1fNXU||D|G1~S6@hU-%WHjGn zupV)Y#4pz|s;|C(Ij~5=9p8OA7{`dF=Y1HCw+g1yv&D`PrgR_v?Z6zQEItkIxGbVF z_UTT@X>}2T_i@zn?*iBz!#FC|b{fAjt~8zpnD$273&w-SF|oGOV7)XrmbjUad)__p zA#IJivaq(_Mx!Cb7*=RI*LcwA zj67lrbKn++jUAW3@oYfWOr4G^0orS#S}n%Lkr+5%BKR#db_25W!Xo#uE3s7MUyflhI@7uU?iutA$39glM6`_y*)N(F5^lr7;#DOweKiWHCYX-wXBiG$e*R zOpuYlYk6N$>-)^wBpsYzu3zAt`@W(LF|H~q(mDvZ{W&Vy5|m)W>=NhP6nCJXR2#Zh zM^$lxIPdXJsjVL1H!b$Ne)A1JsIAC@H8&sD^_v8LmcJ%X2&amRpde}Wy*Jr3$BqLt zCO7dm49%>%8P}f(#fdj^lVH(efwOK9tzn0#0mcwFKb^>M%>C%tC`TQS>{z0;LEnq4 zF2=xe{*C&rH#;I6Y+1Wqpj2A&dl4z4c%kkv23_`R{xAih&%zK09#OK@Ehh%`h6 z5xe4sia$io36F?)ZXL+6@*#H3zRp6ikv@Rv2pEoNJOU{4dXIShv^{P-n7Q)v9qh;r zmWKy~d@MU;;L+hpfSYfICT@xV!+#GzN!$kGPGhA(z(Mx?{j}U~+-<}(Z=HvUFaX&| z<1WC!%6KhY-)oR)Fw8)teQ-TskZ3T>K%`E%USg1FFw8(C@^xemQhI`5X}_Dj*ZbY1^W58WA3u4JwEaB}SteVAAK3CDJ58YNcV3T*2=} zpwK<5IT{-D`aNJYR*?F<8M9RB%TxO4@F&1{Z_fmCvN`giGcJ$^sWuvPY!y>uluMORE$nS&m+azQQjjI6JN!WAez%XQ@i8@x3;QyQuaV9*MgybAS-S?gz zi0K#6@g)f!$th&*2%KIJ=k+v*p}c)h(JTBl!-)xSI%DH=(<~Gv0O_W&LU1f}_nw5~VZn?v5()1o za5$FjsKoI%;ux)?``&XQX2M$;4IO?_J(zC-B)k=QT%C*-=877DdNKg1puv#J+w+u1 zi}~o;9np!?LmM*AIyenKJL6(07XANY7G2DTYS7XpO)WH791eF3XfY%xaYh5cT}&t8 z&4p8Qc2MOwMitQa8iBP0knozK1o_@FxU3e}I1@`z+`mVUnVic>w7O#f4)x7|o#Tf6 zj*=o(!-Freq2?muk`u++1A}2)<(Hr*BJ>SJUn%GtV)Upw-~v4O(Zk8%gm*0hJq5^R z#_)0-d~OD?lG1z(;CpX)0tN%Y8Zqw?*EpVM9hvio;arYbZ3IVQ&zpMhX<$AtRHK?n zLPtUZs3SJ4Q(bISM}d`)#I=63Hk*#S92y|F-A*_ z5jAq8SMn>yT2HKXLUKKg^#EBCyZ{!KT27}MhkQd!KdOx;e~2>&!2F?UINliJ=gFV^ zVOkmHTmYmj8e|HZiYxnL?jlVEj@2MDnW47I+j@jHC8@p~eg^^M>l&Cx0KRt~jb{PM zyVUQeg*23RS=O?sX9J^Nkp^+F7#a-)jOzi2#g7`%n8pVfT2sZ-2QF(+)t*F-`t(Po#CF zecs%x{gH;^UIf0}(4uhwFd9-|uUB)KH*L|hemcApP#QUAHDAw*3A9@R$lIbQAPT_c zoy^`0K!MY^1HiP#;#0yS%I$&@)oWiIi5z$@T{O+}vY1#0d9o9Lj)Mk8`rLA9huh79 z5)C)lrUr+@;uux%nAP{d<8A|@+k;yy3IHqdw(0FhutKP^R|feT&l_xfpD zAQU&yXaw*?(xuooN4ts45$jxb*2nBbV;k9tnP`hKqW+Gu-ksoqUd;R$C9Adl5Z*@s z7#C<5M&ceR@emvz75}I|Eb|@?9o<4L!=?8aVgRmdF&>r0_7Ol%d zI9_2Qq`^ey6az7O#JQCQF$Pxbn9O=hhDgrZM&bYh9uhiH(n=-zy>QIptIRtX;<{-o z^KOOfZ2;GmY4E%iaNPnxWunmuFqNCOt;WM(+P`s#RyRaze;gu`Mr;_GPB&m;a6PTv zX{p&+RiND#zTYLL7-iZkh}KfcVn4nSASd%Jc5XHwU4RB|@{-0VKsGOI14VEh6vAK!(zI24JYscEotlcsBB|A!*(X*KZjl8YGs@3z4pY>kWd`O5<~Y+`}ja={iAb zqj56;9m@WGS{MS$w$lQq@6tl{lkjn>LUBIF(tM8O>-qVZz8C^D$YxHPi!oYbjI53% z##kR?#2raiENwAH)T2?lw=`JLF-lgqSP$M73xDfrTqD*uG7<7iOo2m69HaVK=1mJ7 z-3nCZZH4P40N1tH+XxL&`7@AQwluDfU!6S#q2jRq^Zi33ZXW())$JJ!kb#VO*0IbvaXMm}* zw2^D4LYWZeuj8Cu$N#E(lO`nijV94FE&ybwj)^DIMnP(&VUgmpWS4U;e1@b%YIA|O zSg5toSOPFn(sr`(put!N@=^}h*#?QmoV-XB`(89sAdRs=8rbC!>4+e;(RcwMT@Eb_ zF|f;Fh(7_Gs!;6ZaV*W}$okHl+YA5!8t)Nn+-$NiT4Ri;5%~UA1nH4rk)!80#php(7v11fmdpAJTx*6aUjP~}y;p%KhR==k*m=*va9ng3NK(X3gz#rv3 z3)klXS$)lD?--n30=Rt*Y4Z1IBSumHWJ(%O19)PZ<}U!+D@!){D`;J`kOv(5yiquF zktK|K-X-w98~`FT-UbK*(t0020v=Ym0hht+GyqaX<7~0E(P#sNGz_Em?*w>E5xiC! zHDYa}aRMN3kY^guF9Q$@jTgn*M&l2Fyg}}P$5#ZemBxc&ZKLrmK;9tlfya%4*Gl8V zVr`@GF+koRCQVN959~N>vI|OuK#*yUr4^|Ob7Bm<28?6Qra(&>QfX8tBR5x64ISBA z%Fn6TP~abk>Vn3UvoYS?!ChLd&NsqGB#ME$a7ts$O$m{+ToMuTSopO-%1x}8cJl03 zzR8TyM^ZU6IvxnZgoUY#a*#;QB*ekO4Ay?(hI{9pKy9624v`-YrdtF9b0W$Jgb2Ll zeO_%U%Jb3Q@-X8ZM|acJXeu*)|L3=6(hwJ|<71I&I{M1NvhI8v z#?hx+ClZOK#jax{d%BJn#vifGDH~TW>*k`{5jsrAG{{D@r76qm;;@Jtdc3$6J1rx} zC+E|s$1Sa5Z()zuQ0vl(#C2!ADSHE#_LlcLQI~qld%V}7NySw1I0*?9@~!C0Zv!wR zXxD|?v_!Sbb3gfTWAondo3>5n)I1^$h-aV)Vy~q%oQ*X!{niB zzGT2#3ixUTzxlKPzxmh%zxkX4zxnzDzxnP0zxf2fN|<~ygzg7nGQLAFSxcUR*$9)r zivWDOrU@n=qp5<)*K!uXGt9VTBKS`G6XFl{-xE$SBdCA`nqV7dw$ z!UK=CM_^8Zc?4!HJ@A_g;037UX9dh8OkPlnz;Z1yP5!1BBQopp%^Rs{J_K_H%%dl1POXS)Hh~F?*ggBPYMttvwyTOd< zFn#>7o*aZf^U3}mf(6Usn=F0a62Zv}uZJL?{8@a4=Yx3oYefV!}c)GJ4@MrVYnl_!T|MQ?ne;$iP zHS&HRnmkEs@knbO0Mm~TS*k@inYRdTJkCfzo;_r`ZiU$nlPCAc8xP=}0h2$QFSOSJ z65cyLZ|U*G6!Vvd+8!f9_#+L>Iqr$@aO^4xx4U36{v$A%FFd%g8fGWVGMMJz02V*+ zU5{^Wt=|Zfhbta|$)ifP{^HX+1W0%v`l^kGZVboYJB=Yc(>lq1V-z_CI!wzXY@K61 zbiyQm`(WB~X1?;r`1xafyaE2!-J0|>`P}hcmk-M)!o%+rK-&d)9Wey&Qw?O~Js{yG|H6jj zq9=zfE>iA?$<;aL3m4;3Fu4QdA(;HJ+_*{P(=fS1hv~eCCi0iNjhG(ppJKb^HZ7Xm z_{H|dJ#3f5XVHx?`J+2EI<@j;zNy4H z8Q;&tya^`NWu!-KCG&@BO6CJKsjN?&=(_Z%l%4~3{z#u%^%XFwbH5HImkU^q{H5ft z`Lhm2!n@=|n+`7LFef+>WqrE_Cd-4f)e@Ne(Y(d^E)!St$K>Y+>ny#G1CRVZ0P}X3 z+cPGA4#9NW0rNcJl`pd4u7N*!dkp4Wm@mU*|89TGyM*`tlP!K(JyeWvy8-4giv;rm z!8?0}#k;1#l(S#OQfFvxw1Hq6y0gx<;V5{qJnWCJ8Sz=7-2gs$WZn6OTp@Vc z!Q-QHxxh!}EFCmOZ!j1N&-%{=%>@Aa_Mdgg0rvdn|pLL%L`DrLf1IOZThJ7g5 z=HR~v_MzYg2cJ(_7J{!GC%i&*TzAURd>D&zG-YT4U%jIo%?HIONAra_%F%q`i*hud zbE6#1=g%ld^AR@6(R|sAax|aGq8xnyKsov<0Oe@P(1ful7h4A?N3RQEGfc|Sl%WX^ zg}`?(DMwR=CVV4=2Vq(|w%m@uZ7?_re?Ajn@%cVLF7Uko>u>1~We$aJ{rQH_P{22a zEWUkQsE~QQ@Cwl>ZWD6X5$mg8|qC)2J!Yf3lkh{yzzG3pq7ndwOzO^(I@GT~b&qtVs z0zT4Y@n430C`e!qYVrBJ*-*ge&n!M4J{t=7Fq*~Zi)e*_FQpw1yh3zb_d5{RP{2p? zEI)iLuMj*iOuVl+c+^i&H$kxdEp-yMpQN1+)KEV`-2{OT4Dm&zn*kOt*Q9=ex(ULw zfTMsv0YpE6xGL-JT;T2e zgslLJNBcIwb^vuR*4@(2HD9TJp>QL0xBL_eHPFKD8APU-37?o zG@u@^#K~v2wOc}9JIUpjZsvaG?D^_3xZ8R+7W8t%vFxd<&2=B?H)&AUOSlj~eJ^#o zHotF%{T9IO0A35p3oUuaqP<{oFubRMo6rAF$DJ2b-UHy}nxt>>T{jyqFWt2MkHBt$ zH(*+K#+i>lUe)*_;P-%+02cpN*j@NIzL{3*PklCF0-y?D-50~Y46qWg+HpSvcEUP9 zi{pMi>=t-$tffOcVKbo9ao-91ZUAqMC0Km>eL36?IqrqZ-G&8o)EnPQ0Yj@+dZm!Aeoo)EXVJAEZ zc*b#O9p{<_f#pj$1hDx;`||)x$GQ*KWLU0I*y}sFPVo}pWk3RJ4VEs~A%+821CQ5T z+Hkz*awxdT!7su(MIjgkKLW1+ECe>*LNw@38Wyh*9qT?A-n=2`6!Lf7d2wMO;Dv@Z ze%@r53%tkhmH4~iXfFh-9r|3C;+hn})&s6d5x6Er;F=VHYfiTQS^i#v8*do2*8p1g z1l&phya$k1|5Zq0N11}-Xz$k z0A>Jc9QQe}&jS#jH?ds^;B9U;oW0$Qx4H4UGV4y>xMoJ+ni+v>W(2O85x8bX;F=kM zYi0z?Gx;NM&CAlY`De{S?#Ii2xO6O@HOVvA=m=b+BkT)-YjGAY*QDQ#j(?%_-2@!2 z#o6$*-|u`|cN?BJa@bor9)sPw)1C{wmc#lVhTQ@$G$9;u{JEZ&59Yz0x20G*C&SL0 zS}p?c$`tEPy9KWO<#XTWxHFBG9~-Y5j`jlpt{+|nI0WF@A>k_kOP}k6HGnyQWg(mk z^L)U20Tz#k;1>hzk#_6OTLhj3ya?c30oMH*tOxSE++e`7bOaut8w_~3j;HsY0`QC; z*BWg+gVDEsjKks&7LWB~d={TKh6DiK7Q(en>wW|5JPS7%@SGfhhvV`A56tlx-tz!U zcQAU^k2EcQKDyS8^p6K#Av&)6?-3W_WdP6Z+td9vetZ6%2X(n7Y~6WSy$FCKv2xIx z{yZ0M{l~&S0l)*vJh;39;D&zyb$JTvDeE!Ux(QtCCdgVh-LS^(gCK!+u5}Z*)=l7A zH-T4taJ`$CgmAr^cadAy_*-IaJ`%LkHGpsuxTe>1YUnb;956o76y;#5I4(@9J+$wJIbK@!WJrw>>{<(CA!ngihe;*7! z2miYP_W*dH!-nHYj$C-y@qY&PT;LfG8=mX^`QT-^^AyXg#VbT7*F8X7*!Up8LpUz* zl+GLgkKwq$12``5Fb;ux7cAeFt{cwUd8B7Byh8r2`vlN%d9??KEZtncTz{8;Yq#m> zbNI7b9F7V){4bOwJ`QR?N^OWkV#VbT7*Zn@kb$xQ#-9@!lXuaLj%{y+||PctELbxfJrb%m&|y49lp)MIrB~Xa{%_m zwjJ{fC-+}*?-k)$0QX<<_$v2bnbSok{0e-V)57MPZG(HS2rOTMIVx}bE#GwGUMm9k zS`pq0;C?Iav9kF>JNHuAZyTQbst8{LY|O;@OWd!7e%y0KxC+31S62gUcpe%W3V5i9 zz>`FS0Z$o?l&-}ejIQ-_>2rVBV96$GiaW5Ewd%kQuv~%wlfqTCQ-1|k~ z-Y)|8ei69$i@?2Kgad%w{O5kJ4+AWH>z`{{fA0S>`|3?SD1>J>+S_n%!h0Ob8;>pg z3I7jc$KceD9|rZ)=?wnw*wwjbdslB})wb@<-RHDrcJ*eqZQ2e4e|q}rc6M*~21V@c z*-{vxr!UjJbIbNUon3`-w(Q!mW7p2i?p-~7y_r+jpS9|&eM2I5?dv0IX4P43Yuhrb zcJ*{+)`sz9_Vo5-ws&vNT)bsVW>e3Ox}CdrcGY!l+T5LKs82Vh>m!Z?GaDmhHnU^X zmTlcTySzbAdN1o8l1(>F8KiS}_x7%y-puK%RuvA)%pWq(h-8SjwV$=HF|%&n>dZOq zr|)g>JQiGf8|Mx{>get2>4veVcXN6R_8WRzwt;VWqdsiewgvgtvG;X5@QtmTTXytz z_jVxH@Tonox39CaYb)&7-Q5A(rajyHI{Gf#-NoR=a05d3&OUsg4sY3JeM-YP?6*X; zy}P$BinVvkwoZf+kG}35UB*skH}BcH75M`P>>Q;7*>*oVwr^)pRO#Nn9Sa*fcJ*Cs zK11Y=EuG{(4DF;{+`#0vo~})uAt#qC#haa-FOJdaW)2deEeod5I#w2`($er8uQa zQaVV3QiDzg@R?e#by_DEkhbqv1gW(4lQ>`_5!4}q!;Ih$9=Xz^0zZ{DUW?(yjeF|J z$g3&zO2dnL`EXToZm6a3;`ToESmb3xodhrL_*1`%ylh<4;l)jW>SvLc4YdMZ+!v^x zjl68Ax$xpvL3KFtvZ2Pni@OKaBIa9_9uthwC9|1dV@}k;Y`u(GzET%651Zku_N28^ z<-RUO2EZ#LrJ;g_dJ<4nx>@^GTFpb&Ri)yh;l4!miLhv#tV<&HgY^3k`1uvKH0Q&a zTNl+27$m9ZqcF#?Ff&EIPO1krU2Vl64`YLhG-`ng)MqaD^*C};t;f~Dp&Eh5Xf^vf zl(HZEdpZ#quGSz5?ut||2v<_YR8Rw=62;dOjG{;(wvQeVYmuI+?xpJ#TxdR3eMMY@ zCZN;=>c8-ntRM&Ky;q_yPwL6wVLmfxsh$#B$G{q-r1H)WeB@+M18ysHIa$qANYvkm zqy8AkRwZ?Hg;tYrmKj&6PgP%}cct*~PnL&-_SKCCe4BlfOYx(6fW_dTh0x zUB|ypBJYS}8et;uWFo8dL?mZsS~oDA=jjx_#JW^h#OZ8c(_%WygNm7mewHqu$HZ3Z z6OjIjGtZPn|BV8t$)be%!~V25d@SZ|c)7sx_th5;_+hnUgXK0?^)HfPx-J-}gIT(6 zI{(gLUqG&x2h}t638?J~nIEVh7NIjyAAM^&NC_8D;jc;t&7eI!fIGfW(O3t+Z+uLw zAAc(ym3pcuZ5^V*SO7k_)mA+uDbfp2msO2kFav*6x)vP}N@$w>>jxE1v*ET}^`r@h z4h;;?nwbhRK}DUOhH9iLV4Vr8gsx>G*e_|nT92v=d!Q2#J$Lo0-xxBc@4Q%AD|)+8 zYTQ=hYqdLtMy^WJ>awPEda_=C&kDARR6>~+b0R2lZ?L+aWmTn5494n`TJ{tt&eP~- zrGI6fuCl#_3Kr4YWO`ZDVsx{5>eCs*_Cq7vATG$5ZaIH1!royAo#4_^ciGfj)F5KcaAIq3w2Jk7Sp z$7rP9lGas-qBf;Rr}bD?&U#&M>gYlmGr-76wjdTFlei&U9Wj|COfQ^ahP(Q(KQ_7Z zwDk9{CU-c_uuhfqA-#q;3J#gI0H7rhGHX4YL3s`!K?m`okK-Z+CImBJr06xg~$$7v-ITo(*019s1Ig1ew2NO zbX8SxyV0*ljNC7*?ldt{%t>GrdzbW1(mxC)0idb9KA^~xg506heSRui?m;Tl*&_B# z2$H(|Tyzx>za%XUdl4cSNSf$WJ?yB|u8;rt}2 zzG|`>BFYt%5t8bsF^?lj`X9oibExEYZ}mG6o|r^E!kh)=>id-#Bh{t89*J^QjI}9I@;5Xrv_+OflT~5UG_&5Sro(cay8XlflM~sRH$O zJVKvk5?$u&37~?(v9D=>ONuU>Hw2VRWV*(^>FTSwR4XxaQ~N-QELY6a#kD#;E&N+S zUK^8?n5!W^1&}C2)DPxxgwSfH&}PPPzq>j*kTw!;X;%sD2BD3=Rl}ma$kw;V(Imo5 zPwGVwNs4E4egyV1Rs<`XYK9`s>6{E%zj6beG_CPu$GEF!FW_H_PJL2| zu9iv^KGsQ~Ufh=d7n1Le!vx>QYY<{dr;#jua%@B z->y9KOww%9OUOWzE~bIDK?6AQiCG||lckt76II=HWy*+moc5_{Y4yj;{3MD30{EZF zawKR;zY~3rf&4{D@O-1dfILbjii4$66>c!%FQ=@-l7!Rf_Azx0C1KD}pH=?FLh3^? zsS!wVKI$M+a5P~$ym!1JB28exS1)=PYJ932el2sZN?4P6&H}xh9TT#kWMWfVP51RY z2ylHDS&#{sHEKM=s7p7V2>V!ghxswVWStH!umMrEP0nCKI}>55gLE)U+TRIb zsSSlHlUjDxrlpt+epg&Ghn2wtTgtt*r8tdl1Q{H*l;4IeeXcR3l8;HnA(3+ey}y{@`}Ss`rDZ2S4j0yQcaX_q@tRXN6gf3*iD{et~pf;zZgVA9wKNv?itX$4z zU#mZoedvrNH@+Q{8v*}KXgi*hLz2z#&>s5^IWo<&UIK zha==-IAesAghLy|ufP`#yGVpQU8p`+%*H?k{y04jDxb8@VBDSs4i;=hC1^MK22N%) zF_MwT2O{UO{}ru2Ow%Xa7-!8Z%<0cS%CQxtKc+UKjCRI8r10mE?5TM|7pj951M{8v zr=S%aSdm1wxjUc^`+|siXGPSWL40v*qIoOodZ<*0mnLDeb&aw1uUMRRVPbNFRF)cnyE2Y zHwr3lK?NH4hxhxUf%s=-H4u^1qtvI-=-4+YXerd4YNJH_tNkKtT>{N2ydYv|PDK=E zDQ5a^1PtVlaPj~+8K(L+oN+w^juK$}DU;otAV9o{Go*$BjG1c8L(+L*SlvBHcHalT z$*(%QKWDR>>VFFP;s^?nxHKij{5)$;g;~VI43h(uOt-6LEeDEs zDacawPAJZqI+J@1tP<1VQUxsu=Juv9r;e53)QdwTiJ?SQaYRcoKSu3M zA#IQppaYpSiF)vqu2Z|x=t;-3+^abU8}I8fy0QO9ji5TG6bh1=Y9CQ%anWNY20AFu zm_oQPVF-vx_ky%(HgmuP4|c0x2on`l_h9ITR%LXm8f=VCX}>;EOny1bMFaJetZ!9d zR!e@7;gi&vGQm+NQSUHIir{o6$w<|Cvc8-vw56u4Sy}$5V3s9saVmcl;u~VrHVpJeL;uadKCgRF~Cn5fbF*&3fGBfQR>YQymdMH zvI>kYzWN|`k^n{h4)afrM&+tvqb^lD*!Zxn9QLWTi}f^MRbu%tslI?t1UmpgfP>7< zY{6y`@i!u2V*f!Rpb{OU;#Bnx>PomIsnv{>fg4)k&;b@X0HnCO}UiGHf$wq5MNiXm208!Zd7KcaixsSc%p5h)ElS zXBvT2b~c1SKGk(>dIn*pK?v2l7_~8cu?bqu);l?Xj|C$&;v_)-BrYMEZfUC3W%bk?G zZA0A_cf@9u5aRko2!bSn2|#mGQz;oUjxqj4y%pzqn1iza@%4&8{U@7$)+7gFO_K#?%QO~!;a(9*K2$o>*8C9d9I2=`Hy7JcngZGc94-lh5 zjYSuY2$3^jv=IBR3)DXzFzkPatH-(QpBi$X;b?>sGl}>r@?@Ba_!2S-iIA1el{O9k zDjZ^(gL$klIciWQR|e|4A(Q`*k4dZ*&qo$x{a30(kZ!_q;IS(iR``>}dp=NYRY77v{oA!=x7=_Q?zh0Inc*}OPNNnM3PW+Kh}DiTv(lA@9XZXGm% z5Ln)Gr749MJoePJ_~uxt6uzk_&mIydB1IKug#=^iFqwRR$RtG^DqMIcjXEYH440?I z=?UDR)*y{gP4=;JWa`ya;1=Ot4fRww=Au#^k~WsVR#B$Q;rGA1aNT^qtpVTRAXdSh zPm(>?nC4Wdmd+2Cv`8%cA zf$I0e{QB{MJo$CINZd*FoTLY(KrIg{8X^eP-@=nU$B#z9?Z+**R4z!a^^@BQ!|wfq zWE%6dbMS4dKfVW6fAp77yi8V)P~e|fR1E87%o-*lT~tCv+cm7K zPy$g{{d-gnQKnEf)JWKO-BfdR0Evd!DvAF32oilS@|dk@l}+>APMSY4c$!xQ>LFW1 zcO6I%Y%`CCiTr%tL<-LjVz+Mz((`S$o+@ZCD`twAM>(C-3}%I*{$6qdDaV*14b2v~ zE%E!q#NRpG#0TneL}B~&i8lQY#WnPX!IM8RP=~`JzcpV4g=(s(6iNFj_%#~1q}pk% zTx>p>YBB6)(r8*pp!z$MJS**ohnoa6v7e#RX3zM3AO(m@GC7>QfOa}iSU`l^1oip- zHpd=P=z^2s$Zpn!F_;9Ypk4ysAu}HYo)##q6GoE#I-653#}U0>aH%W|+uwg0+NiRq znCAEgXop*cL>~(|J(D)9bJUmi_I{!#F{6=z5LKO>tE@hA!pTi8aN~>`I|6Vlgi0zM z1(xjvg#w3LoFxP|CrNZ)k?2g8o5U9Cys>AAb(yXElUL;Fi+-eh(^KCLI>YT9zn=(u z$F*Q+E_=trCjB z7K+hKqb@_nWq}#hDE144{V|q1RG3tXfV|L6y}cNdI)q%NOPR=|dOC}ZQgt)f@S)od zW(X&rc(t4$6Qsf{gP^axU;XcjT*0%8d33ZnNAxhIgR(B1=h=m|A*X9{ybdjIi22}v zIoqpPpmU~$o~}gQ<0Wh62vHJs?hn$zvsYS*s)+pyfhdmr^G(0Rl@8v*?y~6cXn{a}7Q=+XIgc9;$R zfJRR}B?B7w?=(TD^HH?Z;-FEMf4IJn9m14UtI$HRzY<*XU=Nxe*6r0feludIP>oRe zL+SQw&sXEvO-<&wpcOVKBVG)dhfVj!;ZEIzj!dB=J935HvG{+1i#CgJkUD-%y`M-O zXnX2fnX{nNImUsW9fP_a+>XH!8Q2$wn_ZSxO_4~ynic7b5-ECK3NT-TS5p0!DXpxM z9-h4i7ELHymNEmm29%`U5J{bZg@_8-UTRZ2z#JQ>6qA0D?4G6FET%1#y-d^`-iau| z@4p^Mu~RBV?m?)=0D`qZ=?r<8hL`M_V>w1gYNqDM#47OexTzi|qRT}Es8r8ifpI5L z5IjMq0p2R2Zp_Cq{%7h-f9#|Z8EKK!= z6v*99a_C;UfH@hPP?#%RGgjXLA%so>gKi)nsBgo~$mO%t?O+rt%qN5~R19E#UU)}| z0Tp^tDe?D9i3f^ny;Bj#*akh7`!P+`{RTrgRo6(OQYVZg3SzSY^=X;pjZ%+z>2R2V zi)fZ{7Kf`(SY&KOBCAm&u&s456_uk^vy|}YCaEU1h$yO>rj{5+e-twMkt@molex!7W-s<<|))?fo7;;O(tSB;&%MuE=N$H`n;&r<{vDb=V^8{L51jG8Da&FOhiy|vGUu~AjdG+1HBxyi3ri4Kh3nbW(~YYGPHpKp_6C7%mqH)Ou@uMp2RlK( z!#!mGYESG#r%9#gp3S{x#2zrdA~})H6zbXNC_9$oVqGaesC?ZkXXx!$@9r)hE61l* zo5I@D)g`CjVcouK%bSgVZ{H?vz=7MIojCK(!|JemSjY<-I1g@XoHoQ3Y*9mrs2=$~ zV||lS&PMc)e7WU@Ve|R@A5;de6`9Qh>`<}Foz5X}E084ed~yF{=$S*E_o;HukAUYExvGodj*Q@|K{03M^*~ zuhtcm`u_J|T~~%prC{}w=Gxl*;9qPbHj5i3&fQxp(VxHQk3e2BHgSegqJ}G@eysR_) zcx3n-gp0)rk}im}Mz075OvymvXvH%SQ(@(XXvU-XNY_P3}>Y>v53OEw$&0+k*;i9 zT2E}yr63>^WSNtcG%=^SrI<5@8spZS?Z)C5zj%?Jxez;$amW>$I5GX>jwkHDL`}s8 zETR}rI(gy`2kkH$>P!a<+4*t?n~`11ZZC77()8){g(UT*6o{mw}r|;XRBfEtRiEbrCnyOz+2eW1JWB{w416v!9@seVt_vAhnHhW zM(J{=a0$(FbB_l~Sw1nGH=2=wy$|nkm+{OrC2niOiJCFoVn2~b;;2r-sgKgIa+G2p z2KC+=O^XU{f1TN0Oj5iGUUB_4i&Q{T*!pGX&u@}&SnSC%K}XxJj#tU64`WaS9cef- zsm&P01E*6=15E1)=YhvY41_!}JX7sROB))+t%iCMkYw9jWj)l6Gc>kwW5$d1ms;)# z)~MLH2Z>83P+07Q!XoO4I~noajCdj%X|l;E$e1+VW-sUFC27tqhu7Py2ZdLRtitUy zH1}Fvjvk2z$DxqgiC%ykquGR^L+k<%lQ@JorP$1{VI4e;*JV)RprcA-L-tQN!X+!9 zqN5Y`YO<4njumNvz9SVE9xv5bKXY>maaXg?8J)^BU<3FMNNJL&-{ANqzo`ax zcJZ_)7_Vdx2W2m`BR4`*=fjRet77Mtd#O?E7+2cLrGwRAQGFb`7>(_5DX5yps1Nch zV*SU7bOXA>B>UEK95RQ>b4rsQy+Bt$&lQ}4aFsaqO+JEUkUW`PzAT<113w2Q=*1zE zQ6k$XqJH4p2&|>3ixf^-x2Xu?Dbl3+Ba}Hh38@yOK;#fKA#7<>drC_yRZm=zu19G- zCy2JR{uZ4oGmBli=*z|gOYw<3KEZE95vor?*#de{m)SUtxeReWu^;(@i-?Hu4`GDi z_D2N5h8KzO-%Nx+E0f!aw0dZNnhITfQ$ZF+4U=t5twee8sxc8G`0RrEAQ>{60F0Wb zsciA=DHw5ehAMdNFVtmp3Z}`Z!euJvmygyneLm5oyUwjY-Wk4{OJz_wMkB-S+cV{5rP zym)u_E_{bKI_}1ukU3Z7#3Sdn9;)6Tz?dNvrl+P!Lnz(G#fIgq)iY_M%r`4?;!psG z>EaVBs3d%Ygdej_mNMAh*nfm3hoowHs0p~j5LVK`G6seN?qtltHYmp9tb7O&IfTrM z5K)MGXW{W@nBJHX8H*{Hn4=YQoMNwcB1|l?oS4#+F&@^()d&I;R=i*c&+VW{?4@8DWy#CatGu^h_ga%c7q~ zzdIRu1TlUxMA}UFcNPW@ET!SFKL``vL15?7|^I7Qivx_B795N&6j5sceI9Ol9uCQyqpAtu_{9$ZMts6M;ap> zM7PH9s2ayItX7!gf+G zn@q{KRI`6f_*l%zX?ll^*uxp~#A>k1c^}!v-Bqj)6RLH})Ge;@O`fUeRO{LJo6(SF zDEY9|AgUqLOIbq3VTv?{(8074(7`<{RkQ2$Y+RB!L(i?z(-(kG7@*LIk(0r(f7jfX z7*E!ZT5jIl$#?phYs4)Y+dJG|21PKQHcB?;?r+G=41E z>_EQ}k033#<;Dfhi+<5ajq?!odm#~hL5H@@%$q~KW zEj4CWPWD7RYsU@p&pDJg9I+-_Kry0?IRy{t#gi59`&qg#p|oUk@66B|9-BhHf6tZJ zQDMd{HD;sCCmuG~98Od})X%)Q_s85R&KivCnW!Y4s;pMod)Vp*pEX>H_8$*&Kvy^JJySImHIE) z#Ega%qrY zGO+EZm+Q_|>Rq{hzPhT?{xA0dQv9@NB(!VaChKLsB=(Ty>Q8Az4;RP`-M6D$LYlNbE> z>KE9~Ito*^3sFmz_wR)}_QGu)E6qK9dv=Ex=I88OE3BX$u8P;&($m*4O!aNKSPFV<_4x~dwqUsB!gu-xR{$&#*@rE00Z*2@j$9?tsiQWt92vU2o1Fg~EBSxi1#a z9=ObjY-D65^9}+!lbw6Jxl={ffw`~q0XB%gV`DNmSj?)`qna?8!J04nz2>TNrBqSIS_(jxe~KULqO(QxRa7M_2c$NUev-BO%bckG|hetnBY@) z>g(tdCj^r*VY{qJPdEwt#4v-YQazYtC}{mws_mg|8g--?M0TPdM&svI?-19DF=e7W zUY*v{agiMsPZ8H-8AIQUlSG{T@v^?msj~TSHl~zD135-rjT;c5gH5wD5dqO$N6N{h zT-e9=5}-JSW`L6f*f>&mQX3nNxJ&|H=~JJVe3U)pdM@lQ0Zbw zo;{y`se9&nm&Rdp&R~!;8?l~@sZMc4b8}ERAEOxTl?$aHxy6>{fzp0mlfxm+O|&N()uAuKi7&Vlv+y^sK~upz*>b^5H^G9rXk;HU=M11UMoosg01fJCPKc)1k!lTlIPNdh!rt_(ppkCw|cv0ka*Vkt~pi|I}Qn8~L? zM4^^b>BS=u=3CU}_^!uYIyXq2ix1~8|Fvc2OUJV{`GhaI7GCT|+2SP3){`bQId_4*Y!qy7es=+$S-Oo@bq?-7b^$2{f0J%mgpkSUUm=n21 zR3*3Io`4qDN7Nc9Ua*Y@3mc}-F%=Jm0#{NVuO1Bxhd#_n=F3UylW40`!Be?<{4$n6 z#^jAw{{Zi)Y&kW{(ETNKX04u5r!yHng%L>luuzUSX9BgAoh6&$#bH}g$&F#pfAN{F zm&m7bXDweERUa$?CpdhnK1tSa))rBgL&Jf3^<8xPqR-*sx7pi;wZzn zTGg#=bG$xlyxix4e3Rv-1-gtK4!aq0CGR7o^qdqU70Fzq!v3E1 zsB}7_=Jiype3MXZAwT1~)afbRMBQh-41JZ=u$r#^Jsj;`)!neKN4*G%eIO<_f(=!i zcwsPp6Bnoa$w=mcz9zH*A1dMn;Xd?5$TmK0t8T#pEV6A5P#4uv8CW_8tAH3M2b8sG zf{-f|L?LdMrm7lZ5Kk(wOB=kn*x|8(qqho#O8s6wnzEF#>p4;&~} zX{|xkSq3F;N}t)FQ_V(9U=`r=yb8CqhE)KaceSabtON>TPgwl8w$d$Yv$_k0*t-xF zeUeB`jnYq5K)T^L%3&9#^zAs)I->H0A|iXoT=AG_zZ_3)gmq1bgFk2?}Nk^@NZCW*ksP)UWT4C)HEkx^S88%#Y2+Kf+8#8HQ;8GBAOca?W zin@>$`N>KeIwNr(XMRBlF~cm@Nk~FLr88RXg(M%!_PC3GA|gN+jR+?$)$4eNG(^m; z8}$iIxT_!X{A`e3g4&hh-*Pl82=F*U%ZF9s*uIQqdWup^vE&#ot`nBnqmec2_@v1y zz86~$vByAl;6tI_VlnJ|CIVtcX^;%~jtj;M95DrbS*{631q8(^0fO>2KE1TCm6=wJ zDM*#a=wWp$BmGA7_DY=uO%|2oNqOODawiy zVb9LWMww_7qc`9x6jphFIffId#}a3Z31>W^84j%jd$?V*~vdj!%fI9}o0n zxLfJ>+yK9?iz4i$5h=oAHBI&m{?#kL3ThS{RYZZ;C(R68^acag(W!)v*3qHmcl6U( zRrhow@`ul?bIYPKJ7dN-V`~c=X1!E52&ou%Ahk?H6+qpmWR19 zHOQD6VvFLqMm>?!l16E-{xY*>K}kxcOkDP0FFHl~vWd_PAe8*|YfJ>QEb37c0c$M< zVC>GsOkP%xxos#T8n+LCYJtV70vfkLF9T*-|F3@SFMn)96ioIR&H>u5_y#N1-sp;A z`M@p^D4xXXZ0q1|5oeJ`Hb5S~XGP=3kgz2Mr?w0O)REaF$j*^m{qz~A$b9x$Jrve? zbE4pS)aOu$kz@GWGLNH#vY-M%^o&5gj0bUa#cW+&%Sl8>%a{kpOzH~KJ>yD9f2{krk<*U>hQe7-wSs^ zGxZ$Y@y~av)j@oiAQ+1&X!2ndv~$k-aXQ1VH7%DDGrno9-e>V~6pOF-kPS=#C?az( z$K-&$3L7>Jxtk{x1Hm79s$QV_j9NWFq}~m zMfx>xEO3mPsV$VG5Zuoxz3I<#hS5wT$5)l!^bt_GnOEYyDuKa?O?@V_De@a7Re9p*hL;tefOndX9phP!U=fEj$d)P zEjK%EzB0TDBz!}ZIXvuqK+)MZMD<#*$v4#1UF0n+ws#Dh&$WU2M#$&gvS7Iu-zni6 z-{}E-x5u)rS^bLyNXa0UL|9*jml{q{Ay+?&UIH3Jh~jAc(1x=R@yJUt>ONYMnY-0S zQnfPLwXk4Dly!mnF``0qFonDXxyP61)iWmdpb?UEeA|@yFzWr1K)rMT3%9KJSNqM) zIXN~es1UEeL5qW8M7<5tGu84&qC7907hq43H1=?6`A8i5@N*FpecY^ueDl>DY3UtM zQQ)(J_vW!`hmR>kIBtT}jVqKmqd`Lfjy2IgKsjT0J5y`ZS^d*1;Rclc2}va>(-kGv ztudXCnn59NUxMXgx3MHxFTU?f-9$zC<^liAyu7DOfpToRW&sNH)6&1#^8A$PZFrig zz}R#cQMhAMLvQe-GrDkki-`zzW$d!C zC!5t8VZ#xg2{nX5cntq z{~-)K(q2*6dAnp-&_s6FNXqI!@yn>e0|zM7o&iIY3`*4^b7N9B$oM3^mODQ29hJP} zQ*?vVM_3eLM>hr5VGU2X{qZEz9iN$}H|7fr?%*^lB0IeHCu38SGc<8pu!33kYcR_? zfI^URcm)!}praH;6B(s`%v>>@*Bc}52Ww}*Uls7RbLpSn#k&UGb_W{V0AmL0&WPf$ z^8?)wHu{(GjZ-HEzXTF=Vy9=_-QQ9hCvIi-n zx#J9q)a~v4J2DNM<^_ml}!u(@-{QWLbXY)n1*z8=TriX)8&R}(cWWximgbh%- z8Fj`Tto|jYGgu=Ue6YgnA%hN9e9>@88s&L2lEYp?t~@gRJhYcsGoeJvx<$1#dqwoM zv%*!7St7MU%Rr?c*Z4-kFly>hmXMWMYDdiwUoU&ow2%8`M$I%DbIzTl_xKXRhrI{y z;vHVQti$y}xD-8%<8>kt z%9{-x*l|{wP;L#yNR!UwC!{sU4sjgW@38jZtf0C-kB+z?n&2 z%YykmiyI3qSSVq=TaKchVyXg{1NPDF@vKR`*^IzdlZ=^in)MT`0-^)L5aKIrVlch@ zBv8196rL6eQn0e$p^_C*aR2nyW`j#*+YQ8hAuBesJ%Y5+`GGbr=P~%xfG-u*Jm7A` zy2&c!4)tBo-@d=epw9sMEhPV5bX`)fam%GhH0(?T_unmUL^_mDSs$M>xV$ilY+Mgq z1xaWQa#?wEz)696HoVJKr2f1{Hk1v2BXO)QmQ%f%b?a7V!sAc{^}P&pTpni0jodj{ zym{BAo=&sVa^$<@qw`czGYJn&1+rHR4Mq-H8RZ<#SVg1`*Gaw^Sr`urS0A_Vrj~4yK(>gS{v*NtUDI$ zVw@$*tliVnba<9lR(VD=@;K|3cG!u=A+#Ny2kzQ|79=-aWZ!eq-OD#lP&6JkIYKK_ zkAlT@u4pNJ%|UVj_8_65B=IT)wM|K^*UyGofvr{iu58Bo#td38PJEW*1RfEQn*K&H zF&xQSxY?1w^I@_xAy73~4G}ywui0#|NdiYXmrMe$GlJ#UCI$*uZYN+_Gu1ZA{p4$lO54mp_CPoNx<+TS`?JJB_<}mZ{|h~KT!qocD-Cy zz{8RS#M@yE+BpsXH8lPWDTfvsB!}u6OurW%e<+9?Qy(Bche!3&bx z&i12gK!*|zr|3zt4)mdLL$q)jiGkR>M|({J8Zxw(P^vn^1Jzg4rg9aA3NF@KnXuc{ zm>Q{u{3D8K>|o_y>Jrc>ce>@M>1AMDXy1I!#I zGtaz6m?>)nGd$_~TF99tWac`V82r!InNBkE_rqnTJ{-%W0<+eJ6_{05Nj1)?z_Kkh zLR9jWd3F{5@4*;WLz26i*pMD#rpU|E!)ZuSrbr9M{d;k~WHoG_tl*9U8u?TGTErMu z&IMfRN+Rc0EJV)9-Jje5&KK>2rDyVIXT5T{6KB1mk@__&9#vf91am2W7L!V&Z(uxC z-k}kXs>f$PSMIPAkI%1Uq#86#_5eFPRTzgPGw8*q1UX8%niNb&vInkkz%z&XFyPU* zx2vZcM&xF+LUKc#c=Ku-pn6im2U)_eW3gf8<@2>KOuLk<4C569o~fA=G^*F97eSOh z$(CgTW%)itt-Py8T$b+C$kCg<*5*c86P#g{bymrYd?u7F$b>~{1Q||*I0Nbk6>`Y) z>{FmW?& zIp^=ru1mQH0oY=1HOOGT=4CeT>E7Pi+1+C-nUx#XLe+1u>lURO()Hno32M82G}oRj zyDzi)knl!rmMMgN95-Vt#U^`blA%Jo%D)QT>8{N?clC8|z0B+A>Dtuk_3Z1|+P!sG zM{oD#U6{MHZ|hv$^_D$dd%8~DzI`p;3+Ub3xyTqxjIoOMnz!*HhxJ`sy1Msvt@S!O zw(i-vrK7{^?c5_iw4Af~lCCX%Yj<|{nQ*VGulKrkcVIf(UQs_O<7(@j7Fe{hckk@lvJ1DhUF>yS+yw`YOl_C-uEQe*r}gaGv9@i!;kS>Zg=mPeVJ9ClAWvPp z>-myFS4STh+JlSVmcYiC)||WM?DmdRSFc{brmf9ey>?AY`^MAiJ&q3@dra`YeOq?+ zdEJ{OQmRA-R`YEXHCmlZD$HYZ5!HJ)||b1&1%Hd*U`OGJXf`^0|Q~%ILUXC(AIwH zx^)OeTBNyaPaks6?i-Hc?%5X>CiphlW3zD2&fe~ecXoBAwrtzfV+bQ1Yu2wn2id!& zZ@ag8)oG`Dr?28)o1M&+^9a}bg9V>V3+U}jf zaA&Mqb;had!*K*>{5!y92MS(Fu5AyVrP$ed27;|+3_HWBRlmwRqh)_ds2aoX7%&hj?*bnfo!k(#j= zuU>4~Oj0tsk*jUHu(Z2n+fJ5M&!(NdJG%S&NcoIa?MxxccXV9d`udHhF%Pm@n5{x4 z?nU*s9hv44w!$sjqDmOG8gKKS?Qdo!mvpkwQ2|g|r=NPpO4b*oYqz&^&yEf_OK$gi zr=PtcwW{HDsfPM=W4b<7w`0rhJxig$?d#fCw__2$w{G9HsjqI+=I*7a$DMQl!q(k; z7uEIduIp@A440j|>iV|r*|E6}<=547>8_qP!%fOKTfDZ_8#>mnId%1U=*8K8ZQj(| z)zZ_=EVVtM>A=yvPRC_1tVTW3F`rIW4{_+i{p1Ed2i`TLW-2uXx zE|psvJW~x>grLcOX#1}J*V)-XNm`Y6{+%xv$Dtim3>xCLGJ3!RLsw7FOp~CQ?&|L8 zmY$iiyXS)uQ&d+~PZ!hORa8~aOdC9bbwDs^FpCB^x{)kNbPq^?y6aAh-_JGu?7xvv5lxk9+y>3snJ+{2CFuXiBS8Za1OR!pbk$;^Ih%Egk?Bx|5ZP--N_ojuW%MO5yt)n`>>Yq&~% zd3M3XG}BxeTUc(*Pb8NuF10U{O8YqpU2VBhTd22-b95&k*sLC}U%FUZs$JTubQpG) z$Boq}Ky$g#o(WdAT1ha#&g#X3a7Qf=PhmL~mWJB{`zH2H%t*ENFSl1Q%OgxKI$D?x zb5h4~jrKBUvu4~DTSsZB5verRs%zMG`Zl*vIx1^y?JTJwby?4t|5oXERb!Ec1smRd zyhEj9)#Zj8-E43enV60gg5G8WVUd+{yEr^FJzAKWkT8lE95(|~W!-$`z`p$cePa`Q z4oqRF)nuV{J5iztV-sWhC7vJ~uQ5ncb2-E>nK8qO3eUZ=;uW zi2}EQwO3}iJ4gE9q-7%(BKNV&sx?!d`O;Elq1u%CFo%=|xnAAXhlv{XymF;n?96RR zsjeu#YW35mZ6``Kn}H~EnV9i?Xs+T~U1>bcEv62imeg@M+UG03J=nv%f7m{d=0NjL zGx21?YLgS#FE9;?ts?B3ifU+?c4h@NU&8*@=jPB_n-q*{9x~jlmn!8F$<<*dd^>k_ zy?UeSrxqis2l~Zp3tkeKui%c!rHY};5s~9vZ z%FYyH#~^dU2_~hkEBl2@C21@!Y1Cm%KA!BM7|lm!^UNv6>dnPcJE@!)nHil@FK^Z7 z#IRShKC8<-vkL1GwsY2!Ek%3=${6>iX1-~NDldbv+|pbq#KUefIrbBu#E$S{f~0hn z>OyHn@oZEoqAfL6BrHs-i)sc4Q6-rfw7o%EZn;4h9%?lLbYlcU>g<|ym%*~vct;7%_4RV z&=ag`#O_#h zs;n?>YN}tJAE{5JR(fb&TL?C>qZgLjC&PObmFn!W##u~i!!3zxOY9c2R;|w0mMRwB ztkq?DqZPx!8OvNt&FaZon+<_#vREQ#TNEY7YaAY_sU!{5Yy}QGMKr66brf^Hc66S_ zDX&BMv7||lqv?rAr)rlhVRF2!8YXRIW?KeUw~1DP8xon4F+bO^QEM{fomOk*#MD;r zi&#C?SADirxoF~Ooq+#&#q`W~(nzTF*=rjj8V`JTWKO8WFfAdRQRR7KSqlPxY<=19 zK9CDD+p3JU)piwmg)%KdrZIzS*EM0VZ5D!}@vA z22m#)tH?xasJU3JOsL#0b_k&OJ$2LTX@{nyc&1W{57`1Nt<#~3+nEa0#Ymd`RG#`Q zY_@G93X5J^s+Eh?hdZ4esgiTR` z;!GrBrV+S}Mn06}xb}Ulr0JEG8IXKoBG0IA;`cU#MVESdm?$EXmDcEz%yBY3Im#4- z_!c#z$g)E5@2>?JUe98UHQO<$R_zy0=iyDLfg*P7P_v%AE?H}q$? z8$Fy?8TqU1%UIJaMe~0RyKZ9d;#VDC3{UmuN^wprh$5z&+3zu1KGQm`HWLN*@|tbd zs{~n=D&HlqB>DY&3w`!CXMg+c@Ajm|%qyBaP(k&QnOd2Bq@qu)iX=sgnRYq`r#zjD z5xiIomRQj=Yn(bbI?|Uu<C^V4yn@wb z1g^K(;SDVuvs64$N+zcdNUGj4xL7Bc1bAsML1;BK0J$EhM&DDlJ*ef@&{9QPqK99K zY2a*wvda>?t9JI=V?~zZED20;q`%E^H?L($d}yX0{yRb^mf4kQirhTnFdo#`ep zOePL2akzSf)7JO< zj?NOLp%W}|-1dZv>q?NpNsIHjV2;^XG4lcS;EWo?YAIek3*MAr5@l&zCVnGtAGXwI zJ5z14T9!iA7b=~IPDv%4TG=J_ET6f4)e)Wr* zkdlgw)aK@*mT^NiJ(${h`{GN}bkADI;gNb*J*`u>rCFwSdUEl6CDX;E@heeQ7pz>Q zZcRsQ8gir_x8vxNjRC1REUj8y3v8NaXhCiinbj5!jV438Z>OoMrVwntNlmpCHKDPe zEyp@HJ~M7JdY2eSmSkQ8Bkmd6=sG3KSxT*nV22vM7$^UqJV+nc?}^|n3}0sZZ8lVz zgC)s^oQEov!JW2PzBmVoM{AGVxO(Cy2O4nz;r^Rh>vX zg)%$CvSTVMt-7C#%4pa4fkjfBYt&^Cii>O-ohaLY!LkUef>@v~m6}H}d~EVm1_qO2 zmDWGXR0KB@x=u6oCB+jQ&s|@f^+|y(!>tgoFv}(>``5)HyM!g|y_H6sv^ZPr6}55J zwlzet*k<3PTE=;?P{(2ywMr$KYm=AsHpFG&`t9Pfd2Gx(SDqChpp{7z>M`cl2q%9nC;~E1Xi(&?&R`rN=F}Xi~WpRd_swY(WQX2zhzBHBL8&VH@y7g;uhw+(`k$VwEA%=NS=78Q$Os?FYURT5NfmRnRr)&V;p_N~VdR|OXrb`F%BZlQGMOtwO- zODvfJx(wh4rfaQ=mUkwVj#ip_NC%s;KaCyg0oJxw*Ou$tQXy3t$-@=9ddHIEcxdP! zax1oyuoc6UEkU&^)sw7=`xyw;wmjy-qosoP8jG#Y%w93*XD+ryVH(%*u*EScQ9aQW zTx>0)KDG9;E5s#A*gI31{T^C=RD(5M%lkyN>EJ!0n&3r@uwB{l(5jSv+pR> zXZWYG+KwBItDWYE=WTL5Qk_eN3d1?vx>F+ei|G8)3>vJO z+`bLnrcOK0l`zm1wOzA%G`BMu9?4H7g~`c@{%JNV$$*@c0pIiV}aiPa-1Avn&;n;fQPVT+G*LU(EWv(D@U(#gykzrpHr_bhT1C8y+;O z(j#gZZJ&3!#BSQK^PMgs{F;pWYUP?xto1yLA=3oM$J9@y3$F3)vcz&Sd}X0ObQG#s zTyB@y*krl@VS6A%T$&S8#DS?~WG`=|)an|w7!9<2JI6rA5IS40w-sjxWOfv*h!V6@ zku__!v5$aVQ)WHcwZM9ok1;+Y3^}r+TtfHQg;0o7WGk}CwBdxMoMR@W)q|^%7>L=w z9P2QXFAneDKU3U4HkRb`dr^J;W&4A_+mpi7=#<--PSfS{hp_~ug#tS*1^n82zoJ#O ze343hi9r_5bA3~Z85C`~6jfa&5Jy>Kik5TbYRNCFgyV_GnDwRVg3*yT^Fx*>ZoF5~ z61b}b^+A@(2I=qTg{IMwDVmr+Lz7c`ho`6A1A@m3RBO!B1NprS%lW3?uIc1MkVN8( zW=4bBCWQk9@=E10HZ(osa-K@^lwc*(Y{g=!%8MYy<%LS^I1QZa-9JJ#hQ`>q;HB0{ z%mS;!R7m2N<}D0O77GWa$9Z{Yvbb-*zAQr;dWrdg6Ww4Yo?`JvgtDN;h*n#&iLB&e z^APVMbMnzEvZADG88CqphZBTg{Dz zN60TNBtNuocrqy*no3Q@p##uqf{9_8wuHB*RFRbTYPTL1Q8@l0yP0X}fGf~DeYx$l zxL7MU>#dyw)|f+zl9n~j_L9|dDmgTP(VI9lvZpvzWnSj05e@<<%KuL9EwxwtucO@HD$<4eN$5K)_IN}}%?(YQqNI6FvD~wE|VH5rN?Y!mZ!mx$? z#c4*p(&7#ulAod#c#G*|!`i}j6dZ|^{b@?B?j3N!YOzCC9w<0;NX=wSO^;0M_X(#9 z^d7%uwx?OgWa4C+;=L6Hohkg!7C+);r%z`E9!$x(ZdOMR&rDoi41AAkgLbTtmyOZO zoUE|qgM!bU#gU=B%jCe&)Qo4F*OiK6(-G$d=KIsWFj5T`sS@A0?Jnui82Jw`x5msn z=W3WAc3Y&(!!S>3uO{z;XoLw;-E|uCK0Ei?M}gO_m~!|v8R8_n>gl7JkcX$( zBS6ZYnPPF@!HIlPRyx)fy^P5otG4(ZpGs0>5^Z&|jAo|x6mb|>X1L4PJME3ow*VI4=3irgs(-En-7uV`Gh8fAaCDX zmOfuNFfo$sJvcViXMb}NbgC|z=8P58gCp)euOp)xJBF=otRGmNy=X6Y+*zk+x$RcE zA9`hyHlw0e_F0{5@G4%b*l0DJ;9{}*cAIPP+J{Rc7uKDp}qEs1b;0~BxD@?n9|=x5`p}3MS5!gIX}&Oo zE{#k3%`!~qM_n4z+Z1U9YejjKuZGfaKmJxalG;#do2JEF@zqKW9 zN8Ecdp4}*C^Ch3TNpUm!&eHjfKMHz0XYWfRNxd<{L`*qawN(wbt&-M8H0h9=ehI05 z-DFoy#|$wVPEI6po7hSe9L#u#1WlJ+mSUuY-daZJF zy9Mm9ul#sT3-95(t|t%sY3wks=d~9(*@o;*xtErq9XiOKgu09|<)8IhVe0&I)Rx0V z_aYv}S+Lo(?wHsVA5hZvvh6=uTum(e+W_KLGVr;G;v9tL_O)v8B7|OmP6}i8rG;%H zDNLk*thC9Er78?rAWCg{)^TA}D@`orf;<6Mt-^>!`Ds>D;jp>7Hkz5K*pk+k*Onu% zf0UXFD?a-@mVIH=eU00fOM#1!siY-oEH_}QG}oL@EFWil+X`7ov`f@X3d5Gv%C^c_ z#9;Bg-BLH%&lEL&iQ*-6^^T!p;Pi&d$sc_R&8c+-+y=9$4c@UMw%f>;vy#Jo+7oId zBg1?4=H0AG`#2LLdG>hkY7;W3RdfrKa~ zQD=~}aLwDkrDO2&d@_AVyGk!vC>>s0qq*~0C~~CgTCTJZy2}ADW!;+tnE_P6#A58Ey*Ndv*ikKm zrCr=p%~p}ES!@^zCqG+^lx0*R)fjODmhSaMzP|CcM%sV%YQpLS{(^TTUqkE_J z7GAo5JHLdt)|TvCi|6+iD6rg=W+c;B$-R)u05(eaTH%3rdYWt{ip8+uX<^1uT3Pot z7ipYhCk^)lWJ++dUG*={o7H|Oi@VOf@-JGylwWwg zlwa*CyUL|YrKP%l=X>c=eW`k>HY_hQ?k)?zK%+65m9C%oF0$dywwnDqSXU#z$cGbs zuJ74=j$M^ZP;z@uvayHn^9A^P0=%2=p`I&%+|-=n z=Q8vkW#}7U(Vcz^I8J{-hQ2jJ?*ns^LvjKC^%(#U^F7qFQM|ru(<2Y|B*QOH`8k=% z?*_1b*c{TI0w1>Yy{FdSwC-l|sPt^<)aME4{m_GKcucP0KeBo6`cvy}B7U+}fqb@X zNj6@=cSJj~>5(r!_+O2qBzd*PN|H(9y~g4h&q=QjuMmETg{OH>yW|~yZueybejWzD z-O`-zxGDE+4*Z;dZnAN~Xs?V$69Ma5y-w00Ayl>6E|K`(#I46Hh9Ob2a^?4oty^dKs{-p4}D8A~e z_#dMz7oOJPz$RA-HAkB7!^C?%c--l<$y({ZOZY1Z53)>Nm2AA9@U(vDVxZlBUb68& z5I2@{s88iJ$;OjLbNaVhTb@rNK0!RXf|W1)U2rd0`3P_RnPlT2&&cJy>Ice4;Uk3i z@*Hpltp27j(Qg2|ezMu=EIK*0sb{l&+qYA=!zhkK)C$3*mKvQp*Ko54ptol&)0xm`5m5>Hmk4CbEe?+KJHfN z!gqpC@*LTd!`?nIqPqGj-j>%T8`~~k7y}u1{Wy2PJ%}I0)n}0Zu^m$X6YUiFEbu(s zbN;EbZrX73`uDB71%0S(dgLKzrpMekp5^^sDJmbSrRo-N=$irmB>JXI`X3kn8F-EJ ze-5m^9?E|{gMS1L`DOWUb6oQJpG`InkiM7y!h_&Ro+H^0^_+a?rk*WBFP9!D&MD$} z-Ae7P!aqUy3~?lL;pe~=m)8f^ki~jhuh-KKUyy7ZceK!U2je-0((4oYTgO{P&*KnRQv)h_8qLl^cJ^S8X^cdnq2W zGPqM6j*oZ{J>d8Atab=^E6;J?690jwctKu@7s9u$Ns_Ui-nh@Ly?#^A`Re2${d4f? z@^*8CQ0Do)PEUt>{Mb#tAx#CEp5f<%YxeZ44{1&k?)28y6O&;*5%>vXX26GC9;t4H z{0UCt@g&IQ4)PCuG34=5aaQhI&QU4O)+zshRhNM8^z{i?@k4)6{ClB`7Sn0^RdKfb z+e!Zg()arK!cT(LXC(*Wi{9Y+dpyQu^Z6R_t|wlQgYXR*d-YJ;El<~>!FSITyW~_o6fpC*RarpDK@2V9iki{Sz7d065Ii75`E2yNREV`isUt(I(%VZ2TC{nelJ!co_en z1(`fe+}kXlj!Z&6ZLWep1ubK9mNq@2Vbs~#0q81I*TyfP6vI7zr(5T}n|ihmrv^G* zM=PB!#8$h7dg~JG80+He80)g@fMZ=!`Zk?KmpxzN^Apymm;Zq-9QstCtDQxU^OJlY zB%&_0-Ny4!Bxl7HZ40+VYK|1J?vD+;iLU!$d!dKCbfarOIMy-6S6qGc9PDby^LEn6 zUY;pl$XoY){=B8{w4>4~?`NRh$@3Y;&NOdblIZH%S-&a${e-KWz?1GF47w`1t`?O2 z47})Kz@N6Bx*6}y3q5$X+6&Y>uw~fpJLr~6zs9$>o%i}5uL{*#=4Fu#-b~4{m+(PcvX_<+8D`M`YNnj zUOvgQ*O|3fYS8DSb`V`xwMaibeLTzwMAPjo!G>qcdI0(@&?iZ&_7K+97#H!J_iJ*M zp>W+Jp+bEbwoY4(|1D&ss|ju)uIR$LE#NtiV;+?EZsTPLekPjE z=spu|bN`uXI>Gu(>2#pUef~TDSGCP+kM^klNUkAn#o~7CNK8ANp=}1o z{0jewGIh42`ocw@j? z8KFBVb_>P3FAa1nHtAruDCuDJUe-*zLiv8OY@sS&wd;J=B}{Ij&pO)~=>)7xUeN(O zt+SD*6SOWlw*!oh(TG0l*occi>#R3+Le?ecc?r6(^~tj@dX8O1p{j2N`EnMhmG~_$ zZ{eTmWQTO0eNpkBj_7X$&#iUxQ2dL*?FjdQPephq_|6FbBKY?sEW7*15xx@qUm`pX zek8)G&sQ_}@d(dA51+vQ=5-x@rON{mUJQ}>4)J$Kbd`5|gnwm~_;*C~UxWT(uylPb z|0KV=Bl>THe+R5S6zC5`^gks0b71i)y2{_OzQe!r>y5DJ_eWUtM(B0{-yLDmAAebgE*$z>lAN79;xT3s?K?a92H!LENA+}J#TVv_kHd!} z{9Qr~lR)t|^L_Zj?)1V+9nxPN;me30KB0Y|jQITp=ud$si4^4ByQxzj$@7v7p2^_( z2&+CfL|Ex>iLmJRMp*O*GWZJ_{BQ<85n<)`T!fY1k0LDk1uyTePj3eA&fr3Xm3|?@ z%D)j|(N9KL^y?!m`bC?&%O8la=!FO?{Y-?d{0Lk55f=US2#bD427fYxAI;z=Gx(VZ zEC2R6-Ta@7u;`~EtnzP*u;>p*So9|%EP5r<*GcqISR-5LAM+8t1YP(dPrnv?JD9ID zzA%uF>rcNP;gduZ*2t>-Zw7Bk`{P;GAD?Hew&e7P-+(SGyN*9M`7}6u(*BgG4->#N zz3RI;!m95;ghd~Wu;^DuSoCWmeE%xtS0cL7-xlF7tP)>-Xrl31n>+^nLtvE??C~ce z`u~Q07kGC>e=?%~ALvhkHPZ;`pN{DN4E;M`%_IU{zH6X`5SaWRlU}~;Kz}P!j2ANL z<=YN)P1;`uj?0&?JJ5Tep9_x5m+w2!Uj_Z=GU??D5A@eUe^VyCeB*)sdgwW@^flB^ zzVbl75c&@Aa734{JkYOzz5yEK}-7%IAJI3-?$5{UA7|S;uWBHF`EI)CKvPL|EzNw~p!8M_6?Ev19tZ z5f)uO?3n&cghiL%I;J!EO!2QFj~_sf=cDpn$MO3jto-Dkj_G$qDpkpouz1b=71qRX!wWBH3?EZ=LuNq`tn@cTSakVUWBPX^{Eb!kk&iRPS9! zer7!EP1k#8C9|X#RsxNeb@0LtH@^t>|L&+iUk_b)tHpQi{ZK@|8M<&x|87K=JfF+p z^^`|2pS4>1C&CI)`M(d|49bYPuU;*EM85#~U0|)ZgZ_OsqDx=o;|}@t6aPMN++UuE=!!4jcZe^3&d2v0 zu;}uE2Q2#G2#YQscuZf2u;}uM2fDC);sGnaPefRB`Nm`Va}gF@zVVp;AU@`RMVD_p zVCDZzgg3LrcKnj=^^~xS@A}Wl2#+E^VYfayJDCB0GLv37PJd^lFLmMzyYy?5>%rj@ z^g}*!7v}W))%a-xR(<3v57^`%VbSF;4|HMq$zyy+gl}D?eEFe6eC2;ngq7dD5f=S{ z2>(YM4xh@%LzrlM&Q9)FrMxdh>6PCT5mtFmMp*O<@E7yN$Mv6!A}snsgzs9VJo&FJ zxlb?ub&Tb|4p{kpHp1dZ{_H^ipxTg9NC5D!qK#f&NA4KL@_v(^cLb z5nb``jPTzR|D)j0-$j?dn=d|2UmlFG=<<6T+0$>hu!AlC2&??vTf6Dw87%*HoL>W9 zbim3_zU>&xw;f~owqq>cc8ulQj{Rt8~w6%O&0{-Mae945Z-R*UcS;kyxi7P>Io3^qO;1BXwrKk}iwu(e5Z z6}o)lfv)$}yJzIN%#rk&pbufo}Sd!SWNwboq&6EWdETx2}@E{J??!A?UvYmTxfF z7x{$)-Sqde4i-Q13kSNee8B-L{_~N({3iJe|A=tSXFjtEKk^mF=^u`;@|W*8(1qnQ z4)}pp@{^x9(8bSf5mtKnjbnNx*TG*}B|rI*u!pF=*f|H}Vu;IF^Jy-y>&2OO7I0KbR*2c`uUzYKns^>e_- zz#CZaYd=`&m%(4zYzFf+>BO_(?;$^}FO}a5;41BRfyXZ=qgY<& zfiI^0dp-Rv;Mbn(-j7%Mw}KyIeAN3~!u!D;dA%%o7nt!+$&)R~rsNnne8kU5a9rNM z&fwd?A3(n~UseA94*ZUnclfz46aVwz&rzQs?=OSD&VFp@4}Swbmxc`b`uE_+sE_vd zl>a}051@~syp6o#5%YI$2EPvc;msX+z5%@ERqpps%I`Apt&CUNFB1NRO!^V<3gd0y z?;YUp!EdjR|1R)b(dU30;9wuOBpZ_VgWp5@g!cP4;G3w=h)@6T!Qb)vsPaDpj_Y$j z_(tr7#(&YjkfHy12IIR=&Osl8z4Zu27$sCx^gs{-q2)20n-J^%9@H4PJrY(0@*W zpYr`hzaM}flPOZ;Dvp^t$-%6k2?zPuUmj%fU!1HTo#+tVAF^zR0rgFN~@ z{a3-~Vt>x}_#@!^=x=(LSnYFnCjFe_upl&@a-py zjBla-SAaiEd=+5!34Ag6&v<+U9Oqxj;3e?Kqx8R=q2t?6K8HMm{O$su!+b92W ze=&pq0-Wsb*o&vYA4i`<|NCwx{tv-l!CtgI|1W0fW;mhgqxqPAqhIam*p~~zE7 zLzg^uh)(;{%?+bFiPlFqfHfXo1^y%SKlGj_dmm;7hRwA^i`*kD=dNSXYamv+&P+h4p7>k6v(`{w?4? zC4K0RBN_T$@UBQ-%HRq1uS5Slk%@mJ_;Tz;kjMMM*D?PM`TtvRr#;pupU$NJL-41N z$L(GoUjn}Y{TlK38{kpaH@zNzAKY0FoRz!)=DdQmKb=QIT+DbJ+T$V~zP-lTKg|!` z3XbKm2mBTE?Qx&~RT=s$_^)WcFdte1cj~htxfT3T^z)=oe+T$0^ygr&KL&mQeP9`H z?ep8}Q1j_SFpi32^veYLag{+^aCz=YItMyl+pfkI&+_ z1~1V5!QNa5USYn$G~M`nD>%IGB7Hgp{to_u+dO?I^N$}y{dpexP0@ViSHS;-KS1vt zDgCYBr|6&CeEQ!2e;9d&@$zobu{S|~?gPj2`M(%Sy+5;?^s={~&(I$NKNgJ_f03d8 zbq0SkgTDvvtjC^x@t?tZAK*?JQ02X7^k7fcC$GW~#r$26!EXZJgnbY8?y?Mh5PSfB z!~ARn{8z*e{q>z-^vgb457aa1Pl}$-A2%c)1mB2$gz`R;iT_*RFEYRQB+1ks_kizX zJPZBvi{LlVe|kOr8=3UH>t+6b>6XTu?`P=GgU_YE@n!k1)ej_8IX>e5wcwKM2kT#z ze;N2)#P5eLJPwZK`Hl>JX9g$7?EuWywtBL)l5FL0P)^F@+O%f7uXuddBEgHTg?hb_ zG~8Fk0GB8cWQQ)++TmGO2HCSakG0fV^f?sY&AnDNZi^Ty6o)xuU#E&!o7>8bMl#Ex z_sx2{Ual_~E*_c8^{1C8R_oL)cF49OGbl4z2U&a28y6 zAK?~!oLg{?OkU0fA^vF3;*le{J{|J!7`S#c*Y7TN@F#_yPTx`CKq_~msyp4SwcOAN zV1d`cGRMAEIQ_3MKkqo*;d3l;*?_Kg5DNoy6ySI%aem!0r>h#`77dHTNw3r=r<38} zHIQ>V7t71(r8Qi+$}#D7sh$;HZkFv^$IQ9=S#*!BKWS}rwHv7Gvs{W-;{-pL3S&zD9fa}Y+_ocEd0dj(Pvs$ilx$!oRohxnY<5WwZqfXQ3 zI$E8;d3>?W1u+YrneNMw36Jh!j_)&0uWGfc?XnkzLbcl(L~wY<(mbr%U)l+&+XT0Ph$anj+ON`EVlsj!{3*tzVcbGwW?dC;l3 zzbnUq^d^&TXdNfqmf}cV*RuEvOu`}ml-qTlvp;Xx>y$r@ZkI0tL_U_CQyYhO9p4)#wxep}<8Hf%onwQuXB2~d+onhU`a^3~lg|05LUy=mIL5Pc za_4GCYP#rN3IgtQ!uT{uLXn|pozn}`V*&kXkKx$I(eS`gl-a((agkU$XZFN;*Lle< zLz#wYSq6VXp(FXk-^36S`?~HNO!n=aNCi?n?uK%^2}j4ua`1NLB)4{8 z<*g|#Ph-_zcODCRI#e?!OUhZn*+Y_PYn}NTCj+;k~wJ0g|EfodH2(5aeJiNN0c+3s=(JTo$8f@sV7=dR3TrEEepF_UOuJ-OpSsTBACq zk;>_va3C-<7R*bo&sx{SM_OwYeT;l3Q91f?=T^@4I?dWsY&taeD z>Z#^vN~6n?>9TO$(99!Sj6T%?{UQG-2X`~L!{Xzvm@%nbR-{`v zbi()|y(D`ZmuGh%ZJ)uxBph7qyRh9CT8yj8g(<3PY^AJMQnfM{txv~{*>ucb%i)jTcG)3Q_pn2KPIHqmY3RE0$i<=PnA5I%?i?Yy z4Q--xXPZlnHF6KR?cvF^C!BC?cN+K@jCx{OhZ?)%?9Bv>)Tg)#a=uzw=0-k3{5|$Q z%`WQ?>cOK+l?E3)gbT3knrs;k3~To|Gl=%Ge$jHKT|atsp~`~8=5OA9V2mU47C2IW zMgetI0)vyUxCR2R+r`;dE7NF06y&E4aU|C>ymLEU^<&H^R1yX7>SlGucp}VKkxfK&WYUr=W2%V`l zk;GTtNvVn6mC9`}$bqqwylj;6-Jb!ccx0#gTz%CrLLOXb=_OxQZX1vRb<967#Mq-%(8?kS1}O;VKcfUpJS5b3#9q{un}%_ zafDq#vT%gN^8UKts%vGlie2}z=u&z!oYEUJD0znTcGb`0s+9@pRrY=hUvzX&j!9Nm zmgusXmgkZ?Rf}*p?b1hZM4~t3bmk>y^=*2H9G}h zUJ*58s8gqNbacu!JqFbVjtPw!MY}fJt61MaN08ByGsm!qWR4bCg!Mb-k(xtwW_#)VHGahAeq=3FCt3woJBua}nS@%Cl~L4_ zYz>_TsD*YH#R(F zImt|X)dCv(ZIb4D#wyqGcv+=Q!jj(wKu*^I!GVfBCZ&FaCu5j`%n~Dc&{23*{=myDwdvT>Vw0a-Z-t2MqD_ z-rnG@waG()DQF?yb^f`}CFdQiwaJ)&)|cYxzuq%MCJnyqb2IVes|ufx-}?y&&!XK= zyl?O8goLM$Wa9nSb?cHFqk==cyEE~6?_ZbPGOwK7C&c>zJ{L5S%{SCCw zN6!l1@|N|zTp8AriW;)8ZK9k2c=>6^VqkQz?HIc4(fAE0~ M$umZAe?q+f2L~9JDgXcg literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/revo-mini-sd_bl.hex b/Tools/bootloaders/revo-mini-sd_bl.hex new file mode 100644 index 00000000000..1e27d0ad396 --- /dev/null +++ b/Tools/bootloaders/revo-mini-sd_bl.hex @@ -0,0 +1,886 @@ +:020000040800F2 +:1000000000060020E1010008610E0008E10D000873 +:10001000390E0008E10D00080D0E0008E30100088C +:10002000E3010008E3010008E3010008C120000823 +:10003000E3010008E3010008E3010008E301000810 +:10004000E3010008E3010008E3010008E301000800 +:10005000E3010008E301000889310008B531000818 +:10006000E13100080D32000839320008E3010008D0 +:10007000E3010008E3010008E3010008E3010008D0 +:10008000E3010008E3010008E3010008E3010008C0 +:10009000E3010008E3010008E301000865320008FD +:1000A000E3010008E3010008E3010008E3010008A0 +:1000B000FD2F0008E3010008E3010008E301000848 +:1000C000E3010008E3010008E3010008E301000880 +:1000D000E3010008E3010008E3010008E301000870 +:1000E000CD320008E3010008E3010008E301000845 +:1000F000E3010008E3010008E3010008E301000850 +:10010000E3010008E3010008E3010008E30100083F +:10011000E3010008E3010008E3010008E30100082F +:10012000E3010008E3010008E3010008E30100081F +:10013000E3010008E3010008E3010008E30100080F +:10014000E3010008E3010008E3010008F9270008C3 +:10015000E3010008E3010008E3010008E3010008EF +:10016000E3010008E3010008E3010008E3010008DF +:10017000E3010008E3010008E3010008E3010008CF +:10018000E3010008E3010008E3010008E3010008BF +:10019000E3010008E3010008E3010008E3010008AF +:1001A000E3010008E3010008E3010008E30100089F +:1001B000E3010008E3010008E3010008E30100088F +:1001C000E3010008E3010008E3010008E30100087F +:1001D000E3010008E3010008E3010008E30100086F +:1001E00002E000F000F8FEE772B63A4880F30888B3 +:1001F000394880F3098839484EF60851CEF200019B +:10020000086040F20000CCF200004EF63471CEF2ED +:1002100000010860BFF34F8FBFF36F8F40F2000003 +:10022000C0F2F0004EF68851CEF200010860BFF334 +:100230004F8FBFF36F8F4FF00000E1EE100A4EF6C4 +:100240003C71CEF200010860062080F31488BFF3F1 +:100250006F8F01F0B5FF01F091FF02F033FE4FF018 +:1002600055301F491B4A91423CBF41F8040BFAE745 +:100270001C49194A91423CBF41F8040BFAE71A495C +:100280001A4A1B4B9A423EBF51F8040B42F8040B2A +:10029000F8E700201749184A91423CBF41F8040B87 +:1002A000FAE701F06FFF02F061FE144C144DAC420E +:1002B00003DA54F8041B8847F9E700F041F8114CC1 +:1002C000114DAC4203DA54F8041B8847F9E701F0FA +:1002D00057BF000000060020002200200000000898 +:1002E0000000002000060020F83600080022002050 +:1002F0004022002040220020A42D0020E001000820 +:10030000E0010008E0010008E00100082DE9F04FDD +:100310002DED108AC1F80CD0C3689D46BDEC108A43 +:10032000BDE8F08F002383F311882846A047002002 +:1003300001F0E8FAFEE701F07DFA00DFFEE70000D9 +:1003400038B501F08FFE054601F0B4FE0446D8B979 +:100350000F4B9D421AD001339D4218BF044641F213 +:10036000883504BF01240025002001F085FE0CB172 +:1003700000F056F800F00AFD00F086FB284600F079 +:10038000A7F800F04DF8F9E70025EDE70546EBE7A3 +:10039000010007B008B500F049FBA0F12003584266 +:1003A000584108BD07B541F21203022101A8ADF87A +:1003B000043000F059FB03B05DF804FB10B52023B6 +:1003C00083F311881248C3680BB101F021FB114A75 +:1003D0000F4800234FF47A7101F0DEFA002383F313 +:1003E00011880D4C236813B12368013B23606368B7 +:1003F00013B16368013B6360084B1B7833B96368D2 +:1004000023B9022000F034FC3223636010BD00BF2A +:1004100040220020BD0300085C230020542200205D +:10042000254B264A10B51C461968013142D00433C9 +:100430009342F9D16268224B9A423BD9214B9B6887 +:1004400003F1006303F580339A4233D2002000F0B9 +:1004500035FB1D4B0220187000F0FCFB1B4B1A6C87 +:1004600000221A64196E1A66196E596C5A64596E14 +:100470005A665A6E5A6942F080025A615A6922F0ED +:1004800080025A615A691A6942F000521A611A6967 +:1004900022F000521A611B6972B64FF0E02320214E +:1004A000C3F8084DD4E9003281F311889D4683F3E7 +:1004B0000888104710BD00BF000001082000010897 +:1004C000FFFF0008002200205422002000380240D4 +:1004D0002DE9F04F93B0A94B00902022FF210AA8EC +:1004E0009D6800F0FDFBA64A1378A3B9A548012139 +:1004F000C3601170202383F31188C3680BB101F02E +:1005000087FAA14A9F4800234FF47A7101F044FA18 +:10051000002383F31188009B9C4A03B113609C491C +:10052000009C00230B70536098469B461E469A46DB +:10053000012000F08FFB24B1944B1B68002B00F0CE +:100540001682002000F072FA0390039B002BF2DB6E +:10055000012000F06FFB039B213B162BE8D801A282 +:1005600052F823F0C1050008E90500087D060008DF +:1005700031050008310500083105000811070008A1 +:10058000E3080008FD0700085F0800088708000866 +:10059000AD08000831050008BF0800083105000853 +:1005A00031090008610600083105000875090008D6 +:1005B000CD05000861060008310500085F08000845 +:1005C0000220FFF7E7FE002840F0FB81009B02219C +:1005D000B8F1000F08BF1C4605A841F21233ADF870 +:1005E000143000F041FAA3E74FF47A7000F01EFADD +:1005F000071EEBDB0220FFF7CDFE0028E6D0013F0F +:10060000052F00F2E081DFE807F0030A0D10133632 +:1006100005230593042105A800F026FA17E05748A2 +:100620000421F9E75B480421F6E75B480421F3E77E +:100630004FF01C09484600F043FA09F104090590FF +:10064000042105A800F010FAB9F12C0FF2D1012015 +:1006500000FA07F747EA0B0B5FFA8BFB4FF0000A33 +:1006600000F08AFB26B10BF00B030B2B08BF002414 +:10067000FFF798FE5CE749480421CDE7002EA5D09E +:100680000BF00B030B2BA1D10220FFF783FE0746D3 +:1006900000289BD001203E4E00F010FA022030705E +:1006A00000F0D8FA4FF000085FFA88F9484600F0E9 +:1006B00015FA044690B1484600F020FA08F1010806 +:1006C0000028F1D1B846044641F21213022105A8D0 +:1006D000ADF814303E4600F0C7F929E701230220A7 +:1006E000337000F0A7FA2546244B9B68AB4207D92C +:1006F000284600F0E5F9013040F068810435F3E761 +:10070000234B00251D70214BB8465D603E46A7E790 +:10071000002E3FF45BAF0BF00B030B2B7FF456AFB7 +:100720001B4B0220187000F095FA322000F07EF981 +:10073000B0F10009FFF64AAF19F003077FF446AFA6 +:100740000E4A926809EB050393423FF63FAFB9F5B5 +:10075000807F3FF73BAF124B0193B94522DD4FF449 +:100760007A7000F063F90390039A002AFFF62EAF27 +:10077000019B039A03F8012B0137EDE700220020CB +:100780005823002040220020BD0300085C230020E5 +:100790005422002004220020082200200C220020E5 +:1007A00058220020C820FFF7F5FD074600283FF437 +:1007B0000DAF1F2D11D8C5F120024A450AAB25F017 +:1007C000030028BF4A4683490192184400F062FAA8 +:1007D000019A8048FF2100F083FA4FEAA9037D497E +:1007E0000193C9F38702284600F082FA06460028E2 +:1007F0003FF46AAF019B05EB830531E70220FFF769 +:10080000C9FD00283FF4E2AE00F0A2F900283FF451 +:10081000DDAE0027B946704B9B68BB4218D91F2F2D +:1008200011D80A9B01330ED027F0030312AA1344F8 +:1008300053F8203C05934846042205A900F012FB1A +:1008400004378146E7E7384600F03AF90590F2E7C9 +:10085000CDF81490042105A800F006F900E7002364 +:10086000642104A8049300F0F5F800287FF4AEAEEC +:100870000220FFF78FFD00283FF4A8AE049800F097 +:100880004FF90590E6E70023642104A8049300F0E3 +:10089000E1F800287FF49AAE0220FFF77BFD0028E4 +:1008A0003FF494AE049800F04BF9EAE70220FFF71A +:1008B00071FD00283FF48AAE00F05AF9E1E702200A +:1008C000FFF768FD00283FF481AE05A9142000F071 +:1008D00055F904210746049004A800F0C5F83946EC +:1008E000B9E7322000F0A2F8071EFFF66FAEBB0793 +:1008F0007FF46CAE384A926807EB0A0393423FF6E6 +:1009000065AE0220FFF746FD00283FF45FAE27F0FA +:1009100003075744BA453FF4A3AE504600F0D0F861 +:100920000421059005A800F09FF80AF1040AF1E7F8 +:100930004FF47A70FFF72EFD00283FF447AE00F029 +:1009400007F9002844D00A9B01330BD008220AA9DA +:10095000002000F0CDF900283AD02022FF210AA87B +:1009600000F0BEF9FFF71EFD1C4800F0D1FF13B0E8 +:10097000BDE8F08F002E3FF429AE0BF00B030B2BDC +:100980007FF424AE0023642105A8059300F062F8EB +:10099000074600287FF41AAE0220FFF7FBFC8146D1 +:1009A00000283FF413AEFFF7FDFC41F2883000F061 +:1009B000AFFF059800F012FA4E4600F0DDF93C4614 +:1009C000B6E506464CE64FF0000AFFE5B8467BE682 +:1009D000374679E65822002000220020A086010038 +:1009E00070B50F4B1B780133DBB2012B0C4611D8CD +:1009F0000C4D29684FF47A730E6AA2FB033201464C +:100A000022462846B047844204D1074B00221A7080 +:100A1000012070BD4FF4FA7000F07AFF0020F8E773 +:100A200010220020502500209023002007B500232D +:100A3000024601210DF107008DF80730FFF7D0FFC6 +:100A400020B19DF8070003B05DF804FB4FF0FF30C4 +:100A5000F9E700000A4608B50421FFF7C1FF80F05E +:100A60000100C0B2404208BD30B4054C2368DD69C6 +:100A7000044B0A46AC460146204630BC604700BFE6 +:100A800050250020A086010070B501F0F1F9094E53 +:100A9000094D3080002428683388834208D901F04A +:100AA000E1F92B6804440133B4F5803F2B60F2D3A5 +:100AB00070BD00BF922300206423002001F086BA9D +:100AC00000F1006000F580300068704700F10060C0 +:100AD000920000F5803001F011BA0000054B1A6851 +:100AE000054B1B889B1A834202D9104401F0BAB906 +:100AF00000207047642300209223002038B5074D62 +:100B000004462868204401F0B5F928B92868204433 +:100B1000BDE8384001F0C6B938BD00BF64230020ED +:100B200010F003030AD1B0F5047F05D200F10050A4 +:100B3000A0F51040D0F80038184670470023FBE7B6 +:100B400000F10050A0F51040D0F8100A70470000E6 +:100B5000064991F8243033B10023086A81F8243023 +:100B60000822FFF7B3BF0120704700BF68230020B1 +:100B7000014B1868704700BF002004E070B52A4B95 +:100B80001B68C3F30B0204461B0C62B140F2134016 +:100B9000824230D040F2194082422ED040F22140B1 +:100BA00082422CD10322214D0C2000FB02525568B9 +:100BB00042F20102934224D0B3F5805F23D041F288 +:100BC0000102934221D041F2030293421FD041F22D +:100BD0000702934214BF3F233123013C0C44013DE3 +:100BE0000A46A24215D215F9016F501C9EB100F8B9 +:100BF000016C0246F5E70122D5E70222D3E70C4D4E +:100C0000D6E73323E9E74123E7E75A23E5E759230A +:100C1000E3E7104605E02C258442157001D9901CAD +:100C20005370401A70BD00BF002004E0F833000884 +:100C3000CC330008022804D1054B4FF400129A610E +:100C400070470128FCD1024B4FF48012F7E700BF38 +:100C500000040240022803D1044B20229A6170470D +:100C60000128FCD1014B1022F8E700BF000402402C +:100C7000022805D1064A536983F020035361704767 +:100C80000128FCD1024A536983F01003F6E700BF44 +:100C90000004024010B50023934203D0CC5CC4543E +:100CA0000133F9E710BD000010B5013810F9013F1C +:100CB0003BB191F900409C4203D11AB10131013A94 +:100CC000F4E71AB191F90020981A10BD1046FCE71C +:100CD00003460246D01A12F9011B0029FAD17047C7 +:100CE00002440346934202D003F8011BFAE770471F +:100CF0002DE9F8431F4D144695F8242007468846F1 +:100D000052BBDFF870909CB395F824302BB92022A9 +:100D1000FF2148462F62FFF7E3FF95F82400C0F15A +:100D20000802A24228BF2246D6B24146920005EBF5 +:100D30008000FFF7AFFF95F82430A41B1E44F6B2E5 +:100D4000082E17449044E4B285F82460DBD1FFF705 +:100D5000FFFE0028D7D108E02B6A03EB8203834211 +:100D6000CFD0FFF7F5FE0028CBD10020BDE8F883F7 +:100D70000120FBE768230020024B1A78024B1A700F +:100D8000704700BF902300201022002010B50F4CA8 +:100D90000F4800F0F7F821460D4800F01FF92468CD +:100DA0000C48626DD2F8043843F00203C2F80438EC +:100DB00000F0AEFD0849204600F016FA626DD2F848 +:100DC000043823F00203C2F8043810BDEC340008E4 +:100DD0005025002040420F00F43400087047000006 +:100DE00000B59BB0EFF3098168226846FFF752FF18 +:100DF000EFF30583044B9A6BDA6A9A6A9A6A9A6AE5 +:100E00009A6A9A6A9B6AFEE700ED00E000B59BB023 +:100E1000EFF3098168226846FFF73CFFEFF3058393 +:100E2000044B9A6B9A6A9A6A9A6A9A6A9A6A9B6A55 +:100E3000FEE700BF00ED00E000B59BB0EFF30981D5 +:100E400068226846FFF726FFEFF30583034B5A6BD2 +:100E50009A6A9A6A9A6A9A6A9B6AFEE700ED00E0CB +:100E6000FEE7000030B5094D0A4491420DD011F85B +:100E7000013B5840082340F30004013B2C4013F091 +:100E8000FF0384EA5000F6D1EFE730BD2083B8EDD0 +:100E9000026843681143016003B1184770470000BE +:100EA00013B5446BD4F894341A681178042915D119 +:100EB000217C022912D11979128901238B40134216 +:100EC0000CD101A904F14C0001F002FFD4F89444C4 +:100ED000019B21790246206800F0CEF902B010BDD6 +:100EE000143001F085BE00004FF0FF33143001F0E4 +:100EF0007FBE00004C3001F057BF00004FF0FF33C1 +:100F00004C3001F051BF0000143001F053BE00001E +:100F10004FF0FF31143001F04DBE00004C3001F0B5 +:100F200023BF00004FF0FF324C3001F01DBF000026 +:100F30000020704710B5D0F894341A68117804294D +:100F4000044617D1017C022914D159795289012311 +:100F50008B4013420ED1143001F0E6FD024648B139 +:100F6000D4F894444FF4807361792068BDE8104050 +:100F700000F070B910BD0000406BFFF7DBBF000050 +:100F8000704700007FB5124B036000234360C0E947 +:100F90000233012502260F4B05740446029001938B +:100FA00000F18402294600964FF48073143001F05A +:100FB00097FD094B0294CDE9006304F523724FF4C9 +:100FC0008073294604F14C0001F05EFE04B070BD50 +:100FD00028340008790F0008A10E00080B682022B1 +:100FE00082F311880A7903EB820290614A7909320F +:100FF00043F822008A7912B103EB8203986102233D +:101000000374C0F89414002383F311887047000020 +:1010100038B5037F044613B190F85430ABB9201DA6 +:1010200001250221FFF734FF04F1140025776FF04A +:10103000010100F095FC84F8545004F14C006FF06D +:101040000101BDE8384000F08BBC38BD10B501216E +:1010500004460430FFF71CFF0023237784F8543044 +:1010600010BD000038B504460025143001F050FDD5 +:1010700004F14C00257701F01FFE201D84F8545028 +:101080000121FFF705FF2046BDE83840FFF752BFBA +:1010900090F8443003F06003202B07D190F84520EE +:1010A000212A4FF0000303D81F2A06D800207047DA +:1010B000222AFBD1C0E90E3303E0034A82630722F0 +:1010C000C2630364012070471122002037B5D0F8B5 +:1010D00094341A681178042904461AD1017C022933 +:1010E00017D11979128901238B40134211D100F1D4 +:1010F0004C05284601F0A0FE58B101A9284601F090 +:10110000E7FDD4F89444019B21790246206800F061 +:10111000B3F803B030BD0000F0B500EB810385B03B +:101120009E6904460D46FEB1202383F3118804EB2B +:101130008507301D0821FFF7ABFEFB685B691B6864 +:1011400006F14C001BB1019001F0D0FD019803A9FC +:1011500001F0BEFD024648B1039B2946204600F03F +:101160008BF8002383F3118805B0F0BDFB685A6942 +:101170001268002AF5D01B8A013B1340F1D104F11B +:101180004402EAE7093138B550F82140DCB12023A8 +:1011900083F31188D4F894241368527903EB820303 +:1011A000DB689B695D6845B104216018FFF770FE3C +:1011B000294604F1140001F0C1FC2046FFF7BAFEF5 +:1011C000002383F3118838BD7047000001F06EB82A +:1011D000012303700023C0E90133C361836203620A +:1011E000C36243620363704738B50446202383F328 +:1011F00011880025C0E90355C0E90555416001F09B +:1012000065F80223237085F3118838BD70B500EBB3 +:10121000810305465069DA600E46144618B1102263 +:101220000021FFF75DFDA06918B110220021FFF732 +:1012300057FD31462846BDE8704001F011B9000065 +:10124000826802F0011282600022C0E904228261F9 +:1012500001F092B9F0B400EB81044789E4680125FC +:10126000A4698D403D43458123600023A2606360F3 +:10127000F0BC01F0ADB90000F0B400EB81040789C7 +:10128000E468012564698D403D43058123600023A6 +:10129000A2606360F0BC01F027BA000070B50223C1 +:1012A000002504460370C0E90255C0E90455C56431 +:1012B000856180F8345001F06FF863681B6823B1D2 +:1012C00029462046BDE87040184770BD037880F875 +:1012D00050300523037043681B6810B504460BB1FA +:1012E000042198470023A36010BD000090F850200F +:1012F000436802701B680BB1052118477047000056 +:1013000070B590F83430044613B1002380F83430BF +:1013100004F14402204601F051F963689B68B3B9B7 +:1013200094F8443013F0600535D00021204601F0D8 +:10133000A3FB0021204601F095FB63681B6813B1F5 +:10134000062120469847062384F8343070BD204695 +:1013500098470028E4D0B4F84A30E26B9A4288BF3C +:10136000E36394F94430E56B002B4FF0200380F2E7 +:101370000381002D00F0F280092284F8342083F3E9 +:1013800011880021D4E90E232046FFF775FF0023C2 +:1013900083F31188DAE794F8452003F07F0343EAEA +:1013A000022340F20232934200F0C58021D8B3F507 +:1013B000807F48D00DD8012B3FD0022B00F09380C6 +:1013C000002BB2D104F14C02A2630222E263236437 +:1013D000C1E7B3F5817F00F09B80B3F5407FA4D1D6 +:1013E00094F84630012BA0D1B4F84C3043F00203FE +:1013F00032E0B3F5006F4DD017D8B3F5A06F31D000 +:10140000A3F5C063012B90D8636894F846205E680A +:1014100094F84710B4F848302046B047002884D0EC +:101420004368A3630368E3631AE0B3F5106F36D033 +:1014300040F6024293427FF478AF5C4BA3630223F1 +:10144000E3630023C3E794F84630012B7FF46DAFCC +:10145000B4F84C3023F00203C4E90E55A4F84C3024 +:10146000256478E7B4F84430B3F5A06F0ED194F852 +:10147000463084F84E30204600F0E6FF63681B6873 +:1014800013B1012120469847032323700023C4E9A8 +:101490000E339CE704F14F03A3630123C3E72378D2 +:1014A000042B10D1202383F311882046FFF7C8FEB8 +:1014B00085F311880321636884F84F5021701B68FD +:1014C0000BB12046984794F84630002BDED084F8C4 +:1014D0004F300423237063681B68002BD6D0022191 +:1014E00020469847D2E794F848301D0603F00F01D4 +:1014F00020460AD501F054F8012804D002287FF4D0 +:1015000014AF2B4B9AE72B4B98E701F03BF8F3E72E +:1015100094F84630002B7FF408AF94F8483013F06D +:101520000F01B3D01A06204602D501F0B9FAADE793 +:1015300001F0ACFAAAE794F84630002B7FF4F5AE40 +:1015400094F8483013F00F01A0D01B06204602D5B6 +:1015500001F092FA9AE701F085FA97E7142284F8ED +:10156000342083F311882B462A4629462046FFF76C +:1015700071FE85F31188E9E65DB1152284F8342007 +:1015800083F311880021D4E90E232046FFF762FE81 +:10159000FDE60B2284F8342083F311882B462A467B +:1015A00029462046FFF768FEE3E700BF58340008ED +:1015B000503400085434000838B590F834300446EC +:1015C000002B3ED0063BDAB20F2A34D80F2B32D88C +:1015D000DFE803F037313108223231313131313136 +:1015E00031313737C56BB0F84A309D4214D2C368E9 +:1015F0001B8AB5FBF3F203FB12556DB9202383F36D +:1016000011882B462A462946FFF736FE85F31188B6 +:101610000A2384F834300EE0142384F83430202375 +:1016200083F3118800231A4619462046FFF712FE5D +:10163000002383F3118838BD036C03B1984700235E +:10164000E7E70021204601F017FA0021204601F0CB +:1016500009FA63681B6813B10621204698470623E0 +:10166000D7E7000010B590F83430142B044629D089 +:1016700017D8062B05D001D81BB110BD093B022B92 +:10168000FBD80021204601F0F7F90021204601F0A7 +:10169000E9F963681B6813B10621204698470623C1 +:1016A00019E0152BE9D10B2380F83430202383F384 +:1016B000118800231A461946FFF7DEFD002383F345 +:1016C0001188DAE7C3689B695B68002BD5D1036C8E +:1016D00003B19847002384F83430CEE7024B002250 +:1016E000C3E900339A6070479423002000230374F9 +:1016F0008268054B1B6899689142FBD25A68036067 +:1017000042601060586070479423002008B5202381 +:1017100083F31188037C032B05D0042B0DD02BB948 +:1017200083F3118808BD436900221A604FF0FF332C +:101730004361FFF7DBFF0023F2E7D0E900321360DB +:101740005A60F3E7002303748268054B1B689968AD +:101750009142FBD85A68036042601060586070473D +:1017600094230020054B19690874186802681A60F0 +:101770005360186101230374FEF7C8BD9423002051 +:1017800030B54B1C0B4D87B0044610D02B690A4A6C +:1017900001A800F001F92046FFF7E4FF049B13B114 +:1017A00001A800F035F92B69586907B030BDFFF783 +:1017B000D9FFF8E7942300200D17000838B50C4D29 +:1017C00041612B6981689A689142044603D8BDE85B +:1017D0003840FFF78BBF1846FFF7B4FF01232C6199 +:1017E000014623742046BDE83840FEF78FBD00BF98 +:1017F00094230020044B1A681B6990689B689842E8 +:1018000094BF0020012070479423002010B5084C9D +:10181000236820691A6822605460012223611A74C7 +:10182000FFF790FF01462069BDE81040FEF76EBD4E +:1018300094230020FFF7EABFFEE7000010B50C4C30 +:10184000FFF74CFF00F09AF80A498022204600F08A +:1018500021F8012344F8180C037400F041FC002324 +:1018600083F3118862B60448BDE8104000F032B836 +:10187000BC2300205C3400086C34000800F002B97E +:10188000034A516853685B1A9842FBD8704700BFFF +:10189000001000E082600222028270478368A3F198 +:1018A0007C0243F80C2C026943F83C2C426943F853 +:1018B000382C074A43F81C2CC26843F8102C02222B +:1018C00003F8082C002203F8072CA3F11800704736 +:1018D0002503000810B5202383F31188FFF7DEFFEE +:1018E00000210446FFF76AFF002383F31188204696 +:1018F00010BD0000024B1B6958610F20FFF732BF7B +:1019000094230020202383F31188FFF7F3BF000006 +:1019100008B50146202383F311880820FFF730FF24 +:10192000002383F3118808BD49B1064B42681B6947 +:1019300018605A60136043600420FFF721BF4FF026 +:10194000FF307047942300200368984206D01A683D +:101950000260506059611846FFF7C8BE704700002A +:1019600038B504460D462068844200D138BD03686E +:1019700023605C604561FFF7B9FEF4E7054B03F1B6 +:101980001402C3E905224FF0FF310022C3E9071218 +:10199000704700BF9423002070B51C4EC0E903239C +:1019A00005460C4601F07AFA334653F8142F9A4252 +:1019B0000DD13062C5E901242A600A2C2CBF001920 +:1019C0000A30C6E90555BDE8704001F055BA316AE4 +:1019D000431AE31838BF1C469368A34202D908197A +:1019E00001F058FA73699A6894420CD85A68AC604E +:1019F0002B606A6015609A685D60121B9A604FF0F8 +:101A0000FF33F36170BD1B68A41AECE79423002038 +:101A100038B51B4C636998420DD0D0E90032136091 +:101A20005A6000228168C2609A680A449A604FF046 +:101A3000FF33E36138BD2246036842F8143F0021BA +:101A400093425A60C16003D1BDE8384001F01CBA2E +:101A50009A688168256A0A449A6001F01FFA6369EE +:101A60009A68411B8A42E5D9AB181D1A092D206AD4 +:101A700098BF01F10A02BDE83840104401F00ABAEB +:101A8000942300202DE9F041184C002704F114069E +:101A9000656901F003FA236AAA68C11A8A4215D857 +:101AA00013442362D5E9003213605A606369D5F8A4 +:101AB0000C80EF60B34201D101F0E6F987F31188A1 +:101AC0002869C047202383F31188E1E76169B142A7 +:101AD00009D013441B1ABDE8F0410A2B2CBFC018D3 +:101AE0000A3001F0D7B9BDE8F08100BF942300208F +:101AF00000207047FEE70000704700004FF0FF3005 +:101B00007047000002290CD0032904D0012907489E +:101B100018BF00207047032A05D8054800EBC20013 +:101B20007047044870470020704700BF50350008D8 +:101B3000202200200435000870B59AB005460846FA +:101B400001A9144600F0C2F801A8FFF7C1F8431C30 +:101B50005B00C5E900340022237003236370C6B222 +:101B600001AB02341046D1B28E4204F1020401D816 +:101B70001AB070BD13F8011B04F8021C04F8010C24 +:101B80000132F0E708B5202383F311880348FFF7FB +:101B90009DFA002383F3118808BD00BF5025002063 +:101BA00090F8443003F01F02012A07D190F8452035 +:101BB0000B2A03D10023C0E90E3315E003F06003C4 +:101BC000202B08D1B0F848302BB990F84520212AB5 +:101BD00003D81F2A04D8FFF75BBA222AEBD0FAE712 +:101BE000034A82630722C26303640120704700BF77 +:101BF0001822002007B5052917D8DFE801F01916CB +:101C000003191920202383F31188104A0190012120 +:101C1000FFF7FCFA01980E4A0221FFF7F7FA0D4888 +:101C2000FFF720FA002383F3118803B05DF804FB6B +:101C3000202383F311880748FFF7EAF9F2E720230E +:101C400083F311880348FFF701FAEBE7A434000897 +:101C5000C83400085025002038B50C4D0C4C0D49F7 +:101C60002A4604F10800FFF767FF05F1CA0204F1F4 +:101C700010000949FFF760FF05F5CA7204F118006A +:101C80000649BDE83840FFF757BF00BF182A0020BB +:101C900020220020843400088E3400089934000883 +:101CA00070B5044608460D46FFF712F8C6B2204646 +:101CB000013403780BB9184670BD32462946FEF749 +:101CC000F3FF0028F3D10120F6E700002DE9F84FDB +:101CD00005460C46FEF7FCFF2B49C6B22846FFF727 +:101CE000DFFF08B10536F6B228492846FFF7D8FFCE +:101CF00008B11036F6B2632E0DD8DFF88C80DFF80D +:101D00008C90234FDFF890A0DFF890B02E7846B982 +:101D10002670BDE8F88F29462046BDE8F84F01F04F +:101D20004DBB252E2BD1072241462846FEF7BCFF8E +:101D300058B9DBF800302360DBF804306360DBF86F +:101D40000830A36007350C34E0E7082249462846EE +:101D5000FEF7AAFF98B90F4BA21C197809090232A5 +:101D6000C95D02F8041C13F8011B01F00F01534573 +:101D7000C95D02F8031CF0D118340835C6E704F831 +:101D8000016B0135C2E700BF7035000899340008C7 +:101D900085350008107AFF1F1C7AFF1F7835000870 +:101DA000BFF34F8F024AD368DB03FCD4704700BFF8 +:101DB000003C024008B5094B1B7873B9FFF7F0FFF0 +:101DC000074B1A69002ABFBF064A5A6002F18832DF +:101DD0005A601A6822F480621A6008BD762C0020CE +:101DE000003C02402301674508B50B4B1B7893B9B3 +:101DF000FFF7D6FF094B1A6942F000421A611A68D0 +:101E000042F480521A601A6822F480521A601A68EA +:101E100042F480621A6008BD762C0020003C02402B +:101E20000B28F0B516D80C4C0C4923787BB90C4D17 +:101E30000E460C234FF0006255F8047B46F8042B45 +:101E4000013B13F0FF033A44F6D10123237051F80C +:101E50002000F0BD0020FCE7A82C0020782C0020FA +:101E600098350008014B53F820007047983500085A +:101E70000C2070470B2810B5044601D9002010BD76 +:101E8000FFF7CEFF064B53F824301844C21A0BB9A3 +:101E90000120F4E712680132F0D1043BF6E700BFFD +:101EA000983500080B2810B5044621D8FFF778FFB5 +:101EB000FFF780FF0F4AF323D360C300DBB243F484 +:101EC000007343F002031361136943F48033136119 +:101ED000FFF766FFFFF7A4FF074B53F8241000F04D +:101EE000D9F8FFF781FF2046BDE81040FFF7C2BFD9 +:101EF000002010BD003C024098350008F8B512F0F3 +:101F00000103144642D18218B2F1016F57D82D4B0C +:101F10001B6813F0010352D02B4DFFF74BFFF32347 +:101F2000EB60FFF73DFF40F20127032C15D824F0AA +:101F300001046618244C401A40F20117B14223698B +:101F400000EB010524D123F001032361FFF74CFFCF +:101F50000120F8BD043C0430E7E78307E7D12B6993 +:101F600023F440732B612B693B432B6151F8046BC5 +:101F70000660BFF34F8FFFF713FF03689E42E9D05F +:101F80002B6923F001032B61FFF72EFF0020E0E710 +:101F900023F44073236123693B4323610B882B8027 +:101FA000BFF34F8FFFF7FCFE2D8831F8023BADB237 +:101FB000AB42C3D0236923F001032361E4E7184651 +:101FC000C7E700BF00380240003C0240084908B59E +:101FD0000B7828B11BB9FFF7EDFE01230B7008BD8C +:101FE000002BFCD0BDE808400870FFF7FDBE00BF25 +:101FF000762C002010B50244064BD2B2904200D19C +:1020000010BD441C00B253F8200041F8040BE0B2AC +:10201000F4E700BF502800400F4B30B51C6F24047C +:1020200007D41C6F44F400741C671C6F44F4004414 +:102030001C670A4C236843F4807323600244084BF6 +:10204000D2B2904200D130BD441C00B251F8045BC2 +:1020500043F82050E0B2F4E700380240007000403E +:102060005028004007B5012201A90020FFF7C2FF58 +:10207000019803B05DF804FB13B50446FFF7F2FFC7 +:10208000A04205D0012201A900200194FFF7C4FF5E +:1020900002B010BD70470000044BD3F874389B00A9 +:1020A00042BF034B01221A70704700BF003002404C +:1020B000A92C0020014B1878704700BFA92C0020E4 +:1020C000EFF3098305494A6B22F001024A63683342 +:1020D00083F30988002383F31188704700EF00E041 +:1020E000202080F3118862B60C4B0D4AD96821F488 +:1020F000E0610904090C0A43DA60D3F8FC200949BD +:1021000042F08072C3F8FC200A6842F001020A60C3 +:102110001022DA7783F82200704700BF00ED00E05C +:102120000003FA05001000E010B5202383F31188A6 +:102130000E4B5B6813F4006314D0F1EE103AEFF32A +:102140000984683C4FF08073E361094BDB6B2366C5 +:1021500084F30988FFF74EFB10B1064BA36110BD55 +:10216000054BFBE783F31188F9E700BF00ED00E0C2 +:1021700000EF00E0370300083A0300087047000052 +:10218000FEE700000A4B0B480B4A90420BD30B4B67 +:10219000DA1C121AC11E22F003028B4238BF002241 +:1021A0000021FEF79DBD53F8041B40F8041BECE72B +:1021B00038370008A42D0020A42D0020A42D0020D5 +:1021C0007047000070B5D0E915439E6800224FF0BB +:1021D000FF3504EB42135101D3F800090028BEBFBC +:1021E000D3F8000940F08040C3F80009D3F8000B91 +:1021F0000028BEBFD3F8000B40F08040C3F8000BAE +:10220000013263189642C3F80859C3F8085BE0D25C +:102210004FF00113C4F81C3870BD00001D4B03EBD8 +:1022200080022DE9F043D2F80CC05D6DDCF814207B +:10223000461CD2F800E005EB063605EB4018516865 +:1022400071450AD3D5F83438012202FA00F023EAA6 +:102250000000C5F83408BDE8F083BCF81040AEEBD0 +:102260000103A34228BF2346D8F81849A4B2B3EB10 +:10227000840FF0D89468A4F1040959F8047F3760FA +:10228000A4EB09071F44042FF7D81C440B449460A7 +:102290005360D4E7AC2C0020890141F0200101619A +:1022A00003699B06FCD41220FFF7EABA10B5054C6F +:1022B0002046FEF78DFF4FF0A0436365024BA365F8 +:1022C00010BD00BFAC2C0020EC35000870B50378C1 +:1022D000012B054650D12A4B446D98421BD1294B06 +:1022E0005A6B42F080025A635A6D42F080025A657E +:1022F0005A6D5A6942F080025A615A6922F080028E +:102300005A610E2143205B6900F0D4FB1E4BE36051 +:102310001E4BC4F800380023C4F8003EC0232360DD +:102320006E6D4FF40413A3633369002BFCDA0123B1 +:1023300033610C20FFF7A4FA3369DB07FCD41220C9 +:10234000FFF79EFA3369002BFCDA0026A6602846C8 +:10235000FFF738FF6B68C4F81068DB68C4F81468CE +:10236000C4F81C684BB90A4BA3614FF0FF3363619B +:10237000A36843F00103A36070BD064BF4E700BF00 +:10238000AC2C002000380240401400400300200222 +:10239000003C30C0083C30C0F8B5446D0546002113 +:1023A0002046FFF779FFA96D00234FF001128F68D7 +:1023B000C4F834384FF00066C4F81C284FF0FF30E2 +:1023C00004EB431201339F42C2F80069C2F8006B6C +:1023D000C2F80809C2F8080BF2D20B686A6DEB6507 +:1023E000636210231361166916F01006FBD11220E8 +:1023F000FFF746FAD4F8003823F4FE63C4F8003837 +:10240000A36943F4402343F01003A3610923C4F8F4 +:102410001038C4F814380A4BEB604FF0C043C4F8CE +:10242000103B084BC4F8003BC4F81069C4F80039ED +:10243000EB6D03F1100243F48013EA65A362F8BD6B +:10244000C835000840800010426D90F84E10D2F858 +:10245000003823F4FE6343EA0113C2F800387047E2 +:102460002DE9F84300EB8103456DDA68166806F044 +:102470000306731E022B05EB41130C4680460FFA30 +:1024800081F94FEA41104FF00001C3F8101B4FF0E3 +:10249000010104F1100398BFB60401FA03F3916936 +:1024A0008EBF334E06F1805606F5004600293AD01D +:1024B000578A04F15801490137436F50D5F81C1869 +:1024C0000B43C5F81C382B180021C3F810195369A9 +:1024D0000127611EA7409BB3138A928B9B08012A98 +:1024E00088BF5343D8F85C20981842EA034301F1AF +:1024F000400205EB8202C8F85C0021465360284682 +:10250000FFF7CAFE08EB8900C3681B8A43EA8453BD +:10251000483464011E432E51D5F81C381F43C5F8BA +:102520001C78BDE8F88305EB4917D7F8001B21F4A8 +:102530000041C7F8001BD5F81C1821EA0303C0E7C7 +:1025400004F13F0305EB83030A4A5A6028462146FB +:10255000FFF7A2FE05EB4910D0F8003923F4004341 +:10256000C0F80039D5F81C3823EA0707D7E700BFC1 +:102570000080001000040002826D1268C265FFF73F +:1025800021BE00005831436D49015B5813F40040EF +:1025900004D013F4001F0CBF02200120704700007C +:1025A0004831436D49015B5813F4004004D013F4E3 +:1025B000001F0CBF022001207047000000EB8101CA +:1025C000CB68196A0B6813604B6853607047000052 +:1025D00000EB810330B5DD68AA691368D36019B9CF +:1025E000402B84BF402313606B8A1468426D1C44E7 +:1025F000013CB4FBF3F46343033323F0030302EB26 +:10260000411043EAC44343F0C043C0F8103B2B6879 +:1026100003F00303012B09B20ED1D2F8083802EB04 +:10262000411013F4807FD0F8003B14BF43F0805377 +:1026300043F00053C0F8003B02EB4112D2F8003BDC +:1026400043F00443C2F8003B30BD00002DE9F041E7 +:10265000244D6E6D06EB40130446D3F8087BC3F897 +:10266000087B38070AD5D6F81438190706D505EBC4 +:1026700084032146DB6828465B689847FA071FD524 +:10268000D6F81438DB071BD505EB8403D968CCB921 +:102690008B69488A5A68B2FBF0F600FB16228AB9A9 +:1026A0001868DA6890420DD2121AC3E90024202378 +:1026B00083F311880B482146FFF78AFF84F31188C2 +:1026C000BDE8F081012303FA04F26B8923EA0203D7 +:1026D0006B81CB68002BF3D021460248BDE8F04166 +:1026E000184700BFAC2C002000EB810370B5DD68FB +:1026F000436D6C692668E6604A0156BB1A444FF484 +:102700000020C2F810092A6802F00302012A0AB266 +:102710000ED1D3F8080803EB421410F4807FD4F8EC +:10272000000914BF40F0805040F00050C4F8000988 +:1027300003EB4212D2F8000940F00440C2F800094D +:10274000D3F83408012202FA01F10143C3F8341826 +:1027500070BD19B9402E84BF4020206020682E8AA9 +:102760008419013CB4FBF6F440EAC44040F0005048 +:102770001A44C6E7F8B504461E48456D05EB4413F8 +:10278000D3F80869C3F80869F10717D5D5F81038E8 +:10279000DA0713D500EB8403D9684B691F68DA6840 +:1027A000974218D2D21B00271A605F60202383F360 +:1027B00011882146FFF798FF87F31188330617D554 +:1027C000D5F834280123A340134211D02046BDE898 +:1027D000F840FFF723BD012303FA04F2038923EA3B +:1027E000020303818B68002BE8D021469847E5E778 +:1027F000F8BD00BFAC2C00202DE9F74F984C666D5A +:102800007569B3691D4015F48058756107D020467D +:10281000FEF744FD03B0BDE8F04FFFF785BC002D87 +:1028200012DAD6F8003E8E489F071EBFD6F8003E4B +:1028300023F00303C6F8003ED6F8043823F0010362 +:10284000C6F80438FEF752FD280505D58448FFF781 +:10285000B9FC8348FEF73AFDA9040CD5D6F8083830 +:1028600013F0060FF36823F470530CBF43F41053B6 +:1028700043F4A053F3602A0704D56368DB680BB107 +:1028800077489847EB0274D4AF0227D5D4F8549018 +:10289000DFF8CCB100274FF0010A09EB4712D2F85C +:1028A000003B03F44023B3F5802F11D1D2F8003B55 +:1028B000002B0DDA62890AFA07F322EA0303638127 +:1028C00004EB8703DB68DB6813B139465846984749 +:1028D000A36D01379B68FFB29F42DED9E80617D58A +:1028E000676D3A6AC2F30A1002F00F0302F4F012A5 +:1028F000B2F5802F00F08880B2F5402F08D104EBAC +:1029000083030022DB681B6A07F5805790426DD174 +:102910002903D6F8184813D5E20302D50020FFF7A3 +:1029200095FEA30302D50120FFF790FE670302D5B1 +:102930000220FFF78BFE260302D50320FFF786FE59 +:102940006D037FF567AFE00702D50020FFF712FFA8 +:10295000A10702D50120FFF70DFF620702D5022073 +:10296000FFF708FF23077FF555AF0320FFF702FFAE +:1029700050E7636DDFF8E8B0019300274FF0010ADC +:10298000A36D9B685FFA87F999453FF67DAF019B80 +:1029900003EB4913D3F8002902F44022B2F5802F4B +:1029A00022D1D3F80029002A1EDAD3F8002942F0F8 +:1029B0009042C3F80029D3F80029002AFBDB606DA0 +:1029C0004946FFF769FC22890AFA09F322EA030360 +:1029D000238104EB8903DB689B6813B149465846A1 +:1029E00098474846FFF71AFC0137C9E7910708BF27 +:1029F000D7F80080072A98BF03F8018B02F1010283 +:102A000098BF4FEA182881E7023304EB830207F5E9 +:102A100080575268D2F818C0DCF80820DCE9001CA6 +:102A2000A1EB0C0C002188420AD104EB83046368FB +:102A30009B699A6802449A605A6802445A6067E740 +:102A400011F0030F08BFD7F800808C4588BF02F84B +:102A5000018B01F1010188BF4FEA1828E3E700BFAD +:102A6000AC2C0020436D03EB4111D1F8003B43F443 +:102A70000013C1F8003B7047436D03EB4111D1F8DF +:102A8000003943F40013C1F800397047436D03EB7C +:102A90004111D1F8003B23F40013C1F8003B70470B +:102AA000436D03EB4111D1F8003923F40013C1F851 +:102AB0000039704700F1604303F561430901C9B271 +:102AC00083F80013012200F01F039A4043099B0082 +:102AD00003F1604303F56143C3F880211A60704736 +:102AE00030B5039C0172043304FB0325C0E906538F +:102AF000049B03630021059BC160C0E90000C0E99D +:102B00000422C0E90842C0E90A11436330BD000055 +:102B1000416A0022C0E90411C0E90A22C2606FF0D4 +:102B20000101FEF71DBF0000D0E90432934201D13C +:102B3000C2680AB9181D7047002070470369196000 +:102B4000C2680132C260C269134482690361934260 +:102B500024BF436A03610021FEF7F6BE38B5044680 +:102B60000D46E3683BB16269131D1268A36213440A +:102B7000E362002007E0237A33B929462046FEF7B6 +:102B8000D3FE0028EDDA38BD6FF00100FBE700004E +:102B9000C368C269013BC3604369134482694361EE +:102BA000934224BF436A436100238362036B03B1F2 +:102BB0001847704770B52023044683F31188866A4E +:102BC0003EB9FFF7CBFF054618B186F311882846BA +:102BD00070BDA36AE26A13F8015BA362934202D359 +:102BE0002046FFF7D5FF002383F31188EFE70000AD +:102BF0002DE9F84F04460E46174698464FF0200937 +:102C000089F311880025AA46D4F828B0BBF1000F3B +:102C100009D141462046FFF7A1FF20B18BF311886F +:102C20002846BDE8F88FD4E90A12A7EB050B521A23 +:102C3000934528BF9346BBF1400F1BD9334601F1A2 +:102C4000400251F8040B43F8040B9142F9D1A36AF6 +:102C500040334036A3624035D4E90A239A4202D376 +:102C60002046FFF795FF8AF31188BD42D8D289F339 +:102C70001188C9E730465A46FEF70CF8A36A5B4450 +:102C80005E44A3625D44E7E710B5029C0172043321 +:102C900004FB0321C0E906130023C0E90A33039BA8 +:102CA0000363049BC460C0E90000C0E90422C0E9DA +:102CB0000842436310BD0000026AC260426AC0E974 +:102CC00004220022C0E90A226FF00101FEF748BE8B +:102CD000D0E904239A4201D1C26822B9184650F8BB +:102CE000043B0B607047002070470000C368C26956 +:102CF0000133C3604369134482694361934224BF33 +:102D0000436A43610021FEF71FBE000038B5044648 +:102D10000D46E3683BB123691A1DA262E2691344C0 +:102D2000E362002007E0237A33B929462046FEF704 +:102D3000FBFD0028EDDA38BD6FF00100FBE7000075 +:102D400003691960C268013AC260C26913448269AA +:102D50000361934224BF436A036100238362036BD0 +:102D600003B118477047000070B520230D46044694 +:102D7000114683F31188866A2EB9FFF7C7FF10B199 +:102D800086F3118870BDA36A1D70A36AE26A0133DD +:102D90009342A36204D3E16920460439FFF7D0FFD0 +:102DA000002080F31188EDE72DE9F84F04460D4629 +:102DB000904699464FF0200A8AF311880026B346C0 +:102DC000A76A4FB949462046FFF7A0FF20B187F315 +:102DD00011883046BDE8F88FD4E90A073A1AA8EB03 +:102DE0000607974228BF1746402F1BD905F140031D +:102DF00055F8042B40F8042B9D42F9D1A36A4033C7 +:102E0000A3624036D4E90A239A4204D3E1692046FA +:102E10000439FFF795FF8BF311884645D9D28AF321 +:102E20001188CDE729463A46FDF734FFA36A3B44B3 +:102E30003D44A3623E44E5E7D0E904239A4217D11A +:102E4000C3689BB1836A8BB1043B9B1A0ED013609D +:102E5000C368013BC360C3691A44836902619A4233 +:102E600024BF436A0361002383620123184670472D +:102E70000023FBE700F088B84FF08043002258633E +:102E80001A610222DA6070474FF080430022DA6054 +:102E9000704700004FF08043586370474FF0804305 +:102EA000586A70474B6843608B688360CB68C36027 +:102EB0000B6943614B6903628B6943620B68036072 +:102EC0007047000008B5264B26481A6940F2FF11EA +:102ED0000A431A611A6922F4FF7222F001021A6190 +:102EE0001A691A6B0A431A631A6D0A431A651E4A55 +:102EF0001B6D1146FFF7D6FF02F11C0100F5806043 +:102F0000FFF7D0FF02F1380100F58060FFF7CAFF3C +:102F100002F1540100F58060FFF7C4FF02F1700177 +:102F200000F58060FFF7BEFF02F18C0100F58060C4 +:102F3000FFF7B8FF02F1A80100F58060FFF7B2FFCC +:102F400002F1C40100F58060FFF7ACFF02F1E0017F +:102F500000F58060FFF7A6FFBDE8084000F09AB8D2 +:102F60000038024000000240F835000808B500F0C3 +:102F700007FAFEF763FCFFF78FF8BDE80840FEF79D +:102F80006BBE000070470000104B1A6C42F001024B +:102F90001A641A6E42F001021A660D4A1B6E93689B +:102FA00043F0010393604FF0804353229A624FF045 +:102FB000FF32DA6200229A615A63DA605A600122B3 +:102FC0005A6108211A601C20FFF774BD00380240C6 +:102FD000002004E04FF0804208B51169D3680B402F +:102FE000D9B2C9439B07116107D5202383F3118808 +:102FF000FEF744FC002383F3118808BD08B5FFF7F2 +:10300000E9FFBDE80840FFF78FB800001F4B1A69C1 +:103010006FEAC2526FEAD2521A611A69C2F3080209 +:103020001A614FF0FF301A695A69586100215A69D4 +:1030300059615A691A6A62F080521A621A6A02F079 +:1030400080521A621A6A5A6A58625A6A59625A6AED +:103050001A6C42F080521A641A6E42F080521A665C +:103060001A6E0B4A106840F480701060186F00F4FC +:103070004070B0F5007F1EBF4FF4803018671967AD +:10308000536823F40073536000F05AB900380240CB +:1030900000700040334B4FF080521A64324A4FF4B4 +:1030A000404111601A6842F001021A601A689107E3 +:1030B000FCD59A6822F003029A602A4B9A6812F0B3 +:1030C0000C02FBD1196801F0F90119609A601A68C5 +:1030D00042F480321A601A689203FCD55A6F42F0AB +:1030E00001025A671F4B5A6F9007FCD51F4A5A605E +:1030F0001A6842F080721A601B4A53685904FCD562 +:10310000184B1A689201FCD5194A9A60194B1A6833 +:10311000194B9A42194B21D1194A1168194A914207 +:103120001CD140F205121A60144A136803F00F0311 +:10313000052BFAD10B4B9A6842F002029A609A680A +:1031400002F00C02082AFAD15A6C42F480425A6406 +:103150005A6E42F480425A665B6E704740F20572C6 +:10316000E1E700BF0038024000700040085440070B +:1031700000948838002004E011640020003C0240E4 +:1031800000ED00E041C20F41084A08B551691368DB +:103190000B4003F00103536123B1054A13680BB1DF +:1031A00050689847BDE80840FEF7BEBF003C0140AC +:1031B000242D0020084A08B5516913680B4003F01C +:1031C0000203536123B1054A93680BB1D068984755 +:1031D000BDE80840FEF7A8BF003C0140242D0020B8 +:1031E000084A08B5516913680B4003F004035361A2 +:1031F00023B1054A13690BB150699847BDE80840EF +:10320000FEF792BF003C0140242D0020084A08B57B +:10321000516913680B4003F00803536123B1054A59 +:1032200093690BB1D0699847BDE80840FEF77CBFB1 +:10323000003C0140242D0020084A08B5516913685C +:103240000B4003F01003536123B1054A136A0BB11D +:10325000506A9847BDE80840FEF766BF003C014051 +:10326000242D0020174B10B55A691C68144004F433 +:1032700078725A61A30604D5134A936A0BB1D06AD7 +:103280009847600604D5104A136B0BB1506B9847F2 +:10329000210604D50C4A936B0BB1D06B9847E2051D +:1032A00004D5094A136C0BB1506C9847A30504D59B +:1032B000054A936C0BB1D06C9847BDE81040FEF7FF +:1032C00033BF00BF003C0140242D00201A4B10B535 +:1032D0005A691C68144004F47C425A61620504D5A2 +:1032E000164A136D0BB1506D9847230504D5134A48 +:1032F000936D0BB1D06D9847E00404D50F4A136E5F +:103300000BB1506E9847A10404D50C4A936E0BB1D3 +:10331000D06E9847620404D5084A136F0BB1506F02 +:103320009847230404D5054A936F0BB1D06F984793 +:10333000BDE81040FEF7F8BE003C0140242D0020FF +:10334000062108B50846FFF7B5FB06210720FFF761 +:10335000B1FB06210820FFF7ADFB06210920FFF78E +:10336000A9FB06210A20FFF7A5FB06211720FFF77E +:10337000A1FBBDE8084006212820FFF79BBB000009 +:1033800008B5FFF743FE00F00BF8FDF71FFFFDF750 +:10339000F7FDFFF7F7FDBDE80840FFF76BBD000044 +:1033A0000023054A19460133102BC2E9001102F12E +:1033B0000802F8D1704700BF242D0020034611F801 +:1033C000012B03F8012B002AF9D1704753544D33D8 +:1033D00032463F3F3F00000053544D3332463430B5 +:1033E000780053544D3332463432780053544D33C1 +:1033F000324634343658580000000000CC33000800 +:103400003F00000013040000D83300083F00000014 +:1034100019040000E23300083F000000210400000E +:10342000EC3300083F00000000000000FD0E000823 +:10343000E90E0008250F0008110F00081D0F0008F5 +:10344000090F0008F50E0008E10E0008310F000812 +:103450000000000001000000000000006D61696EC6 +:103460000000000069646C6500000000643400081E +:10347000D823002050250020010000003918000842 +:10348000000000004172647550696C6F7400254241 +:103490004F415244252D424C002553455249414C41 +:1034A00025000000020000000000000019110008C3 +:1034B0008511000840004000E8290020F82900207C +:1034C00002000000000000000300000000000000F7 +:1034D000C91100080000000010000000082A0020A8 +:1034E000000000000100000000000000AC2C0020E3 +:1034F00001010200F51B0008051B0008A11B0008C4 +:10350000851B0008430000000C3500080902430039 +:10351000020100C03209040000010202010005247A +:103520000010010524010001042402020524060004 +:1035300001070582030800FF09040100020A0000D8 +:10354000000705010240000007058102400000005D +:10355000120000005835000812011001020000405E +:10356000091241570002010203010000040309048B +:1035700025424F41524425007265766F2D6D696E6C +:10358000692D736400303132333435363738394180 +:103590004243444546000000004000000040000057 +:1035A0000040000000400000000001000000020098 +:1035B0000000020000000200000002000000020003 +:1035C00000000200000002000000000001130008DB +:1035D000B915000865160008400040000C2D0020B9 +:1035E0000C2D0020010000001C2D00208000000098 +:1035F0004001000003000000000180020000000004 +:10360000AAAAAAAA00010000FFFF00000000000013 +:1036100000A00A004005000000000000AAAAAAAA13 +:1036200040000000CFFF000000000000000000008C +:103630000000000000000000AAAAAAAA00000000E2 +:10364000FFFF00000000000000000000000000007C +:1036500000000000AAAAAAAA00000000FFFF0000C4 +:10366000000000000000000000000000000000005A +:10367000AAAAAAAA00000000FFFF000000000000A4 +:10368000000000000000000000000000AAAAAAAA92 +:1036900000000000FFFF000000000000000000002C +:1036A0000000000000000000AAAAAAAA0000000072 +:1036B000FFFF00000000000000000000000000000C +:1036C000000000000A0000000000000003000000ED +:1036D00000000000000000000000000000000000EA +:1036E00000000000000000000000000000000000DA +:1036F00000000000000000007C000000000000004E +:1037000000000F0000000000FF009600000000080D +:103710000096000000000800040000006C3500085E +:103720000000000000000000000000000000000099 +:08373000000000000000000091 +:00000001FF From e89eb34d55f12f12afc3821b944710dbed17d7a9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 31 Mar 2022 21:33:23 +0100 Subject: [PATCH 0584/3101] AP_HAL_ChibiOS: add I2C and compass backend to drivers in memory on H750 move more of EKF into memory on H750 move rc handling into memory on H750 disable double math on SPRacing H7 --- libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat | 2 ++ libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld | 12 ++++++++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat index bad4197e5ea..13538e105fa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat @@ -15,6 +15,8 @@ MCU_CLOCKRATE_MHZ 480 env OPTIMIZE -O2 +define HAL_WITH_EKF_DOUBLE 0 + STM32_ST_USE_TIMER 2 # internal flash is off limits diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld index 4aec753fb9b..87423bd42d5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld @@ -135,6 +135,7 @@ SECTIONS lib/lib*.a:*Filter.*(.text* .rodata*) lib/lib*.a:*Filter2p.*(.text* .rodata*) lib/lib*.a:SPIDevice.*(.text* .rodata*) + lib/lib*.a:I2CDevice.*(.text* .rodata*) lib/lib*.a:Util.*(.text* .rodata*) lib/lib*.a:Device.*(.text* .rodata*) lib/lib*.a:Scheduler.*(.text* .rodata*) @@ -143,8 +144,15 @@ SECTIONS lib/lib*.a:crc.*(.text* .rodata*) lib/lib*.a:matrixN.*(.text* .rodata*) lib/lib*.a:matrix_alg.*(.text* .rodata*) - lib/lib*.a:AP_NavEKF*.*(.text* .rodata*) - lib/lib*.a:EKFGSF*.*(.text* .rodata*) + lib/lib*.a:AP_NavEKF3*.*(.text* .rodata*) + lib/lib*.a:AP_NavEKF_*.*(.text* .rodata*) + lib/lib*.a:EKF*.*(.text* .rodata*) + lib/lib*.a:AP_Compass_Backend.*(.text* .rodata*) + lib/lib*.a:AP_RCProtocol_Backend.*(.text* .rodata*) + lib/lib*.a:AP_RCProtocol.*(.text* .rodata*) + lib/lib*.a:AP_RCProtocol_CRSF.*(.text* .rodata*) + lib/lib*.a:AP_RCTelemetry.*(.text* .rodata*) + lib/lib*.a:AP_CRSF_Telem.*(.text* .rodata*) lib/lib*.a:vector2.*(.text* .rodata*) lib/lib*.a:quaternion.*(.text* .rodata*) lib/lib*.a:polygon.*(.text* .rodata*) From 64743b73caa18db5ebd432d01b5a14e280e1496d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 31 Mar 2022 21:34:17 +0100 Subject: [PATCH 0585/3101] AP_Bootloader: change timeout on external flash init --- Tools/AP_Bootloader/AP_Bootloader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index 58bde11adc9..947b679e465 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -163,7 +163,7 @@ int main(void) while (!ext_flash.init()) { // keep trying until we get it working // there's no future without it - chThdSleep(1000); + chThdSleep(chTimeMS2I(20)); } #endif From 2d62da92985b18318d9726f6d0f96610b95ae287 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 31 Mar 2022 21:34:33 +0100 Subject: [PATCH 0586/3101] AP_FlashIface: make sure XIP flash is ready before returning --- libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp index 193b1cd78ac..2c3929f08db 100644 --- a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp +++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp @@ -921,6 +921,9 @@ bool AP_FlashIface_JEDEC::start_xip_mode(void** addr) Debug("XIP mode unsupported on this chip"); return false; } + + bool success = false; + switch(_desc.entry_method) { case AP_FlashIface_JEDEC::XIP_ENTRY_METHOD_1: { @@ -937,7 +940,8 @@ bool AP_FlashIface_JEDEC::start_xip_mode(void** addr) cmd.addr = 0; cmd.dummy = _desc.fast_read_dummy_cycles; _dev->set_cmd_header(cmd); - return _dev->enter_xip_mode(addr); + success = _dev->enter_xip_mode(addr); + break; } case AP_FlashIface_JEDEC::XIP_ENTRY_METHOD_2: { @@ -965,7 +969,8 @@ bool AP_FlashIface_JEDEC::start_xip_mode(void** addr) // correct dummy bytes because of addition of alt bytes cmd.dummy = _desc.fast_read_dummy_cycles - 1; _dev->set_cmd_header(cmd); - return _dev->enter_xip_mode(addr); + success = _dev->enter_xip_mode(addr); + break; } default: { @@ -973,6 +978,9 @@ bool AP_FlashIface_JEDEC::start_xip_mode(void** addr) return false; } } + // make sure that the flash is ready once we enter XIP + DELAY_MICROS(100); + return success; } bool AP_FlashIface_JEDEC::stop_xip_mode() From 29e8f96275204f365e5c9a83a55b5189c6659cf2 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 1 Apr 2022 20:59:10 +0100 Subject: [PATCH 0587/3101] AP_InertialSensor: move frontend update into ramfunc. --- libraries/AP_InertialSensor/AP_InertialSensor.h | 4 ++-- libraries/AP_InertialSensor/AP_InertialSensor_Backend.h | 8 ++++---- .../AP_InertialSensor/AP_InertialSensor_Invensense.h | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 6f46f473361..284c93d6fec 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -215,10 +215,10 @@ class AP_InertialSensor : AP_AccelCal_Client float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); } // update gyro and accel values from accumulated samples - void update(void); + void update(void) __RAMFUNC__; // wait for a sample to be available - void wait_for_sample(void); + void wait_for_sample(void) __RAMFUNC__; // class level parameters static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 7c0f1d23c1b..db5d226269f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -137,7 +137,7 @@ class AP_InertialSensor_Backend void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__; // rotate gyro vector, offset and publish - void _publish_gyro(uint8_t instance, const Vector3f &gyro); /* front end */ + void _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */ // this should be called every time a new gyro raw sample is // available - be it published or not the sample is raw in the @@ -151,7 +151,7 @@ class AP_InertialSensor_Backend void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle); // rotate accel vector, scale, offset and publish - void _publish_accel(uint8_t instance, const Vector3f &accel); /* front end */ + void _publish_accel(uint8_t instance, const Vector3f &accel) __RAMFUNC__; /* front end */ // this should be called every time a new accel raw sample is available - // be it published or not @@ -286,10 +286,10 @@ class AP_InertialSensor_Backend bool gyro_harmonic_notch_enabled(void) const { return _imu._harmonic_notch_filter.enabled(); } // common gyro update function for all backends - void update_gyro(uint8_t instance); /* front end */ + void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */ // common accel update function for all backends - void update_accel(uint8_t instance); /* front end */ + void update_accel(uint8_t instance) __RAMFUNC__; /* front end */ // support for updating filter at runtime uint16_t _last_accel_filter_hz; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 798dea82a18..3230a7f83d3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -45,7 +45,7 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend enum Rotation rotation); /* update accel and gyro state */ - bool update() override; /* front end */ + bool update() override __RAMFUNC__; /* front end */ void accumulate() override; /* front end */ /* From 98aec61899676a61e071a9d1c3ebb538cd2de250 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Fri, 1 Apr 2022 19:52:40 -0500 Subject: [PATCH 0588/3101] Tools: add airspeed defaults to vehicle defaults files --- Tools/autotest/default_params/blimp.parm | 2 ++ Tools/autotest/default_params/copter.parm | 2 ++ Tools/autotest/default_params/rover.parm | 2 ++ Tools/autotest/default_params/sailboat.parm | 2 ++ Tools/autotest/default_params/sub.parm | 2 ++ 5 files changed, 10 insertions(+) diff --git a/Tools/autotest/default_params/blimp.parm b/Tools/autotest/default_params/blimp.parm index 9db84765738..192455b115c 100644 --- a/Tools/autotest/default_params/blimp.parm +++ b/Tools/autotest/default_params/blimp.parm @@ -1,3 +1,5 @@ +ARSPD_PIN 1 +ARSPD_BUS 2 EK2_ENABLE 1 FRAME_TYPE 0 FS_THR_ENABLE 1 diff --git a/Tools/autotest/default_params/copter.parm b/Tools/autotest/default_params/copter.parm index 07a8a376668..7f41498a4ec 100644 --- a/Tools/autotest/default_params/copter.parm +++ b/Tools/autotest/default_params/copter.parm @@ -1,5 +1,7 @@ FRAME_TYPE 0 FS_THR_ENABLE 1 +ARSPD_PIN 1 +ARSPD_BUS 2 ATC_RAT_YAW_P 0.09 ATC_RAT_YAW_I 0.009 BATT_MONITOR 4 diff --git a/Tools/autotest/default_params/rover.parm b/Tools/autotest/default_params/rover.parm index d26a8146f54..9095f40720d 100644 --- a/Tools/autotest/default_params/rover.parm +++ b/Tools/autotest/default_params/rover.parm @@ -1,3 +1,5 @@ +ARSPD_PIN 1 +ARSPD_BUS 2 ATC_SPEED_P 0.1 ATC_STR_RAT_FF 0.75 BATT_MONITOR 4 diff --git a/Tools/autotest/default_params/sailboat.parm b/Tools/autotest/default_params/sailboat.parm index be01673f04d..54fb607a1a2 100644 --- a/Tools/autotest/default_params/sailboat.parm +++ b/Tools/autotest/default_params/sailboat.parm @@ -1,4 +1,6 @@ ARSPD_OFFSET 2013 +ARSPD_PIN 1 +ARSPD_BUS 2 FRAME_CLASS 2 LOIT_TYPE 1 MOT_SLEWRATE 0 diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index ddb5c84c8df..31ce6df1a6d 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -1,3 +1,5 @@ +ARSPD_PIN 1 +ARSPD_BUS 2 BATT_MONITOR 4 BTN0_FUNCTION 0 BTN10_SFUNCTION 0 From 1a843f659ff0e3f21a15ad4e8a7224e2b4b70e7d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 6 Apr 2022 08:47:42 +0100 Subject: [PATCH 0589/3101] AP_HAL_ChibiOS: bdshot version of MatekF765-Wing --- .../hwdef/MatekF765-Wing-bdshot/defaults.parm | 2 ++ .../hwdef/MatekF765-Wing-bdshot/hwdef-bl.dat | 4 ++++ .../hwdef/MatekF765-Wing-bdshot/hwdef.dat | 14 ++++++++++++++ 3 files changed, 20 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/defaults.parm new file mode 100644 index 00000000000..7b610d2326f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/defaults.parm @@ -0,0 +1,2 @@ +SERVO13_FUNCTION 120 +NTF_LED_TYPES 455 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef-bl.dat new file mode 100644 index 00000000000..b3863b4997b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef-bl.dat @@ -0,0 +1,4 @@ +# hw definition file for processing by chibios_pins.py +# for Matek F765-Wing bootloader + +include ../MatekF765-Wing/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef.dat new file mode 100644 index 00000000000..ec6b1ee22da --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF765-Wing-bdshot/hwdef.dat @@ -0,0 +1,14 @@ +# Bi-directional dshot version of MatekF765-Wing + +include ../MatekF765-Wing/hwdef.dat + +# undefine the pins we are going to change +undef PA0 PA1 PA2 PA3 + +# hw definition file for processing by chibios_pins.py +# for Matek F765-Wing + +PA0 TIM2_CH1 TIM2 PWM(1) GPIO(50) BIDIR +PA1 TIM2_CH2 TIM2 PWM(2) GPIO(51) +PA2 TIM5_CH3 TIM5 PWM(3) GPIO(52) BIDIR +PA3 TIM5_CH4 TIM5 PWM(4) GPIO(53) From 1072cc772d1f7e0eba8966fb0b9d82835c2f4fe8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 6 Apr 2022 08:48:12 +0100 Subject: [PATCH 0590/3101] bootloaders: MatekF765-Wing-bdshot bootloader --- .../bootloaders/MatekF765-Wing-bdshot_bl.bin | Bin 0 -> 16136 bytes .../bootloaders/MatekF765-Wing-bdshot_bl.hex | 1011 +++++++++++++++++ 2 files changed, 1011 insertions(+) create mode 100755 Tools/bootloaders/MatekF765-Wing-bdshot_bl.bin create mode 100644 Tools/bootloaders/MatekF765-Wing-bdshot_bl.hex diff --git a/Tools/bootloaders/MatekF765-Wing-bdshot_bl.bin b/Tools/bootloaders/MatekF765-Wing-bdshot_bl.bin new file mode 100755 index 0000000000000000000000000000000000000000..e8ddc8903d38a0cb8ee77dd69d6ed77c59003bcd GIT binary patch literal 16136 zcmb_@3w%`7)$cx!$xJcZ?Y1jRlVeg$zTvDrbQCKs7-OZ=QBLX^T_xRFFsmY` za=&9~phV`i(Qs}>_E&1g3S(uCtJQI9;CA#nkmN8iO|LL~1%{Odf$QpM){3l^zjkov z?vVMMJ!PX|B9+J)D>A;;hVQ;X2!YHK3lbq3LXe|==vI``CMHwUBD6#PznbO`NUP?5 zuG{;LoE{7!b^lGbxBD~cr^LEzIAc$~aHx!!7%49n3o}yJRq|mhOh}QRNbii>JEprD zrR@x{+`V?C~(veY>z)ByF@BQI(LSsk81F}pN$N~6=Q*3qDp;U)bom=C|X)5fh6RlmKktB{UwAQ(G-6TM1H_8#sZYyhg7-iXS zciAg*g=~>o@KBI1Y;lZ+YQ<+951wA?Y7@C3Qpa6BUfAl`bcz7ao-x+6*k$k%>8A0U zUBqnMoPD3MA^Uz~L-ppn@2hUO`~K>NYL7$mvDG!z9!oMeqJ^w6))>)3^Bm9C*H%gW z)*e~2^2`9?CnVBRc|zW;qqQoJb;MU#RYS|CWM6^vO8@tIhTm894Bu9`j^Qq*q|Xsn zT-@<}=9UxkwymS5OvU;k)09i02}~0?#Y2FHSWn2S7f!UA;&gjNpnJ(wCU+AbqcI^- z{!p5bBGyhmC;e2GT8_TxGFcQeX-<3LDUnFSW524@H??;$=I6>Yyglv&t^|L!UnSN< zasK=}__Gu>${*79WF^<3cN1${1$pwI&?d|jS_Q4Q%FTeH`^WM+4!CkeVk6SDopBmR zxn{17*e;!?{ofzs+RVk%Y@hWp&#aNHk|%{|_9M+!6U|xGm#GW&eW^tAoQ7!B$Ztft z6N$oXOy*~irzXeENF$o>0?$O9j$utoq$5W1?b6r|(#D!qFT6fPrf34W0_3ys6*&vC zc~`VO3<-n;EQ+dU4xXoDet|Kc2AO558_A{q9Up$CmbkP@I{nQmD-Rs179uT;PVeW< z-!2~w^X7HsSMXb1J{(>p<))SksY84!k=926+pKzlNY_QXhUS5nj0qYljYQRvvWr^n zz0rMRMDmSY*P)fPoYtlZkUS+)8@mU%%FF){CO146O!Pb*qH|gqQwt=&Ey$Q|y^Too zqQHgq6=2?d(R-tNM$5577eslXxuaar41xF3k|@a^3cW7ybhUcA`dfv>nm-tw6K1Tl z1=duE@rR>%*cmT7)Mx1~XQE7jT@rWjHw|N_TV1aUw+b`7Cf9oR^O)1Jk#$EBv)VrL z?IWxyC%$)n5nTw*9vD43L{gP2l}EBAPQ86ZI5N31bx_4UF19$9U0HUd>#}eAvPkif zOjp-(rp59>g6a>ALONQ>T;<9QN4QpIG1JD$jP3IE z%syO^FEHR|Q#2)xq5cTdrW)L(S`el*Z@EgbeteinZ;#T_2Zzb^U4j?XdedPzJL|@b zzO*)`aB}c^iQ_U~(wA1rrlqQua)Gy~UgQ@;r;P0o!r>xN_1mMW=1bu}$XU%)f{2tm zO2Wi?pP7Gw$Je#MzJ28IVfxLx`pMj(AceU-mkBa+Cxx_tW5sEJItnONh_Pxa8I#`29hJIQjYPeS^qnX%MZ!9K zPrjZtoq;?Dk%(38X9dPQX-G|eBC89O$Qz2O<0)B966fS9Nh!cNOVYqCjbDkptoS5` z9PhL{aJIVAgo0PIn^Ig=vf0iER?#GU;Ne}r77E^SmyrO)zoM9E==!|=wA)Bt+Db-x zW4yqZFBb0E<_uU^Ru`JxFuJI3?7XsP?N zB0D2J{#FsbL*aDTFR7uUmv=Yvr+|O;q00}LIME>BYt}KWd!j8Un|QR5l#2awUO{08 zv1%-u`W+}Mkz)Y{UnPNNI%k)G&NM(we80)X3XHFitz zmyf>fFb8Mtd)tu`V)N2xtH$`k^&;&%df9jHv|!04hCArc1gi6xCCs<9LRx8yW74Uq zqfF!4-0yU70+E@`m%}9QqcD^AVYno3Fl-Q1{qtrwJ7##9QwMYJ@!E|N)=1}(Mdb`L zWb?TY>slrGYFlgKb4+LL$+nTNxA7sivdXh-#8+2EApr-O^S3y-;D6k6@(!kOGz@!( zFhx4QZj@P8+_8U@*q?N8A!7%#fLSsUHqsW;L8Go)Cya+Vffa(r%uyzX%!!6Dk5W!( zaj-#X*PLkBTQ+|{cFtOvC8&ix!q4P9)0#Iwl)cSQ9z(gn)4!jyNc%h-hErL>{4OZQ5Vf9i^sq_)a z_GVEx-GghF5{=z4g}9N^{rEakkzecL1!Geh_LOCH8m!{RYexu84jb(2u#>OBegiV_ zkFau#;^kb`n4c%F&t-W2P>dlmN7FDow$agOv^vU-X z6m)O5M#Iaja`+DDM>RDA!9qt`PP${mMfrS-rI>nMv%u1A1$#Lld%X zF6_8ova5CR5!02$N$3kHhTfMN^q!YHR(o)omKn_|=oc3NcT*hqK$wo⁢C69b}&5 z_0+I!yWQ`&*&43)_yD{E1F8&Nx5ghFP%Uh6Frl5|&aRyzvv8-oI@eK??Iq^m0Kc%+ zkr(I@oi#*yAx2D^ZhGd@v&ZW6}sZg*GZw%1H;vUplB?t<7} zbHStSFbM~7V#Q@aQ1>s0YpMruclh|+BN*-No<9&-MR83R4hlgzw;7yIqtu5yXr~Gr zI_3&=j1nnPIKkaj5+8p%#t#J(SetUVxJ}fBTMHLtnoYuQ%b2!(!0@?oW+}z==kPo3 zo*L4&(@oc|%E&0C6ku?E$e{NmVVaPT%IC%Bu@btDI!|?OZOsfXxDcno1+nhSwZLDJ zv1ei>xQ}BcKSm2~lmyhoY1A60=MO+nVop$WXH9Ms=Sfg=7pTd^OoA>+&HwKDzleRg z{t&Fqe`WpO)MZ()4V(c&s2-r@Z=qa-yerv8%g0creW{7I1+>g1;p!4?3n|=sl<66+ zM84O}yEyDIr~CK-TgX_sZS=g_3_YTY%L|nOD$A_Zx-G#wdR7hAi*yx za|EhUmB>-hHszdY26Sl+Jy(=STcD`>n`_bgOE;a7pDi+q8BP40sUh>j?3~O{_Ct&{ z^U-~Fy(2xuyvPr&Ah0tPz?H+MY%@P(5{YzXlxpKF*lZuYJwXdbI*;-Sw9)lb`q1(# zC{ws22{_P#kzP!~ZBNpI!abCvMKC@eogXt2_&L)<4Ca&-x*Ky^6Q9%M5XIn#u|jzV zofB60U=8Nf{h1Eh=X+h0F7{1IIQ{l+`Ht1JC1De^z_Qi(88avXsmX=rw|*ZG1qm6TUNuUSUtgso zXMdlOei$>&pcoinS1G2rWuZjY1e)c03OLElnQSScW_i7dSj}^}zFBiq`lu9I=8lDq z4`dZ`{ahuxL2Yqt*tH>H0~f(&qI_BGH$sC`Suw`>GW{#Ek`W?9D^$4cO5?G$9n6cg z`26u3he`ft7N2&D2IubgBAACezFhhemzZ@GAQ?QnGo*Xf!gK1`@ zc`@c7_aY-1<9z4Z5?0%OIk6X1(CK4hwI@>~b1w&T_AV6o81N_;*BMC4fo!nO%Zi%-soniKM<0@-AcFda2F^E=5tr`&G3* zdBtk%4$PfLO2Pb($aP|lXzid>@BYkdhgH|jHTh(*KxO5;PV`ZlXhBDMUbR!dDQT8P zlT&29Iq<})%n3Wt>u;7<6wn<&toTX+<&X(-vVXKrY!i=l9e{l5CAaFPATiVS$XFsD z7^C0Ucch^8U|EWFH^zFqV>^-Be8(=^w?7*?Ab+QTu^kG%3Tu$wA{O=^ zyG$Ba2B!npp~eNl_RGiX7+Z5+9q!bCHGM}sAm6KdRdyE~EhG86gHF-9ffMUC7%@)Q zcyrKxg>|$zwvRL&xyNPQz*_4z5Ni+SOTV&xWXloy9eNvi7Wv76tUI6E&=h_ZG^Agf z9G{Iz*e%}=-Edu;gZd68PC*XRNF_I`@X0Vgw2`xJV1U=!#M^YCgbY*pKN+JE%bDxM z-SWoO<~S!Q&FS}v?Jq&J7VyACc|&Y}3bJwBr!YTvfk~K=!bww@v|dI!AT#-VkcDR@ z>xj+O8n(F(;w|Ba@^E}51S`=Tp=0!bKNd8mm;@H1CXKxUC^GiaHK@ynC!h`mbCOV> zMq2@OHu{Tj)-@j6U>j0NRU(b?6%2g3zGLdO-Y+G<9lQFe_FDUwqwbGg2$R4`==!&i zjw2N%<(AlP4W<;I>zp!oQQs8X!oCkb^0t^>b&Lld*2yYY`<)zHj1Hxw1W)Z3dAQtK3Gnp@!O0vB+a@&^fts3!tZdd^ByPE@<;GFQit&o*bo; z@`=K?VRyh|c+;rOv(}@=Ii*C-Dsq^Pdu|4mfpqwtN@{GL9bzk1#PucYs(k@^hksRJ ztn+H%E!&njZDWzDH%Nev<>AlD@iarf9dsOk=Z2oBv|Z=d;2p~3F^YG%EpEnQtuby> zBFzdTm~gYw*<+D~@i{AzivfE7*7?&{D3KKkky>S+C9Q(qpUpC3EtL)FurVa(IEmA< z_BFmdVGAvMUBq{O3;FFsA`;uhRo*UEiR+999bwo7audYE;} z>GN2RQ(ijHf#0He>Pn}aJMRSg48$qx=CKw|*32XLRnPO5EyueUwNDe)>0cBA3q^_4 zCp9az({cBA<#^WCOfe~ug>iaJKo7${)A+I4_-GCK3zFkB-4z`vE(fS)0LS%ir<+Hw zIY4hv3EFX>WlGQ}vhLMxgK#&b?>+dn8e)<5-RHEM63HA_j2@h#YVy^_{fw6 z+)x(VGoZ3iSyg9J8c<$%{kgZPgml=1oRyLO7%dd4Dds@VH_$oEou)(4adRoN;j&#W7uiZ9xy?d&=Rb21R z%xR5F^l**0`&na-__w%R75Q^k)OilVr?NwwkZ@u-fe~q?3HS>&bR8Cfm>qJ~vKB`* z?x9?WN**J5-)I$gh_mY{EF(P_t@D)Rw2FHje?9xMVTRFEpF00wfEh|-DKzz?ekU$Z z@JEywR*>sqbEa(YFnFJ`*1sgaW8CYQ+NUq=mD7FJ+TP@DtMh!}K870^gSQ@fPs{fl zFH7+2qi~I<4cdH&@lCDkp;{0LPaD7HuT(7W`-BusglTz5dptDnSE#iFuJoCuHV@7~W)@ zxy15b=Q^Sj-W?!|tX|H_F5+coDf4oulU?+04yMltn_UloB(o6h>AcM54u!AjopL4e zGuUqlokIfD*Osu4XR*v&b=*GI&ZT~NY7DziQ5OoD__dJ&nuGHYMn%t7ObfGu|?y%t6*XsZxmyM<-!MhUAPU-m~fK z)M)>B^inu6l0$8DbTF&~T^1qHc~jk#zGAMB4zHPpD3OhdYUpR`FhEK6d_xil$@o%~v|};WbqveW>YBtGhwl_o4il52@L@ z>O=X!p&OcnuIs$iE0nM(r83+T-<*BJ4I#Cq1eR|s^47SwE8AreuwR)fL`dv&&^U?F zP)?Kjrm2hc-VeeWOGO;=`SF8Y&*V;VWjC=4Q&Znx&&qsmoaeA6s+Q9_mn($~i!?X& zgqfQl2b%)nKVz+Qy-}o3kW3@eta(WEy_>B(EuQDb&rlg8OIf?duSR4*RhJTx6ueD% z$Xxy$Us>&DhxRP2c5AwCD8yL@I4iSpxyFz44p9V3OD@0IQ2DO!f5j)|FpjJ z73me$rXO}q7PKqVt*azb%M{-!^FI`1-bQ_?OG>ZcrN`>MT{aP^o0!JKZhWHvDg9fx zf5L}s!cL*(bnFyuKDE7>v&5vsiS&C?G&%<&y*N0OK^mC8ajvA#=H9FUe_2h-D7{i#X`1)On+|4iL3&P=D>Tt-0*aZ87Hk`o4PFoFP zXEvp{DaVwERpG>ccMRZ6r(r{=tL1~*ix%_ z3bgk%kHBx zT|i}Di3}+$UcZhH+p&Jyj(3P%stU2Na+5`3*vh(Hh^$IzQS6v6NWUrWsjl;ztoQH6Jd{ZJm=3l> zdyTGJg^hL{>M z5mUpbFoJM#$9HK=jeE`4V`?a#iFlki#k=Xc=vkQN(w1v0n;qKITGu->&*f$sEfUA> za}*)kiI`)Nb#Fl5<=Bz_7S=2y8XB`Tkw6{X)q?{$PGZ4%degs){GdaBJ9AnlsU%-@ zj^=jBXYWCdvNqYKO}1&7g>{CXI}k;5H`2QPQ>!FKe^SmZ^1@D=;o=5;>?t|@I5fNS z&vKSY$o&BCVWeL*m~yjf&~CV0d|-LnX~xnkFDhcAczwS}$h-0)4fOb~!-EX>maMb4 zB|+tdb5nFWk?%tQ(ruPg+nwT~dyUSW>KA(!v2zH6BTEa8Ymj9B$p#f{Je&phQw{rl zWlker#o+V#sYvanlb|?o=ktTkOs4RcjuSHRAZA3cqR4e8UtXV~Au0hzQ$DODbYbZsEQSuK+2Hs+f0-j-tR&AG7-p} zCBeJ)@=2t2{L`+m7k?KEAI_!_3_isB-qn z+e`-yj*Fa=8M3ox;kWWOk}s&>qpbqJeizdWK1%4#+<9?#Rr#Ws&ij}fZU^q7Sx#VX zqA$m)!HVhsDU%%Y2r2mua{6WSMUFcTU&%5~TbVDIfo~UA4SdhU0IdsxR@M>XJdT{= zvR+iHaTgB|(2DScWg9a^$41;?;P+m;pr@gNS+^Ft%+3e#J1aea2m_qtgp~gu`aoCa z_hNRvR@A#)xZ6^AO6vmu2jWW}y?EN4-DS7!4%n@F_olsB;)#abQg*OJ$1F|le06Mi zq|V6|S`ioFym-=yw^+Mw{QiU}YqbC#G zE_O=16PzwCfWDyjh&XH9ss;L#zBSt@lrmscdyhuWE2NXNHY-UR)<0 zEQ_jV#e2@HbK*T`#sBEq=PBP?Eq0DcSIF2j%ib=7cus6aL`0?$o`k%Fy{j!ZV@K>& zc0-rbn+5LkQU21VN39oGZqtt6!xJ$aGz%>5ytvntCHENUQptEhnVyEtp6kR<-QRxG zSc;cZ(+OFZIv3tl4mN&T=j0IPt>Fe8Svss6J0&$M0A0D-E|Jxc|5WtQbwj7}cC0&< zv7Cm!W24LvcVP#?l-1?#^bGzB+bg#D4Llonm<}Oy9|7mcQGe z8Z4z$@(fWbd8YUFV)va>>w5{PbXIhEW?}8Wa2QUV$|2Vq-S@f^tEX!}sL;DtVaoJg z#tPk=-;S4=j3D(h$adI1-3f|taf)y|_JJawfg%UX{u;)+T3qMQcd}np0_c5BZ-E_s~*KEvxrXibXQ7 z=HvDI-*6A<8vjW-wf|q_)4^L!kzPufGAvq0yjAG|=ZLP#IdZ6K*gaRx_trr}YCCq6 zA1oVW$La+K2MrI#LJA;xfiTL&5WUV+LfB6cgK|bG47AMb*O= z?7+Gm{NRt_wLcGE@GZhxtdUu4ZB#nj#9uo8a`rR4O@|wn)Snnm)3MSNjvbLQJAsKv zPx?A9+iy$_@z}M*qQ30A>%5p#-|VnUJAF3pPqLwWMsJv7d#P2`68jZO3`+kNbE8CK z<{piiQQ}bA9s8JNd#OKSd#oKL6-qydHFDc8uRHRoI)NcYc@8Ctfl=HjQ4HfTGfEUg zB=&Ji0>h69%Z^YSe~T>%Uuc-1M*mT!#oBmp^v9!Z;uFi4T@evA$PHFNe#jhl z#BQOhvOspp95Qz%J@w}}q-LD!WKN=O?0kjL-++7RxCOLJjnmEw+C55NJV3jOIPJ+bzkqTfWj(3=R7aaLG2nr7~p5?PM(6Ng;?{M-eZr$J-H8`{t|l+wtmkTC zk!Rj6f0dVNcR1sB_V#-F&Gwt@h3&N%f#{ON_kX6) z_=0Xn-zoaF_rnE3wQx1^+IV~YcS_fnt}AV?4I}=7eUdL^wt`FMlaxz(>WxV&|^_x1*lElhN0*G)pWp(hWZ0yOd8Bcr#YZe6g0EpWqgyEx6@j3jdM1 zvB>X%^GkkN!7t;CXKFtN7rxG~=MhuzRen(!fCMc0M+x{U$EYoDvQ9$$bVfWH4 zfN#H1baAdxp3U%5zA^Y4gZ5hf^hX|(z~7=og1Fn#IEv513BFAdnuRBXiP#Tv-7(?$ z`c+*OagH4V$LQ%%Fu^hP65|VlDe7OqPMde;Lc$i4aK>2jaOfO z^=As@ys-T2DayA$gy#rLz^}iIwby^E^w!d1@awPe(o;H558;iVV^{sY4!*d}I60_y z_aHK)$KB&GmhM6%2bm;3i(k6Y_;*0?-e?LLpQR*QEA3a|0cbJ;H`57^3$o-}9tZz)EUT8pG+ zY1z05ZFR;i_}!3KnX@GWJK|3Yx$J{qCzpxSI9@J7`FF|oJ5WBEEPGLYJ6V1V<=2zt z7f|j@mQSMm>twkf6JMRE%Dn%3v79xbCadD7B`25=W2al0QYYF+ zd|sW~&`3t?-lLbvb}qPT#8+V0n7y$ImIHaP%1xwuyOMK+&*Nsq1KLY&t~B0vb7h9x zC_3PEFd~0kJ>J_Y(z|o3d%mkx%#_mEu@L?$M;c|a-K`XI#J7+_Hgn^>)s1vq^+v0! zx^X{7=D~wUarw3y-6mJHxP~9^ofuIq1~G!x2*w*KtHdWz&w`KSab~=CT4gK#dW^~J zksm{xcaPj`vR_gcW>)ImdRWs_IJGSrO2vE}5{sGPRK&bAbEBc^&6tm)8h`T>&+lK? z08UW94v#wm)~VTSPbCNT6j6Vgx>Dn>@^G-Ys%oqqDnw6eJ66MnPDsJ`;Fo0VGVYQS zaBLE;1#rP@aJByc$4}rxY}snh1ist;0Zuc44{*V2a7+FHPB($C9{B1f@LhKeZkkKy zzsJLmG;OV}X;0yM^_4Av3w z-LXW=^Vp*)h^p5>qxi>C%xYM79mwem*DJ_Vk-vyM4f%7(rz3wFc|P)|#??^#zP`>*01;A#S<2Ky7SO_kXKVo_`-21o{r zXlfZz*?8B0oNxkSc5U?UUdY^0v{v1*vT&iWEdGxqDvQ@GdibXGP1kBSKfD+<{G;y+ z@mSWM)7n4(_fQtmY(RP%=_g1}AU%Tg0MdG-SCHt)M-Vsl0MaW+Par*qv>)kzAbF6w zkSH8OfADWhji-U;p8Ga$pm`AQ6h6Gy(K%6=(98sU$=3V6yV|q4aT}TKSXHv7boP7) z0->swuc;_mF&l6!9gKfZPbZp52KX?DogBjZ$5Et-J}v=5Zbv^Y>X443pOz=)L;W`l z{;>Eb7NU8|jK2X!J_E@Km?pd9&k!~Z1Jx>qwbN_|%}6HFN#M$4bodgnr)HMi1*9y8N$^}MpIjkXS+i}!CSmc- zH_gAx)39-V?R^i5SbxESg@uc5x_NQYEhP?T>2d+ceBVfMYG8g)JpViq@WumI|A{@A7o_$-8b|94OXu2E&u=k literal 0 HcmV?d00001 diff --git a/Tools/bootloaders/MatekF765-Wing-bdshot_bl.hex b/Tools/bootloaders/MatekF765-Wing-bdshot_bl.hex new file mode 100644 index 00000000000..5593469368f --- /dev/null +++ b/Tools/bootloaders/MatekF765-Wing-bdshot_bl.hex @@ -0,0 +1,1011 @@ +:020000040800F2 +:100000000006022001020008510F0008D10E00086E +:10001000290F0008D10E0008FD0E00080302000899 +:100020000302000803020008030200085D2700081D +:10003000030200080302000803020008030200088C +:10004000030200080302000803020008030200087C +:100050000302000803020008C1380008E93800085C +:100060001139000839390008613900080302000815 +:10007000030200080302000803020008030200084C +:10008000030200080302000803020008030200083C +:10009000030200080302000803020008893900086F +:1000A000030200080302000803020008030200081C +:1000B000713A000803020008030200080302000866 +:1000C00003020008030200080302000803020008FC +:1000D00003020008030200080302000803020008EC +:1000E000ED390008030200080302000803020008BB +:1000F00003020008030200080302000803020008CC +:1001000003020008030200080302000803020008BB +:1001100003020008030200080302000803020008AB +:10012000030200080302000803020008030200089B +:10013000030200080302000803020008030200088B +:10014000030200080302000803020008112F000840 +:10015000030200080302000803020008030200086B +:10016000030200080302000803020008030200085B +:10017000030200080302000803020008030200084B +:1001800003020008030200085D3A000803020008A9 +:10019000030200080302000803020008030200082B +:1001A000030200080302000803020008030200081B +:1001B000030200080302000803020008030200080B +:1001C00003020008030200080302000803020008FB +:1001D00003020008030200080302000803020008EB +:1001E00003020008030200080302000803020008DB +:1001F00003020008030200080302000803020008CB +:1002000002E000F000F8FEE772B63A4880F3088892 +:10021000394880F3098839484EF60851CEF200017A +:10022000086040F20000CCF200004EF63471CEF2CD +:1002300000010860BFF34F8FBFF36F8F40F20000E3 +:10024000C0F2F0004EF68851CEF200010860BFF314 +:100250004F8FBFF36F8F4FF00000E1EE100A4EF6A4 +:100260003C71CEF200010860062080F31488BFF3D1 +:100270006F8F02F031FB02F0D3FA03F0C3F94FF0B5 +:1002800055301F491B4A91423CBF41F8040BFAE725 +:100290001C49194A91423CBF41F8040BFAE71A493C +:1002A0001A4A1B4B9A423EBF51F8040B42F8040B0A +:1002B000F8E700201749184A91423CBF41F8040B67 +:1002C000FAE702F0EBFA03F0FDF9144C144DAC42DE +:1002D00003DA54F8041B8847F9E700F041F8114CA1 +:1002E000114DAC4203DA54F8041B8847F9E702F0D9 +:1002F000D3BA0000000602200022022000000008FD +:100300000000022000060220A03E00080022022079 +:1003100068220220682202207834022000020008AD +:100320000002000800020008000200082DE9F04F5A +:100330002DED108AC1F80CD0D0F80CD0BDEC108A8D +:10034000BDE8F08F002383F311882846A0470020E2 +:1003500001F024FEFEE701F0B9FD00DFFEE700003A +:1003600038B500F0C1FC40B1174B6FF07F0100229F +:1003700059720E211A729972DA7202F0AFF90546BB +:1003800002F0E2F90446D0B9104B9D4219D0013376 +:100390009D4241F2883512BF044600250124002009 +:1003A00002F0A6F90CB100F059F800F039FD00F0A8 +:1003B000DBFB284600F0E0F800F050F8F9E70025F4 +:1003C000EDE70546EBE700BF00220220010007B081 +:1003D00008B500F095FBA0F120035842584108BD34 +:1003E00007B541F21203022101A8ADF8043000F074 +:1003F000A5FB03B05DF804FB202310B583F311883F +:100400001248C3680BB101F04FFE0023104A4FF4AD +:100410007A710E4801F00CFE002383F311880D4C15 +:10042000236813B12368013B2360636813B16368D9 +:10043000013B6360084B1B7833B9636823B9022022 +:1004400000F068FC3223636010BD00BF6822022008 +:10045000F9030008842302207C220220F8B5404BD7 +:10046000404A1C461968013179D004339342F9D1CE +:100470006268A24273D33C4B9B6803F1006303F5AF +:10048000C0339A426BD2002000F088FB0220374B29 +:10049000187000F035FC364B00211A6C19641A6E86 +:1004A00019661A6E5A6C59645A6E59665A6E5A69B0 +:1004B00042F080025A615A6922F080025A615A69F8 +:1004C0001A6942F000521A611A6922F000521A6148 +:1004D0001B6972B64FF0E023C3F8084DD4E900045D +:1004E000BFF34F8FBFF36F8F224AC2F88410BFF360 +:1004F0004F8F536923F480335361BFF34F8FD2F88A +:10050000803043F6E076C3F3C905C3F34E335B0195 +:1005100003EA060C29464CEA81770139C2F8747265 +:10052000F9D2203B13F1200FF2D1BFF34F8FBFF36D +:100530006F8FBFF34F8FBFF36F8F536923F4003377 +:1005400053610023C2F85032BFF34F8FBFF36F8F58 +:10055000202383F31188854680F308882047F8BD5F +:100560000080010820800108002202207C22022055 +:100570000038024000ED00E02DE9F04F93B0A94BA8 +:100580002022FF2100900AA89D6800F0F3FBA64AF4 +:100590001378A3B90121A5481170C360202383F308 +:1005A0001188C3680BB101F07FFD0023A04A4FF40E +:1005B0007A719E4801F03CFD002383F31188009B73 +:1005C0009C4A03B1136000239B49009C98469B46BC +:1005D0001E469A460B705360012000F091FB24B137 +:1005E000944B1B68002B00F01682002000F088FA64 +:1005F0000390039B002BF2DB012000F077FB039BB1 +:10060000213B162BE8D801A252F823F06906000816 +:100610009106000825070008D9050008D90500083B +:10062000D9050008B90700088B090008A5080008CB +:10063000070900082F09000855090008D905000816 +:1006400067090008D9050008D9090008090700084A +:10065000D90500081D0A00087506000809070008EA +:10066000D9050008070900080220FFF7B1FE00289D +:1006700040F0FB81009B022105A8B8F1000F08BFE4 +:100680001C4641F21233ADF8143000F057FAA3E7DC +:100690004FF47A7000F034FA071EEBDB0220FFF70C +:1006A00097FE0028E6D0013F052F00F2E081DFE849 +:1006B00007F0030A0D1013360523042105A805933E +:1006C00000F03CFA17E004215648F9E704215B48A2 +:1006D000F6E704215A48F3E74FF01C09484609F1B0 +:1006E000040900F05DFA0421059005A800F026FA3F +:1006F000B9F12C0FF2D101204FF0000A00FA07F7F0 +:1007000047EA0B0B5FFA8BFB00F080FB26B10BF086 +:100710000B030B2B08BF0024FFF762FE5CE70421EC +:100720004848CDE7002EA5D00BF00B030B2BA1D131 +:100730000220FFF74DFE074600289BD001203E4EC9 +:1007400000F02CFA4FF000080220307000F0D8FAC8 +:100750005FFA88F9484600F031FA044690B14846FD +:1007600008F1010800F03AFA0028F1D1B846044631 +:1007700041F21213022105A83E46ADF8143000F0F4 +:10078000DDF929E7012325460220337000F0AEFA97 +:10079000244B9B68AB4207D9284600F001FA013090 +:1007A00040F068810435F3E70025234BB8463E4608 +:1007B0001D70204B5D60A7E7002E3FF45BAF0BF090 +:1007C0000B030B2B7FF456AF02201B4B187000F06D +:1007D00097FA322000F094F9B0F10009FFF64AAF21 +:1007E00019F003077FF446AF0E4A09EB0503926840 +:1007F00093423FF63FAFB9F5807F3FF73BAF124BD7 +:10080000B945019322DD4FF47A7000F079F9039035 +:10081000039A002AFFF62EAF039A0137019B03F8D3 +:10082000012BEDE700220220802302206822022013 +:10083000F9030008842302207C22022004220220E3 +:10084000082202200C22022080220220C820FFF76A +:10085000BFFD074600283FF40DAF1F2D11D8C5F18D +:1008600020020AAB25F0030084494A45184428BFFA +:100870004A46019200F058FA019AFF217F4800F0A1 +:1008800079FA4FEAA903C9F387027C492846019304 +:1008900000F078FA064600283FF46AAF019B05EBAA +:1008A000830531E70220FFF793FD00283FF4E2AE15 +:1008B00000F0BEF900283FF4DDAE0027B946704BCA +:1008C0009B68BB4218D91F2F11D80A9B01330ED049 +:1008D00027F0030312AA134453F8203C059348461B +:1008E000042205A9043700F035FB8146E7E73846C6 +:1008F00000F056F90590F2E7CDF81490042105A810 +:1009000000F01CF900E70023642104A8049300F020 +:100910000BF900287FF4AEAE0220FFF759FD002846 +:100920003FF4A8AE049800F06BF90590E6E70023C9 +:10093000642104A8049300F0F7F800287FF49AAE2D +:100940000220FFF745FD00283FF494AE049800F024 +:1009500067F9EAE70220FFF73BFD00283FF48AAE83 +:1009600000F076F9E1E70220FFF732FD00283FF4BE +:1009700081AE05A9142000F071F907460421049006 +:1009800004A800F0DBF83946B9E7322000F0B8F8E7 +:10099000071EFFF66FAEBB077FF46CAE384A07EB5D +:1009A0000A03926893423FF665AE0220FFF710FDFE +:1009B00000283FF45FAE27F003075744BA453FF4E1 +:1009C000A3AE50460AF1040A00F0EAF804210590AB +:1009D00005A800F0B3F8F1E74FF47A70FFF7F8FCE0 +:1009E00000283FF447AE00F023F9002844D00A9BCA +:1009F00001330BD008220AA9002000F0C3F9002817 +:100A00003AD02022FF210AA800F0B4F9FFF7E8FC51 +:100A10001C4801F0C9FA13B0BDE8F08F002E3FF476 +:100A200029AE0BF00B030B2B7FF424AE00236421C3 +:100A300005A8059300F078F8074600287FF41AAE61 +:100A40000220FFF7C5FC814600283FF413AEFFF7F4 +:100A5000C7FC41F2883001F0A7FA059800F018FAB7 +:100A60004E463C4600F0D2F9B6E506464CE64FF05D +:100A7000000AFFE5B8467BE6374679E68022022089 +:100A800000220220A0860100F7B50C46184E4FF454 +:100A90007A71054602FB01F396F90020501C0BD138 +:100AA0001448294601930268176A2246B8478442CF +:100AB000019B03D1002310E0002AF1D096F9002019 +:100AC000511C01D0012A0DD10B4829460268166A33 +:100AD0002246B047844205D10123084A0120137001 +:100AE00003B0F0BD4FF4FA7001F05EFA0020F7E7B2 +:100AF00010220220F8270220D4230220D023022033 +:100B0000002307B5024601210DF107008DF80730DB +:100B1000FFF7BAFF20B19DF8070003B05DF804FBB2 +:100B20004FF0FF30F9E700000A46042108B5FFF74F +:100B3000ABFF80F00100C0B2404208BD074B0A463F +:100B400030B41978064B53F82140014623682046FB +:100B5000DD69044BAC4630BC604700BFD0230220A7 +:100B60004C3B0008A086010070B50A4E00240A4DD7 +:100B700001F0BAFC308028683388834208D901F03C +:100B8000ADFC2B6804440133B4F5C03F2B60F2D3B5 +:100B900070BD00BFD22302208C23022001F052BD81 +:100BA00000F1006000F5C0300068704700F100609F +:100BB000920000F5C03001F0DDBC0000054B1A6862 +:100BC000054B1B889B1A834202D9104401F086BC56 +:100BD000002070478C230220D223022038B5074D15 +:100BE00004462868204401F081FC28B92868204484 +:100BF000BDE8384001F092BC38BD00BF8C23022014 +:100C000010F0030309D1B0F5846F04D200F1005055 +:100C1000A0F571200368184670470023FBE7000029 +:100C200000F10050A0F57120D0F8200470470000BA +:100C3000064991F8243033B100230822086A81F86C +:100C40002430FFF7B3BF0120704700BF902302207C +:100C5000014B1868704700BF002004E0204B02469B +:100C6000F0B518681F4BC0F30B06000C1F885D68B9 +:100C7000BE4293F9084022D09F89BE4221D01F8BEB +:100C8000BE4206D102240C2505FB04335D6893F9AE +:100C90000840B0F5805F16D041F20103984208BFCA +:100CA0005A24013A013D0B460A4493420DD215F9EC +:100CB000016F581C5EB1034600F8016CF5E7002493 +:100CC000E1E70124DFE74124EBE7184605E02C25A6 +:100CD00090421D7001D2981C5C70401AF0BD00BF9C +:100CE000002004E01422022000207047022803D1D3 +:100CF000024B4FF080629A61704700BF000C0240C7 +:100D0000022803D1024B4FF480629A61704700BF02 +:100D1000000C0240022804D1024A536983F4806324 +:100D200053617047000C0240002310B5934203D07A +:100D3000CC5CC4540133F9E710BD0000013810B594 +:100D400010F9013F3BB191F900409C4203D11AB127 +:100D50000131013AF4E71AB191F90020981A10BD57 +:100D60001046FCE703460246D01A12F9011B00297F +:100D7000FAD1704702440346934202D003F8011BA4 +:100D8000FAE770472DE9F8431F4D14460746884699 +:100D900095F8242052BBDFF870909CB395F824306E +:100DA0002BB92022FF2148462F62FFF7E3FF95F879 +:100DB00024004146C0F1080205EB8000A24228BF92 +:100DC0002246D6B29200FFF7AFFF95F82430A41B5D +:100DD00017441E449044E4B2F6B2082E85F824600D +:100DE000DBD1FFF725FF0028D7D108E02B6A03EB02 +:100DF00082038342CFD0FFF71BFF0028CBD1002016 +:100E0000BDE8F8830120FBE790230220024B1A780B +:100E1000024B1A70704700BFD0230220102202201C +:100E200038B5154C154D204600F0F0FB29462046FC +:100E300000F018FC2D681248EA6ED2F8043843F02E +:100E40000203C2F8043801F0AFF80E49284600F05A +:100E500011FDEA6E0C48D2F804380C49A04223F088 +:100E60000203C2F804384FF4E1330B6003D0BDE84D +:100E7000384000F02BBB38BDF8270220583C000852 +:100E800040420F00603C0008D4230220B823022017 +:100E900038B50B4B04461A780A4B53F822500A4BCC +:100EA0009D420CD0094B002118221846FFF762FF23 +:100EB000046001462846BDE8384000F007BB38BD55 +:100EC000D02302204C3B0008F8270220B823022040 +:100ED00000B59BB0EFF3098168226846FFF724FF55 +:100EE000EFF30583044B9A6BDA6A9A6A9A6A9A6AF4 +:100EF0009A6A9A6A9B6AFEE700ED00E000B59BB033 +:100F0000EFF3098168226846FFF70EFFEFF30583D0 +:100F1000044B9A6B9A6A9A6A9A6A9A6A9A6A9B6A64 +:100F2000FEE700BF00ED00E000B59BB0EFF30981E4 +:100F300068226846FFF7F8FEEFF30583034B5A6B10 +:100F40009A6A9A6A9A6A9A6A9B6AFEE700ED00E0DA +:100F5000FEE7000030B50A44084D91420DD011F86B +:100F6000013B5840082340F30004013B2C4013F0A0 +:100F7000FF0384EA5000F6D1EFE730BD2083B8EDDF +:100F8000026843681143016003B1184770470000CD +:100F9000024A136843F0C0031360704700780040B2 +:100FA00013B50E4C204600F085FA04F1140000231E +:100FB0004FF480720A49009400F046F9094B4FF44F +:100FC0008072094904F13800009400F0BFF9074A23 +:100FD000074BC4E9172302B010BD00BFD423022081 +:100FE00040240220910F0008402502200078004094 +:100FF00080F93703037C30B5214C002918BF0C461B +:10100000012B0CD11F4B984209D11F4B1A6C42F097 +:1010100080421A641A6E42F080421A661B6E226881 +:10102000036EC16D03EB52038466B3FBF2F3626897 +:10103000150442BF23F0070503F0070343EA450305 +:10104000CB60A36843F040034B60E36843F00103C7 +:101050008B6042F4967343F001030B604FF0FF3353 +:101060000B62510505D512F0102205D0B2F1805F58 +:1010700004D080F8643030BD7F23FAE73F23F8E7DF +:10108000543B0008D4230220003802402DE9F047E9 +:10109000C66D05463768F4692107346219D014F02B +:1010A000080118BF8021E20748BF41F02001A307D3 +:1010B0004FF0200348BF41F04001600748BF41F4B2 +:1010C000807183F31188281DFFF75AFF002383F3F3 +:1010D0001188E2050AD5202383F311884FF40071AB +:1010E000281DFFF74DFF002383F311884FF02009DF +:1010F0004FF0000A14F0200838D13B0616D54FF007 +:10110000200905F1380A200610D589F311885046C8 +:1011100000F050F9002836DA0821281DFFF730FFCB +:1011200027F080033360002383F31188790614D5F8 +:10113000620612D5202383F31188D5E913239A423E +:1011400008D12B6C33B127F040071021281DFFF781 +:1011500017FF3760002383F31188E30618D5AA6EC2 +:101160001369ABB15069BDE8F047184789F311889E +:10117000736A284695F86410194000F0B5F98AF3AF +:101180001188F469B6E7B06288F31188F469BAE7A8 +:10119000BDE8F087F8B51546826804460B46AA42BA +:1011A00000D28568A1692669761AB5420BD2184625 +:1011B0002A46FFF7B9FDA3692B44A3612846A3681B +:1011C0005B1BA360F8BD0CD9AF1B18463246FFF776 +:1011D000ABFD3A46E1683044FFF7A6FDE3683B44C7 +:1011E000EBE718462A46FFF79FFDE368E5E70000B6 +:1011F00083689342F7B50446154600D28568D4E962 +:101200000460361AB5420BD22A46FFF78DFD63699A +:101210002B4463612846A3685B1BA36003B0F0BD49 +:101220000DD93246AF1B0191FFF77EFD01993A4679 +:10123000E0683144FFF778FDE3683B44E9E72A467C +:10124000FFF772FDE368E4E710B50A440024C361C8 +:10125000029B8460C16002610362C0E90000C0E9D2 +:10126000051110BD08B5D0E90532934201D182685D +:1012700082B98268013282605A1C42611970002171 +:10128000D0E904329A4224BFC368436100F0A8FE4B +:10129000002008BD4FF0FF30FBE7000070B52023B1 +:1012A00004460E4683F31188A568A5B1A368A26918 +:1012B000013BA360531CA36115782269934224BFAC +:1012C000E368A361E3690BB120469847002383F3E9 +:1012D0001188284607E03146204600F071FE0028BC +:1012E000E2DA85F3118870BD2DE9F74F04460E460A +:1012F00017469846D0F81C904FF0200A8AF31188C0 +:101300004FF0000B154665B12A4631462046FFF7DF +:1013100041FF034660B94146204600F051FE0028D7 +:10132000F1D0002383F31188781B03B0BDE8F08F60 +:10133000B9F1000F03D001902046C847019B8BF301 +:101340001188ED1A1E448AF31188DCE7C160C3617D +:10135000009B82600362C0E905111144C0E90000EE +:1013600001617047F8B504460D461646202383F305 +:101370001188A768A7B1A368013BA36063695A1CE1 +:1013800062611D70D4E904329A4224BFE36863614C +:10139000E3690BB120469847002080F3118807E0ED +:1013A0003146204600F00CFE0028E2DA87F311886F +:1013B000F8BD0000D0E9052310B59A4201D182683A +:1013C0007AB982680021013282605A1C82611C78DD +:1013D00003699A4224BFC368836100F001FE20467E +:1013E00010BD4FF0FF30FBE72DE9F74F04460E46E6 +:1013F00017469846D0F81C904FF0200A8AF31188BF +:101400004FF0000B154665B12A4631462046FFF7DE +:10141000EFFE034660B94146204600F0D1FD0028AA +:10142000F1D0002383F31188781B03B0BDE8F08F5F +:10143000B9F1000F03D001902046C847019B8BF300 +:101440001188ED1A1E448AF31188DCE702684368AC +:101450001143016003B11847704700001430FFF7D3 +:1014600043BF00004FF0FF331430FFF73DBF0000D3 +:101470003830FFF7B9BF00004FF0FF333830FFF7C7 +:10148000B3BF00001430FFF709BF00004FF0FF3179 +:101490001430FFF703BF00003830FFF763BF0000D0 +:1014A0004FF0FF323830FFF75DBF0000002070477B +:1014B000FFF776BD044B036000234360C0E90233AD +:1014C00001230374704700BF6C3B000810B5202354 +:1014D000044683F31188FFF78DFD02232374002354 +:1014E00083F3118810BD000038B5C36904460D466A +:1014F0001BB904210844FFF7A9FF294604F1140091 +:10150000FFF7B0FE002806DA201D4FF48061BDE829 +:101510003840FFF79BBF38BD026843681143016044 +:1015200003B118477047000013B5446BD4F8A438D2 +:101530001A681178042915D1217C022912D1197950 +:10154000012312898B4013420CD101A904F14C00F4 +:1015500001F05EFFD4F8A4480246019B217920687F +:1015600000F0D4F902B010BD143001F0E1BE00006B +:101570004FF0FF33143001F0DBBE00004C3001F0BF +:10158000B3BF00004FF0FF334C3001F0ADBF00009F +:10159000143001F0AFBE00004FF0FF31143001F005 +:1015A000A9BE00004C3001F07FBF00004FF0FF32B9 +:1015B0004C3001F079BF000000207047D0F8A4380B +:1015C0001A6810B511780446042917D1017C022944 +:1015D00014D15979012352898B4013420ED1143012 +:1015E00001F042FE024648B1D4F8A4484FF480739B +:1015F00061792068BDE8104000F076B910BD0000A8 +:10160000406BFFF7DBBF0000704700007FB5124B57 +:1016100001250426044603600023057400F18402BA +:1016200043602946C0E902330C4B02901430019309 +:101630004FF48073009601F0F3FD094B04F29442DD +:10164000294604F14C000294CDE900634FF4807305 +:1016500001F0BAFE04B070BD943B0008011600080A +:10166000291500080B68202282F311880A7903EB00 +:10167000820210624A790D3243F822008A7912B14F +:1016800003EB820318620223C0F8A418037400233A +:1016900083F311887047000038B5037F044613B107 +:1016A00090F85430ABB90125201D0221FFF734FF1B +:1016B00004F114006FF00101257700F09DFC04F1A6 +:1016C0004C0084F854506FF00101BDE8384000F040 +:1016D00093BC38BD10B5012104460430FFF71CFF50 +:1016E0000023237784F8543010BD000038B5044639 +:1016F0000025143001F0ACFD04F14C00257701F019 +:101700007BFE201D84F854500121FFF705FF204681 +:10171000BDE83840FFF752BF90F85C3003F060033B +:10172000202B06D190F85D200023212A03D81F2A00 +:1017300006D800207047222AFBD1C0E9143303E009 +:10174000034A02650722426583650120704700BF96 +:1017500038220220D0F8A43837B51A680446117828 +:1017600004291AD1017C022917D119790123128980 +:101770008B40134211D100F14C05284601F0FCFECC +:1017800058B101A9284601F043FED4F8A448024606 +:10179000019B2179206800F0B9F803B030BD00004A +:1017A00000EB8103F0B51E6A85B004460D46FEB11C +:1017B000202383F3118804EB8507301D0821FFF7F0 +:1017C000ABFEFB6806F14C005B691B681BB1019026 +:1017D00001F02CFE019803A901F01AFE024648B15F +:1017E000039B2946204600F091F8002383F31188DB +:1017F00005B0F0BDFB685A691268002AF5D01B8A53 +:10180000013B1340F1D104F15C02EAE70D3138B538 +:1018100050F82140DCB1202383F31188D4F8A428A8 +:101820001368527903EB8203DB689B695D6845B1FD +:1018300004216018FFF770FE294604F1140001F03E +:101840001DFD2046FFF7BAFE002383F3118838BD43 +:101850007047000001F0B6B801232822002110B51E +:10186000044600F8243BFFF785FA0023C4E901335E +:1018700010BD000038B50446202383F311880025ED +:101880004160C0E90355C0E90555C0E9075501F0BD +:10189000A9F80223237085F3118838BD70B500EBD9 +:1018A0008103054650690E461446DA6018B11022CD +:1018B0000021FFF75FFAA06918B110220021FFF79D +:1018C00059FA31462846BDE8704001F055B900008C +:1018D000826802F0011282600022C0E90422C0E99D +:1018E0000622026201F0D4B9F0B4012500EB8104B4 +:1018F00047898D40E4683D43A46945812360002306 +:10190000A2606360F0BC01F0EFB90000F0B4012503 +:1019100000EB810407898D40E4683D4364690581DB +:1019200023600023A2606360F0BC01F069BA00008C +:1019300070B502230025044603704566056280F8F1 +:101940004C50C0E90255C0E90455C0E9065501F004 +:10195000AFF863681B6823B129462046BDE8704094 +:10196000184770BD037880F868300523037043681A +:101970001B6810B504460BB1042198470023A360EF +:1019800010BD000090F86820436802701B680BB11E +:10199000052118477047000070B590F84C30044698 +:1019A00013B1002380F84C3004F15C02204601F0B2 +:1019B00091F963689B68B3B994F85C3013F06005E3 +:1019C00035D00021204601F0F7FB0021204601F030 +:1019D000E9FB63681B6813B106212046984706237C +:1019E00084F84C3070BD204698470028E4D0B4F805 +:1019F0006230626D9A4288BF636594F95C30656DB0 +:101A0000002B4FF0200380F20381002D00F0F280C4 +:101A1000092284F84C2083F3118800212046D4E960 +:101A20001423FFF773FF002383F31188DAE794F898 +:101A30005D2003F07F0343EA022340F20232934227 +:101A400000F0C58021D8B3F5807F48D00DD8012B98 +:101A50003FD0022B00F09380002BB2D104F164023E +:101A6000226502226265A365C1E7B3F5817F00F0BC +:101A70009B80B3F5407FA4D194F85E30012BA0D1B8 +:101A8000B4F8643043F0020332E0B3F5006F4DD098 +:101A900017D8B3F5A06F31D0A3F5C063012B90D850 +:101AA0006368204694F85E205E6894F85F10B4F88E +:101AB0006030B047002884D04368236503686365BD +:101AC0001AE0B3F5106F36D040F6024293427FF42D +:101AD00078AF5C4B2365022363650023C3E794F86A +:101AE0005E30012B7FF46DAFB4F8643023F0020355 +:101AF000A4F86430C4E91455A56578E7B4F85C30FF +:101B0000B3F5A06F0ED194F85E30204684F86630AD +:101B100001F026F863681B6813B10121204698473D +:101B2000032323700023C4E914339CE704F1670303 +:101B300023650123C3E72378042B10D1202383F3EB +:101B400011882046FFF7C4FE85F3118803216368DE +:101B500084F8675021701B680BB12046984794F8B1 +:101B60005E30002BDED084F8673004232370636876 +:101B70001B68002BD6D0022120469847D2E794F864 +:101B8000603020461D0603F00F010AD501F094F8DD +:101B9000012804D002287FF414AF2B4B9AE72B4B7B +:101BA00098E701F07BF8F3E794F85E30002B7FF4C0 +:101BB00008AF94F8603013F00F01B3D01A06204636 +:101BC00002D501F00DFBADE701F000FBAAE794F8A8 +:101BD0005E30002B7FF4F5AE94F8603013F00F0107 +:101BE000A0D01B06204602D501F0E6FA9AE701F0E4 +:101BF000D9FA97E7142284F84C2083F311882B46F6 +:101C00002A4629462046FFF76FFE85F31188E9E64C +:101C10005DB1152284F84C2083F311880021204601 +:101C2000D4E91423FFF760FEFDE60B2284F84C2074 +:101C300083F311882B462A4629462046FFF766FE85 +:101C4000E3E700BFC43B0008BC3B0008C03B000802 +:101C500038B590F84C300446002B3ED0063BDAB243 +:101C60000F2A34D80F2B32D8DFE803F03731310890 +:101C7000223231313131313131313737456DB0F8C0 +:101C800062309D4214D2C3681B8AB5FBF3F203FB9A +:101C900012556DB9202383F311882B462A46294615 +:101CA000FFF734FE85F311880A2384F84C300EE0E8 +:101CB000142384F84C30202383F31188002320461A +:101CC0001A461946FFF710FE002383F3118838BD2A +:101CD000836D03B198470023E7E70021204601F018 +:101CE0006BFA0021204601F05DFA63681B6813B1AE +:101CF0000621204698470623D7E7000010B590F844 +:101D00004C300446142B29D017D8062B05D001D807 +:101D10001BB110BD093B022BFBD80021204601F06E +:101D20004BFA0021204601F03DFA63681B6813B1AD +:101D3000062120469847062319E0152BE9D10B23ED +:101D400080F84C30202383F3118800231A4619466B +:101D5000FFF7DCFD002383F31188DAE7C3689B6992 +:101D60005B68002BD5D1836D03B19847002384F8BD +:101D70004C30CEE7024B0022C3E900339A60704733 +:101D800040260220002382680374054B1B68996873 +:101D90009142FBD25A6803604260106058607047FD +:101DA0004026022008B5202383F31188037C032BEF +:101DB00005D0042B0DD02BB983F3118808BD4369DE +:101DC00000221A604FF0FF334361FFF7DBFF00236F +:101DD000F2E7D0E9003213605A60F3E7002382682B +:101DE0000374054B1B6899689142FBD85A680360DD +:101DF000426010605860704740260220054B196908 +:101E000008741868026853601A601861012303742B +:101E1000FEF78CBA402602204B1C30B5044687B032 +:101E20000A4D10D02B6901A8094A00F001F920469B +:101E3000FFF7E4FF049B13B101A800F035F92B690B +:101E4000586907B030BDFFF7D9FFF8E740260220F8 +:101E5000A51D000838B50C4D044641612B69816809 +:101E60009A68914203D8BDE83840FFF78BBF184607 +:101E7000FFF7B4FF01232C61014623742046BDE81F +:101E80003840FEF753BA00BF40260220044B1A68C0 +:101E90001B6990689B68984294BF0020012070479E +:101EA0004026022010B5084C236820691A68546047 +:101EB0002260012223611A74FFF790FF0146206916 +:101EC000BDE81040FEF732BA40260220FFF7EABF15 +:101ED000FEE7000010B50C4CFFF74CFF00F09AF83D +:101EE00080220A49204600F021F8012344F8180C0A +:101EF000037400F043FC002383F3118862B60448A6 +:101F0000BDE8104000F032B868260220C83B000847 +:101F1000D83B000800F002B9034A516853685B1AC5 +:101F20009842FBD8704700BF001000E08260022298 +:101F3000028270478368A3F17C0243F80C2C02698B +:101F400043F83C2C426943F8382C074A43F81C2CD0 +:101F5000C268A3F1180043F8102C022203F8082CE1 +:101F6000002203F8072C70474503000810B5202312 +:101F700083F31188FFF7DEFF00210446FFF76AFFB5 +:101F8000002383F31188204610BD0000024B1B691B +:101F900058610F20FFF732BF40260220202383F331 +:101FA0001188FFF7F3BF000008B50146202383F333 +:101FB00011880820FFF730FF002383F3118808BD44 +:101FC00049B1064B42681B6918605A601360436050 +:101FD0000420FFF721BF4FF0FF307047402602205A +:101FE0000368984206D01A6802605060184659612A +:101FF000FFF7C8BE7047000038B504460D4620689C +:10200000844200D138BD036823605C604561FFF7FE +:10201000B9FEF4E7054B4FF0FF3103F11402C3E9B9 +:1020200005220022C3E90712704700BF40260220A4 +:1020300070B51C4E05460C46C0E9032301F0CEFAEC +:10204000334653F8142F9A420DD130620A2C2CBF1C +:1020500000190A302A60C5E90124C6E90555BDE822 +:10206000704001F0A9BA316A431AE31838BF1C4620 +:102070009368A34202D9081901F0ACFA73699A680F +:1020800094420CD85A68AC602B606A6015609A68FC +:102090005D60121B9A604FF0FF33F36170BDA41AAC +:1020A0001B68ECE74026022038B51B4C6369984258 +:1020B0000DD08168D0E9003213605A600022C260FE +:1020C0009A680A449A604FF0FF33E36138BD0368B1 +:1020D0002246002142F8143F93425A60C16003D166 +:1020E000BDE8384001F070BA9A688168256A0A44F0 +:1020F0009A6001F073FA6369411B9A688A42E5D9D4 +:10210000AB181D1A206A092D98BF01F10A02BDE81B +:102110003840104401F05EBA402602202DE9F0411B +:10212000184C002704F11406656901F057FA236A78 +:10213000AA68C11A8A4215D81344D5F80C802362C4 +:10214000D5E9003213605A606369EF60B34201D190 +:1021500001F03AFA87F311882869C047202383F3F6 +:102160001188E1E76169B14209D013441B1ABDE847 +:10217000F0410A2B2CBFC0180A3001F02BBABDE881 +:10218000F08100BF4026022000207047FEE70000DB +:10219000704700004FF0FF307047000002290CD05C +:1021A000032904D00129074818BF00207047032ADB +:1021B00005D8054800EBC20070470448704700206E +:1021C000704700BFBC3C000848220220703C000859 +:1021D00070B59AB005460846144601A900F0C2F849 +:1021E00001A8FEF7BFFD431C0022C6B25B001046EB +:1021F000C5E9003423700323023404F8013C01AB29 +:10220000D1B202348E4201D81AB070BD13F8011B4E +:10221000013204F8010C04F8021CF1E708B5202390 +:1022200083F311880348FFF795FA002383F311889D +:1022300008BD00BFF827022090F85C3003F01F02B1 +:10224000012A07D190F85D200B2A03D10023C0E9B1 +:10225000143315E003F06003202B08D1B0F8603090 +:102260002BB990F85D20212A03D81F2A04D8FFF744 +:1022700053BA222AEBD0FAE7034A026507224265E5 +:1022800083650120704700BF3F22022007B5052962 +:1022900017D8DFE801F0191603191920202383F35A +:1022A0001188104A01210190FFF7F8FA01980221E4 +:1022B0000D4AFFF7F3FA0D48FFF718FA002383F3EE +:1022C000118803B05DF804FB202383F311880748CD +:1022D000FFF7E2F9F2E7202383F311880348FFF7C1 +:1022E000F9F9EBE7103C0008343C0008F82702201D +:1022F00038B50C4D0C4C2A460C4904F10800FFF788 +:1023000067FF05F1CA0204F110000949FFF760FFF9 +:1023100005F5CA7204F118000649BDE83840FFF718 +:1023200057BF00BFD030022048220220F03B0008F7 +:10233000FA3B0008053C000870B5044608460D4607 +:10234000FEF710FDC6B22046013403780BB91846DB +:1023500070BD32462946FEF7F1FC0028F3D101207A +:10236000F6E700002DE9F04705460C46FEF7FAFCBB +:102370002B49C6B22846FFF7DFFF08B10E36F6B28A +:1023800028492846FFF7D8FF08B11036F6B2632E69 +:102390000BD8DFF88C80DFF88C90234FDFF894A007 +:1023A0002E7846B92670BDE8F08729462046BDE85C +:1023B000F04701F0ADBB252E2ED10722414628461D +:1023C000FEF7BCFC70B9194B224603F1140153F817 +:1023D000040B8B4242F8040BF9D11B7807351534F6 +:1023E0001370DDE7082249462846FEF7A7FC98B996 +:1023F000A21C0F4B197802320909C95D02F8041CAE +:1024000013F8011B01F00F015345C95D02F8031CCD +:10241000F0D118340835C3E7013504F8016BBFE784 +:10242000DC3C0008053C0008FA3C0008E43C0008DD +:1024300020F4F01F2CF4F01FBFF34F8F024AD36833 +:10244000DB03FCD4704700BF003C024008B5074BDB +:102450001B7853B9FFF7F0FF054B1A69002A04DA1D +:10246000044A5A6002F188325A6008BD2E330220B5 +:10247000003C02402301674508B5054B1B7833B982 +:10248000FFF7DAFF034A136943F00043136108BD05 +:102490002E330220003C02400B28F0B516D80C4C1D +:1024A0000C4923787BB90E460B4D0C234FF000628C +:1024B000013B55F8047B46F8042B13F0FF033A4424 +:1024C000F6D10123237051F82000F0BD0020FCE775 +:1024D00060330220303302200C3D0008014B53F8DA +:1024E000200070470C3D00080C2070470B2810B5E9 +:1024F000044601D9002010BDFFF7CEFF064B53F86C +:1025000024301844C21A0BB90120F4E712680132D2 +:10251000F0D1043BF6E700BF0C3D00080B2810B5D6 +:10252000044621D8FFF788FFFFF790FF0F4AF323F7 +:10253000D360C300DBB243F4007343F002031361C2 +:10254000136943F480331361FFF776FFFFF7A4FFAD +:10255000074B53F8241000F0D9F82046FFF78CFF02 +:10256000BDE81040FFF7C2BF002010BD003C024094 +:102570000C3D0008F8B512F00103144642D1821850 +:10258000B2F1026F57D82D4B1B6813F0010352D0E4 +:10259000FFF75CFF2A4DF32340F20127EB60FFF7C2 +:1025A0004BFF032C15D824F00104401A40F2011708 +:1025B0006618234CB14200EB0105236924D123F0B6 +:1025C000010301202361FFF757FFF8BD043C0430ED +:1025D000E7E78307E7D12B6923F440732B612B696D +:1025E0003B432B6151F8046B0660BFF34F8FFFF73D +:1025F00023FF03689E42E9D02B6923F001032B617E +:10260000FFF73AFF0020E0E723F4407323612369DA +:102610003B4323610B882B80BFF34F8FFFF70CFFE9 +:102620002D88ADB231F8023BAB42C3D0236923F011 +:1026300001032361E4E71846C7E700BF0038024002 +:10264000003C0240084908B50B7828B11BB9FFF7D8 +:10265000FDFE01230B7008BD002BFCD00870BDE807 +:102660000840FFF709BF00BF2E3302200244074B8A +:10267000D2B210B5904200D110BD441C00B253F844 +:10268000200041F8040BE0B2F4E700BF50280040FE +:102690000F4B30B51C6F240407D41C6F44F4007436 +:1026A0001C671C6F44F400441C670A4C02442368F6 +:1026B000D2B243F480732360074B904200D130BD07 +:1026C000441C51F8045B00B243F82050E0B2F4E738 +:1026D00000380240007000405028004007B5012239 +:1026E00001A90020FFF7C2FF019803B05DF804FBC9 +:1026F00013B50446FFF7F2FFA04205D0012201A95D +:1027000000200194FFF7C4FF02B010BD0144BFF3E5 +:102710004F8F064B884204D3BFF34F8FBFF36F8FA9 +:102720007047C3F85C022030F4E700BF00ED00E022 +:10273000034B1A681AB9034AD2F874281A60704712 +:10274000643302200030024008B5FFF7F1FF024B6E +:102750001868C0F3407008BD64330220EFF30983AA +:10276000054968334A6B22F001024A6383F3098802 +:10277000002383F31188704700EF00E0202080F3EE +:10278000118862B60D4B0E4AD96821F4E061090444 +:10279000090C0A430B49DA60D3F8FC2042F080723E +:1027A000C3F8FC20084AC2F8B01F116841F00101CB +:1027B00011601022DA7783F82200704700ED00E004 +:1027C0000003FA0555CEACC5001000E0202310B57B +:1027D00083F311880E4B5B6813F4006314D0F1EEA1 +:1027E000103AEFF309844FF08073683CE361094BC2 +:1027F000DB6B236684F30988FFF748FB10B1064BB7 +:10280000A36110BD054BFBE783F31188F9E700BF17 +:1028100000ED00E000EF00E0570300085A03000855 +:1028200070B5BFF34F8FBFF36F8F1A4A0021C2F804 +:102830005012BFF34F8FBFF36F8F536943F40033D0 +:102840005361BFF34F8FBFF36F8FC2F88410BFF394 +:102850004F8FD2F8803043F6E074C3F3C900C3F35E +:102860004E335B0103EA0406014646EA81750139ED +:10287000C2F86052F9D2203B13F1200FF2D1BFF31E +:102880004F8F536943F480335361BFF34F8FBFF3CE +:102890006F8F70BD00ED00E0FEE700000A4B0B48B3 +:1028A0000B4A90420BD30B4BC11EDA1C121A22F0BA +:1028B00003028B4238BF00220021FEF75BBA53F8B7 +:1028C000041B40F8041BECE7083F000878340220A2 +:1028D00078340220783402207047000070B5D0E9C7 +:1028E0001B4300224FF0FF359E6804EB4213510159 +:1028F000D3F80009002805DAD3F8000940F0804039 +:10290000C3F80009D3F8000B002805DAD3F8000B50 +:1029100040F08040C3F8000B013263189642C3F8C0 +:102920000859C3F8085BE0D24FF00113C4F81C3813 +:1029300070BD00001D4B03EB8002D2F80CC02DE9E6 +:10294000F043DCF81420461CDD6ED2F800E005EB05 +:10295000063605EB4018516871450AD30122D5F8B7 +:10296000343802FA00F023EA0000C5F83408BDE864 +:10297000F083AEEB0103BCF81040A34228BF23460E +:10298000D8F81849A4B2B3EB840FF0D89468A4F136 +:10299000040959F8047F3760A4EB09071F44042F8A +:1029A000F7D81C440B4494605360D4E7683302208A +:1029B000890141F02001016103699B06FCD41220CA +:1029C000FFF7AABA10B5054C2046FEF745FF4FF0B9 +:1029D000A043E366024B236710BD00BF68330220AB +:1029E000603D000870B503780546012B50D12A4B95 +:1029F000C46E98421BD1294B0E2143205A6B42F0E2 +:102A000080025A635A6D42F080025A655A6D5A69C3 +:102A100042F080025A615A6922F080025A615B6971 +:102A200000F0E8FB1E4BE3601E4BC4F800380023A7 +:102A3000EE6EC4F8003EC02323604FF40413A3637A +:102A40003369002BFCDA01230C203361FFF764FAB1 +:102A50003369DB07FCD41220FFF75EFA3369002BE1 +:102A6000FCDA00262846A660FFF738FF6B68C4F83A +:102A70001068DB68C4F81468C4F81C684BB90A4BCA +:102A8000A3614FF0FF336361A36843F00103A360C8 +:102A900070BD064BF4E700BF6833022000380240E7 +:102AA0004014004003002002003C30C0083C30C00D +:102AB000F8B5C46E054600214FF000662046FFF7CA +:102AC00077FF296F00234FF001128F684FF0FF301E +:102AD000C4F83438C4F81C2804EB431201339F4275 +:102AE000C2F80069C2F8006BC2F80809C2F8080B06 +:102AF000F2D20B68EA6E6B6763621023136116698A +:102B000016F01006FBD11220FFF706FAD4F80038B1 +:102B100023F4FE63C4F80038A36943F4402343F070 +:102B20001003A3610923C4F81038C4F814380A4B01 +:102B3000EB604FF0C043C4F8103B084BC4F8003BB7 +:102B4000C4F81069C4F800396B6F03F1100243F444 +:102B500080136A67A362F8BD3C3D00084080001006 +:102B6000C26E90F86610D2F8003823F4FE6343EA90 +:102B70000113C2F8003870472DE9F84300EB8103D8 +:102B8000C56E0C468046DA680FFA81F9480116686E +:102B900006F00306731E022B05EB41134FF00001F4 +:102BA00094BFB604384EC3F8101B4FF0010104F176 +:102BB000100398BF06F1805601FA03F3916998BF9C +:102BC00006F5004600293AD0578A04F158013743E8 +:102BD00049016F50D5F81C180B430021C5F81C386B +:102BE0002B180127C3F81019A7405369611E9BB326 +:102BF000138A928B9B08012A88BF5343D8F874200C +:102C0000981842EA034301F140022146C8F87400D3 +:102C1000284605EB82025360FFF7CAFE08EB8900E5 +:102C2000C3681B8A43EA845348341E4364012E510F +:102C3000D5F81C381F43C5F81C78BDE8F88305EBB0 +:102C40004917D7F8001B21F40041C7F8001BD5F83D +:102C50001C1821EA0303C0E704F13F030B4A28468E +:102C6000214605EB83035A60FFF7A2FE05EB4910EE +:102C7000D0F8003923F40043C0F80039D5F81C38E7 +:102C800023EA0707D7E700BF008000100004000216 +:102C9000026F12684267FFF721BE00005831C36E11 +:102CA00049015B5813F4004004D013F4001F0CBF1B +:102CB00002200120704700004831C36E49015B5873 +:102CC00013F4004004D013F4001F0CBF02200120B5 +:102CD0007047000000EB8101CB68196A0B68136034 +:102CE0004B6853607047000000EB810330B5DD682E +:102CF000AA691368D36019B9402B84BF40231360BD +:102D00006B8A1468C26E1C4402EB4110013C09B28C +:102D1000B4FBF3F46343033323F0030343EAC443F4 +:102D200043F0C043C0F8103B2B6803F00303012BB2 +:102D30000ED1D2F8083802EB411013F4807FD0F89E +:102D4000003B14BF43F0805343F00053C0F8003BF6 +:102D500002EB4112D2F8003B43F00443C2F8003BBF +:102D600030BD00002DE9F041244D0446EE6E06EB27 +:102D70004013D3F8087B3807C3F8087B0AD5D6F888 +:102D80001438190706D505EB840321462846DB686D +:102D90005B689847FA071FD5D6F81438DB071BD5B0 +:102DA00005EB8403D968CCB98B69488A5A68B2FBB1 +:102DB000F0F600FB16228AB91868DA6890420DD244 +:102DC000121AC3E90024202383F311880B482146FB +:102DD000FFF78AFF84F31188BDE8F081012303FA2D +:102DE00004F26B8923EA02036B81CB68002BF3D0DA +:102DF00021460248BDE8F041184700BF6833022071 +:102E000000EB81034A0170B5DD68C36E6C6926680A +:102E1000E66056BB1A444FF40020C2F810092A6835 +:102E200002F00302012A0AB20ED1D3F8080803EB1C +:102E3000421410F4807FD4F8000914BF40F0805091 +:102E400040F00050C4F8000903EB4212D2F8000928 +:102E500040F00440C2F800090122D3F8340802FA15 +:102E600001F10143C3F8341870BD19B9402E84BF75 +:102E70004020206020681A442E8A8419013CB4FB4B +:102E8000F6F440EAC44040F00050C6E7F8B5044606 +:102E90001E48C56E05EB4413D3F80869F107C3F863 +:102EA000086917D5D5F81038DA0713D500EB840375 +:102EB000D9684B691F68DA68974218D2D21B00277D +:102EC0001A605F60202383F311882146FFF798FF83 +:102ED00087F31188330617D50123D5F83428A3408A +:102EE000134211D02046BDE8F840FFF723BD01236F +:102EF00003FA04F2038923EA020303818B68002B9F +:102F0000E8D021469847E5E7F8BD00BF68330220C6 +:102F10002DE9F74FA24CE66E7569B3691D4015F4B3 +:102F20008058756107D02046FEF702FD03B0BDE86A +:102F3000F04FFFF74BBC002D12DAD6F8003E9F078A +:102F400005D0D6F8003E23F00303C6F8003ED6F8BD +:102F50000438934823F00103C6F80438FEF712FD45 +:102F6000280505D58E48FFF7B9FC8D48FEF7FAFC19 +:102F7000A9040CD5D6F8083813F0060FF36823F42B +:102F800070530CBF43F4105343F4A053F3602A076B +:102F900004D56368DB680BB181489847EB0200F108 +:102FA0008A80AF0227D5D4F86C9000274FF0010A31 +:102FB000DFF8ECB109EB4712D2F8003B03F44023F1 +:102FC000B3F5802F11D1D2F8003B002B0DDA6289C6 +:102FD0000AFA07F322EA0303638104EB8703DB6841 +:102FE000DB6813B13946584698470137236FFFB263 +:102FF0009B689F42DED9E80618D5E76E3A6AC2F3AD +:103000000A1002F00F0302F4F012B2F5802F00F064 +:103010009C80B2F5402F09D104EB8303002207F511 +:103020008057DB681B6A904240F082802B03D6F801 +:1030300018481DD5E70302D50020FFF793FEA6032D +:1030400002D50120FFF78EFE600302D50220FFF7B4 +:1030500089FE210302D50320FFF784FEE20202D598 +:103060000420FFF77FFEA30202D50520FFF77AFEBA +:103070006F037FF55BAFE60702D50020FFF706FF81 +:10308000A50702D50120FFF701FF600702D5022046 +:10309000FFF7FCFE210702D50320FFF7F7FEE2064B +:1030A00002D50420FFF7F2FEA3067FF53FAF05200F +:1030B000FFF7ECFE3AE7E36E00274FF0010ADFF876 +:1030C000E0B00193236F5FFA87F99B6899453FF65B +:1030D00068AF019B03EB4913D3F8002902F44022A7 +:1030E000B2F5802F22D1D3F80029002A1EDAD3F8B6 +:1030F000002942F09042C3F80029D3F80029002AA1 +:10310000FBDB4946E06EFFF753FC22890AFA09F31C +:1031100022EA0303238104EB8903DB689B6813B174 +:103120004946584698474846FFF704FC0137C9E727 +:10313000910701D1D7F80080072A02F101029CBF54 +:1031400003F8018B4FEA18286DE7023307F5805723 +:1031500004EB83025268D2F818C0DCF80820DCE9DE +:10316000001CA1EB0C0C002188420AD104EB830463 +:1031700063689B699A6802449A605A6802445A607C +:1031800054E711F0030F01D1D7F800808C4501F10D +:10319000010184BF02F8018B4FEA1828E4E700BF61 +:1031A00068330220C36E03EB4111D1F8003B43F4B6 +:1031B0000013C1F8003B7047C36E03EB4111D1F817 +:1031C000003943F40013C1F800397047C36E03EBB4 +:1031D0004111D1F8003B23F40013C1F8003B7047C4 +:1031E000C36E03EB4111D1F8003923F40013C1F889 +:1031F00000397047090100F16043012203F5614382 +:10320000C9B283F8001300F01F039A4043099B00E2 +:1032100003F1604303F56143C3F880211A607047EE +:1032200030B50433039C0172002104FB0325C16007 +:10323000C0E90653049B0363059BC0E90000C0E995 +:103240000422C0E90842C0E90A11436330BD00000E +:103250000022416AC260C0E90411C0E90A226FF08D +:103260000101FEF7C9BE0000D0E90432934201D14A +:10327000C2680AB9181D70470020704703691960B9 +:103280000021C2680132C260C2691344826993425C +:10329000036124BF436A0361FEF7A2BE38B504464A +:1032A0000D46E3683BB162690020131D1268A362FA +:1032B0001344E36207E0237A33B929462046FEF738 +:1032C0007FFE0028EDDA38BD6FF00100FBE700005B +:1032D000C368C269013BC360436913448269934276 +:1032E000436124BF436A436100238362036B03B1DC +:1032F0001847704770B52023044683F31188866A07 +:103300003EB9FFF7CBFF054618B186F31188284672 +:1033100070BDA36AE26A13F8015B9342A36202D311 +:103320002046FFF7D5FF002383F31188EFE7000065 +:103330002DE9F84F04460E46174698464FF02009EF +:1033400089F311880025AA46D4F828B0BBF1000FF4 +:1033500009D141462046FFF7A1FF20B18BF3118828 +:103360002846BDE8F88FD4E90A12A7EB050B521ADC +:10337000934528BF9346BBF1400F1BD9334601F15B +:10338000400251F8040B914243F8040BF9D1A36AAF +:10339000403640354033A362D4E90A239A4202D32F +:1033A0002046FFF795FF8AF31188BD42D8D289F3F2 +:1033B0001188C9E730465A46FDF7B6FCA36A5E4459 +:1033C0005D445B44A362E7E710B5029C04330172DD +:1033D00004FB0321C460C0E906130023C0E90A33DB +:1033E000039B0363049BC0E90000C0E90422C0E919 +:1033F0000842436310BD0000026A6FF00101C26021 +:10340000426AC0E904220022C0E90A22FEF7F4BDA4 +:10341000D0E904239A4201D1C26822B9184650F873 +:10342000043B0B607047002070470000C368002118 +:10343000C2690133C36043691344826993424361A3 +:1034400024BF436A4361FEF7CBBD000038B5044694 +:103450000D46E3683BB1236900201A1DA262E269B0 +:103460001344E36207E0237A33B929462046FEF786 +:10347000A7FD0028EDDA38BD6FF00100FBE7000082 +:1034800003691960C268013AC260C2691344826963 +:103490009342036124BF436A036100238362036B89 +:1034A00003B118477047000070B520230D4604464D +:1034B000114683F31188866A2EB9FFF7C7FF10B152 +:1034C00086F3118870BDA36A1D70A36AE26A013396 +:1034D0009342A36204D3E16920460439FFF7D0FF89 +:1034E000002080F31188EDE72DE9F84F04460D46E2 +:1034F000904699464FF0200A8AF311880026B34679 +:10350000A76A4FB949462046FFF7A0FF20B187F3CD +:1035100011883046BDE8F88FD4E90A073A1AA8EBBB +:103520000607974228BF1746402F1BD905F14003D5 +:1035300055F8042B9D4240F8042BF9D1A36A40367C +:103540004033A362D4E90A239A4204D3E1692046B6 +:103550000439FFF795FF8BF311884645D9D28AF3DA +:103560001188CDE729463A46FDF7DEFBA36A3D44C4 +:103570003E443B44A362E5E7D0E904239A4217D1D5 +:10358000C3689BB1836A8BB1043B9B1A0ED0136056 +:10359000C368013BC360C3691A4483699A420261EC +:1035A00024BF436A036100238362012318467047E6 +:1035B0000023FBE700F094B84FF0804300225863EB +:1035C0001A610222DA6070474FF080430022DA600D +:1035D000704700004FF08043586370474FF08043BE +:1035E000586A70474B6843608B688360CB68C360E0 +:1035F0000B6943614B6903628B6943620B6803602B +:103600007047000008B52C4B40F2FF712B481A6937 +:103610000A431A611A6922F4FF6222F007021A6152 +:103620001A691A6B0A431A631A6D0A431A65244A07 +:103630001B6D1146FFF7D6FF00F5806002F11C01FB +:10364000FFF7D0FF00F5806002F13801FFF7CAFFF5 +:1036500000F5806002F15401FFF7C4FF00F58060BF +:1036600002F17001FFF7BEFF00F5806002F18C01EE +:10367000FFF7B8FF00F5806002F1A801FFF7B2FF85 +:1036800000F5806002F1C401FFF7ACFF00F5806037 +:1036900002F1E001FFF7A6FF00F5806002F1FC01F6 +:1036A000FFF7A0FF02F58C7100F58060FFF79AFF2D +:1036B000BDE8084000F08AB800380240000002402F +:1036C0006C3D000808B500F003FAFEF703FCFFF7B5 +:1036D0002FF8BDE80840FEF70BBE00007047000061 +:1036E0000F4B1A6C42F001021A641A6E42F001028A +:1036F0001A660C4A1B6E936843F0010393604FF007 +:1037000080436B229A624FF0FF32DA6200229A61A4 +:103710005A63DA605A6001225A611A60704700BF2A +:1037200000380240002004E04FF0804208B51169E3 +:10373000D3680B40D9B29B076FEA0101116107D52D +:10374000202383F31188FEF7E5FB002383F3118820 +:1037500008BD00001B4B4FF0FF3000211A696FEAD3 +:1037600042526FEA52521A611A69C2F30A021A618E +:103770001A695A6958615A6959615A691A6A62F034 +:1037800080521A621A6A02F080521A621A6A5A6ADF +:1037900058625A6A59625A6A0B4A106840F480703B +:1037A0001060186F00F44070B0F5007F03D04FF444 +:1037B000803018671967536823F40073536000F072 +:1037C0005FB900BF0038024000700040364B4FF434 +:1037D0004041364A1A64364A11601A6842F00102C2 +:1037E0001A601A689207FCD59A6822F003029A6060 +:1037F0002D4B9A6812F00C02FBD1196801F0F90107 +:1038000019609A601A6842F480321A601A6890034C +:10381000FCD55A6F42F001025A67234B5A6F910749 +:10382000FCD5244A5A601A6842F080721A60204B14 +:103830005A685204FCD51A6842F480321A605A68F9 +:10384000D003FCD51A6842F400321A60184A536853 +:103850009903FCD5144B1A689201FCD5164A9A605C +:1038600040F20112C3F88C200022C3F8902040F2ED +:103870000733124A1360136803F00F03072BFAD1C2 +:10388000094B9A6842F002029A609A6802F00C02B0 +:10389000082AFAD15A6C42F480425A645A6E42F4B1 +:1038A00080425A665B6E7047003802400004001088 +:1038B00000700040086C400900948838003C0240C9 +:1038C000074A08B5536903F00103536123B1054A60 +:1038D00013680BB150689847BDE80840FEF776BF03 +:1038E000003C0140F8330220074A08B5536903F051 +:1038F0000203536123B1054A93680BB1D06898471E +:10390000BDE80840FEF762BF003C0140F8330220EA +:10391000074A08B5536903F00403536123B1054A0C +:1039200013690BB150699847BDE80840FEF74EBFD8 +:10393000003C0140F8330220074A08B5536903F000 +:103940000803536123B1054A93690BB1D0699847C5 +:10395000BDE80840FEF73ABF003C0140F8330220C2 +:10396000074A08B5536903F01003536123B1054AB0 +:10397000136A0BB1506A9847BDE80840FEF726BFAE +:10398000003C0140F8330220164B10B55C6904F48A +:1039900078725A61A30604D5134A936A0BB1D06AB0 +:1039A0009847600604D5104A136B0BB1506B9847CB +:1039B000210604D50C4A936B0BB1D06B9847E205F6 +:1039C00004D5094A136C0BB1506C9847A30504D574 +:1039D000054A936C0BB1D06C9847BDE81040FEF7D8 +:1039E000F5BE00BF003C0140F8330220194B10B572 +:1039F0005C6904F47C425A61620504D5164A136D71 +:103A00000BB1506D9847230504D5134A936D0BB144 +:103A1000D06D9847E00404D50F4A136E0BB1506E79 +:103A20009847A10404D50C4A936E0BB1D06E984709 +:103A3000620404D5084A136F0BB1506F98472304F2 +:103A400004D5054A936F0BB1D06F9847BDE810407D +:103A5000FEF7BCBE003C0140F833022008B5034825 +:103A6000FDF714FBBDE80840FEF7B0BED4230220EA +:103A700008B5FFF759FEBDE80840FEF7A7BE0000F5 +:103A8000062108B50846FFF7B5FB06210720FFF71A +:103A9000B1FB06210820FFF7ADFB06210920FFF747 +:103AA000A9FB06210A20FFF7A5FB06211720FFF737 +:103AB000A1FB06212820FFF79DFB07211C20FFF713 +:103AC00099FB0C215220BDE80840FFF793BB000092 +:103AD00008B5FFF73FFE00F00DF8FDF7E9FCFDF734 +:103AE000B9FEFDF791FDFFF7F9FDBDE80840FFF7CE +:103AF00061BD00000023054A19460133102BC2E9BD +:103B0000001102F10802F8D1704700BFF83302201B +:103B1000034611F8012B03F8012B002AF9D1704755 +:103B200053544D3332463F3F3F3F3F3F0053544D88 +:103B3000333246375B347C355D780053544D333235 +:103B400046375B367C375D7800000000F82702209E +:103B5000D4230220009600000000000000000000B6 +:103B60000000000000000000000000000000000055 +:103B70007914000865140008A11400088D140008C9 +:103B80009914000885140008711400085D140008D9 +:103B9000AD1400080000000085150008711500082C +:103BA000AD15000899150008A51500089115000825 +:103BB0007D15000869150008B9150008000000000F +:103BC00001000000000000006D61696E000000004F +:103BD00069646C6500000000D03B0008802602206C +:103BE000F827022001000000D11E0008000000009C +:103BF0004172647550696C6F740025424F415244A4 +:103C0000252D424C002553455249414C25000000CA +:103C10000200000000000000A11700080D180008B5 +:103C200040004000A0300220B0300220020000001E +:103C30000000000003000000000000005118000810 +:103C40000000000010000000C03002200000000052 +:103C500001000000000000006833022001010200A2 +:103C60008D2200089D210008392200081D2200082D +:103C700043000000783C000809024300020100C034 +:103C800032090400000102020100052400100105B0 +:103C90002401000104240202052406000107058214 +:103CA000030800FF09040100020A000000070501E3 +:103CB00002400000070581024000000012000000E1 +:103CC000C43C0008120110010200004009124157D3 +:103CD00000020102030100000403090425424F41D0 +:103CE000524425004D6174656B463736352D576952 +:103CF0006E672D626473686F74003031323334350F +:103D000036373839414243444546000000800000C0 +:103D10000080000000800000008000000000020021 +:103D20000000040000000400000004000000040083 +:103D30000000040000000400000004000000000077 +:103D400099190008511C0008FD1C000840004000A3 +:103D5000E0330220E033022001000000F0330220B3 +:103D60008000000040010000050000000000802AE3 +:103D700000000000AAAAAAAA00000024FFFF000079 +:103D80000000000000A00A00000000010000000088 +:103D9000AAAAAAAA00000001FFFF0000000000007C +:103DA000000000000001000000000000AAAAAAAA6A +:103DB00000010000FFFF0000000000000000000004 +:103DC0000040100000000000AAAAAAAA00400000BB +:103DD000FFFB000000000000000000000080420027 +:103DE00000000000AAAAAAAA00404100FFFF0000AC +:103DF000000000800800000000000000000000003B +:103E0000AAAAAAAA00000000FFFF0000000000000C +:103E1000000000000000000000000000AAAAAAAAFA +:103E200000000000FFFF0000000000000000000094 +:103E30000000000000000000AAAAAAAA00000000DA +:103E4000FFFF000000000000000000000000000074 +:103E500000000000AAAAAAAA00000000FFFF0000BC +:103E60000000000000000000000000000000000052 +:103E70000000000000000000000000000000000042 +:103E80000000000000000000000000000000000032 +:103E90000000000000000000000000000000000022 +:103EA0008F0000000000000000801E0000000000E5 +:103EB000FF00000000000000203B00083F00000061 +:103EC000490400002D3B00083F00000051040000A1 +:103ED0003B3B00083F000000009600000000080087 +:103EE000960000000008000004000000D83C000814 +:103EF00000000000000000000000000000000000C2 +:083F00000000000000000000B9 +:00000001FF From c770a2bd58eb71eb2139cfb6f6c53834df508f58 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Wed, 6 Apr 2022 18:35:50 -0500 Subject: [PATCH 0591/3101] hwdef: minimal GPS support for Flywoo baords --- libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat index 54af8561138..6d0bade20c9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/hwdef.dat @@ -166,6 +166,10 @@ define PRECISION_LANDING 0 define HAL_BARO_WIND_COMP_ENABLED 0 define GRIPPER_ENABLED 0 define HAL_PARACHUTE_ENABLED 0 + + +# minimal GPS drivers to reduce flash usage +include ../include/minimal_GPS.inc define HAL_SPRAYER_ENABLED 0 define AP_BATTMON_SMBUS_ENABLE 0 define AP_BATTMON_FUEL_ENABLE 0 From c1c18331f931b932b78d9aaa0d62a720fe1d82ed Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 09:47:04 +1000 Subject: [PATCH 0592/3101] AP_AHRS: remove instance id from EK2 external interface Removes passing of instance id in interfaces where -1 was the only value ever passed in --- libraries/AP_AHRS/AP_AHRS.cpp | 48 +++++++++++++++++------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 77ab99942b6..cacf7591e3b 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -492,7 +492,7 @@ void AP_AHRS::update_EKF2(void) if (active_EKF_type() == EKFType::TWO) { Vector3f eulers; EKF2.getRotationBodyToNED(_dcm_matrix); - EKF2.getEulerAngles(-1,eulers); + EKF2.getEulerAngles(eulers); roll = eulers.x; pitch = eulers.y; yaw = eulers.z; @@ -508,7 +508,7 @@ void AP_AHRS::update_EKF2(void) // get gyro bias for primary EKF and change sign to give gyro drift // Note sign convention used by EKF is bias = measurement - truth _gyro_drift.zero(); - EKF2.getGyroBias(-1,_gyro_drift); + EKF2.getGyroBias(_gyro_drift); _gyro_drift = -_gyro_drift; // calculate corrected gyro estimate for get_gyro() @@ -522,7 +522,7 @@ void AP_AHRS::update_EKF2(void) // get z accel bias estimate from active EKF (this is usually for the primary IMU) float &abias = _accel_bias.z; - EKF2.getAccelZBias(-1,abias); + EKF2.getAccelZBias(abias); // This EKF is currently using primary_imu, and abias applies to only that IMU for (uint8_t i=0; i<_ins.get_accel_count(); i++) { @@ -536,7 +536,7 @@ void AP_AHRS::update_EKF2(void) } _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()]; nav_filter_status filt_state; - EKF2.getFilterStatus(-1,filt_state); + EKF2.getFilterStatus(filt_state); update_notify_from_filter_status(filt_state); } } @@ -822,7 +822,7 @@ Vector3f AP_AHRS::wind_estimate(void) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getWind(-1,wind); + EKF2.getWind(wind); break; #endif @@ -987,7 +987,7 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const break; #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - return EKF2.getAirSpdVec(-1, vec); + return EKF2.getAirSpdVec(vec); #endif #if HAL_NAVEKF3_AVAILABLE @@ -1063,7 +1063,7 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const if (!_ekf2_started) { return false; } - EKF2.getQuaternion(-1, quat); + EKF2.getQuaternion(quat); break; #endif #if HAL_NAVEKF3_AVAILABLE @@ -1116,7 +1116,7 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // EKF2 is secondary - EKF2.getEulerAngles(-1, eulers); + EKF2.getEulerAngles(eulers); return _ekf2_started; #endif @@ -1174,7 +1174,7 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const if (!_ekf2_started) { return false; } - EKF2.getQuaternion(-1, quat); + EKF2.getQuaternion(quat); break; #endif @@ -1263,7 +1263,7 @@ Vector2f AP_AHRS::groundspeed_vector(void) #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getVelNED(-1,vec); + EKF2.getVelNED(vec); return Vector2f(vec.x, vec.y); #endif @@ -1376,7 +1376,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getVelNED(-1,vec); + EKF2.getVelNED(vec); return true; #endif @@ -1413,7 +1413,7 @@ bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getMagNED(-1,vec); + EKF2.getMagNED(vec); return true; #endif @@ -1444,7 +1444,7 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getMagXYZ(-1,vec); + EKF2.getMagXYZ(vec); return true; #endif @@ -1477,7 +1477,7 @@ bool AP_AHRS::get_vert_pos_rate(float &velocity) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - velocity = EKF2.getPosDownDerivative(-1); + velocity = EKF2.getPosDownDerivative(); return true; #endif @@ -1555,7 +1555,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const case EKFType::TWO: { Vector2f posNE; float posD; - if (EKF2.getPosNE(-1,posNE) && EKF2.getPosD(-1,posD)) { + if (EKF2.getPosNE(posNE) && EKF2.getPosD(posD)) { // position is valid vec.x = posNE.x; vec.y = posNE.y; @@ -1646,7 +1646,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { - bool position_is_valid = EKF2.getPosNE(-1,posNE); + bool position_is_valid = EKF2.getPosNE(posNE); return position_is_valid; } #endif @@ -1716,7 +1716,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { - bool position_is_valid = EKF2.getPosD(-1,posD); + bool position_is_valid = EKF2.getPosD(posD); return position_is_valid; } #endif @@ -1841,7 +1841,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const } if (always_use_EKF()) { uint16_t ekf2_faults; - EKF2.getFilterFaults(-1,ekf2_faults); + EKF2.getFilterFaults(ekf2_faults); if (ekf2_faults == 0) { ret = EKFType::TWO; } @@ -1897,7 +1897,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const nav_filter_status filt_state; #if HAL_NAVEKF2_AVAILABLE if (ret == EKFType::TWO) { - EKF2.getFilterStatus(-1,filt_state); + EKF2.getFilterStatus(filt_state); should_use_gps = EKF2.configuredToUseGPSForPosXY(); } #endif @@ -2155,7 +2155,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - EKF2.getFilterStatus(-1,status); + EKF2.getFilterStatus(status); return true; #endif @@ -2345,7 +2345,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: imu_idx = EKF2.getPrimaryCoreIMUIndex(); - EKF2.getAccelZBias(-1,accel_bias.z); + EKF2.getAccelZBias(accel_bias.z); break; #endif #if HAL_NAVEKF3_AVAILABLE @@ -2703,7 +2703,7 @@ bool AP_AHRS::get_origin(Location &ret) const #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - if (!EKF2.getOriginLLH(-1,ret)) { + if (!EKF2.getOriginLLH(ret)) { return false; } return true; @@ -2811,7 +2811,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f & #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: // use EKF to get innovations - return EKF2.getInnovations(-1, velInnov, posInnov, magInnov, tasInnov, yawInnov); + return EKF2.getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); #endif #if HAL_NAVEKF3_AVAILABLE @@ -2877,7 +2877,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3 case EKFType::TWO: { // use EKF to get variance Vector2f offset; - return EKF2.getVariances(-1, velVar, posVar, hgtVar, magVar, tasVar, offset); + return EKF2.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); } #endif From 04a64a20fa16e80e00a4a667ffde236f67f67fc0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 09:47:04 +1000 Subject: [PATCH 0593/3101] AP_NavEKF2: remove instance id from EK2 external interface Removes passing of instance id in interfaces where -1 was the only value ever passed in --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 102 ++++++++++------------------ libraries/AP_NavEKF2/AP_NavEKF2.h | 58 +++++++--------- 2 files changed, 61 insertions(+), 99 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 490ee7b1bb6..5add8077836 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -913,74 +913,59 @@ int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF2::getPosNE(int8_t instance, Vector2f &posNE) const +bool NavEKF2::getPosNE(Vector2f &posNE) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getPosNE(posNE); + return core[primary].getPosNE(posNE); } // Write the last calculated D position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF2::getPosD(int8_t instance, float &posD) const +bool NavEKF2::getPosD(float &posD) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getPosD(posD); + return core[primary].getPosD(posD); } // return NED velocity in m/s -void NavEKF2::getVelNED(int8_t instance, Vector3f &vel) const +void NavEKF2::getVelNED(Vector3f &vel) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getVelNED(vel); + core[primary].getVelNED(vel); } } // return estimate of true airspeed vector in body frame in m/s for the specified instance // An out of range instance (eg -1) returns data for the primary instance // returns false if estimate is unavailable -bool NavEKF2::getAirSpdVec(int8_t instance, Vector3f &vel) const +bool NavEKF2::getAirSpdVec(Vector3f &vel) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - return core[instance].getAirSpdVec(vel); + return core[primary].getAirSpdVec(vel); } return false; } // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s -float NavEKF2::getPosDownDerivative(int8_t instance) const +float NavEKF2::getPosDownDerivative() const { - if (instance < 0 || instance >= num_cores) instance = primary; // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration if (core) { - return core[instance].getPosDownDerivative(); + return core[primary].getPosDownDerivative(); } return 0.0f; } // return body axis gyro bias estimates in rad/sec -void NavEKF2::getGyroBias(int8_t instance, Vector3f &gyroBias) const +void NavEKF2::getGyroBias(Vector3f &gyroBias) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getGyroBias(gyroBias); - } -} - -// return body axis gyro scale factor error as a percentage -void NavEKF2::getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const -{ - if (instance < 0 || instance >= num_cores) instance = primary; - if (core) { - core[instance].getGyroScaleErrorPercentage(gyroScale); + core[primary].getGyroBias(gyroBias); } } @@ -1031,38 +1016,34 @@ void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainSca } // return the individual Z-accel bias estimates in m/s^2 -void NavEKF2::getAccelZBias(int8_t instance, float &zbias) const +void NavEKF2::getAccelZBias(float &zbias) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getAccelZBias(zbias); + core[primary].getAccelZBias(zbias); } } // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) -void NavEKF2::getWind(int8_t instance, Vector3f &wind) const +void NavEKF2::getWind(Vector3f &wind) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getWind(wind); + core[primary].getWind(wind); } } // return earth magnetic field estimates in measurement units / 1000 -void NavEKF2::getMagNED(int8_t instance, Vector3f &magNED) const +void NavEKF2::getMagNED(Vector3f &magNED) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getMagNED(magNED); + core[primary].getMagNED(magNED); } } // return body magnetic field estimates in measurement units / 1000 -void NavEKF2::getMagXYZ(int8_t instance, Vector3f &magXYZ) const +void NavEKF2::getMagXYZ(Vector3f &magXYZ) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getMagXYZ(magXYZ); + core[primary].getMagXYZ(magXYZ); } } @@ -1101,13 +1082,12 @@ bool NavEKF2::getLLH(struct Location &loc) const // An out of range instance (eg -1) returns data for the primary instance // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set -bool NavEKF2::getOriginLLH(int8_t instance, struct Location &loc) const +bool NavEKF2::getOriginLLH(struct Location &loc) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (!core) { return false; } - return core[instance].getOriginLLH(loc); + return core[primary].getOriginLLH(loc); } // set the latitude and longitude and height used to set the NED origin @@ -1148,11 +1128,10 @@ bool NavEKF2::getHAGL(float &HAGL) const } // return the Euler roll, pitch and yaw angle in radians for the specified instance -void NavEKF2::getEulerAngles(int8_t instance, Vector3f &eulers) const +void NavEKF2::getEulerAngles(Vector3f &eulers) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getEulerAngles(eulers); + core[primary].getEulerAngles(eulers); } } @@ -1176,35 +1155,31 @@ void NavEKF2::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const } // return the quaternions defining the rotation from NED to XYZ (autopilot) axes -void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const +void NavEKF2::getQuaternion(Quaternion &quat) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getQuaternion(quat); + core[primary].getQuaternion(quat); } } // return the innovations for the specified instance -bool NavEKF2::getInnovations(int8_t instance, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +bool NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { if (core == nullptr) { return false; } - if (instance < 0 || instance >= num_cores) instance = primary; - - return core[instance].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + return core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); } // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements -bool NavEKF2::getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +bool NavEKF2::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const { if (core == nullptr) { return false; } - if (instance < 0 || instance >= num_cores) instance = primary; - return core[instance].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + return core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); } // should we use the compass? This is public so it can be used for @@ -1265,11 +1240,10 @@ void NavEKF2::setTerrainHgtStable(bool val) 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ -void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const +void NavEKF2::getFilterFaults(uint16_t &faults) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getFilterFaults(faults); + core[primary].getFilterFaults(faults); } else { faults = 0; } @@ -1278,11 +1252,10 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const /* return filter status flags */ -void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) const +void NavEKF2::getFilterStatus(nav_filter_status &status) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getFilterStatus(status); + core[primary].getFilterStatus(status); } else { memset(&status, 0, sizeof(status)); } @@ -1291,11 +1264,10 @@ void NavEKF2::getFilterStatus(int8_t instance, nav_filter_status &status) const /* return filter gps quality check status */ -void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const +void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const { - if (instance < 0 || instance >= num_cores) instance = primary; if (core) { - core[instance].getFilterGpsStatus(status); + core[primary].getFilterGpsStatus(status); } else { memset(&status, 0, sizeof(status)); } @@ -1601,7 +1573,7 @@ void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt } } -// get a yaw estimator instance +// get yaw estimator instance const EKFGSF_yaw *NavEKF2::get_yawEstimator(void) const { if (core) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index a1f5b1a123d..20362c67b77 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -68,40 +68,30 @@ class NavEKF2 { // return -1 if no primary core selected int8_t getPrimaryCoreIMUIndex(void) const; - // Write the last calculated NE position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the primary instance + // Write the last calculated NE position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(int8_t instance, Vector2f &posNE) const; + bool getPosNE(Vector2f &posNE) const; - // Write the last calculated D position relative to the reference point (m) for the specified instance. - // An out of range instance (eg -1) returns data for the primary instance + // Write the last calculated D position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(int8_t instance, float &posD) const; + bool getPosD(float &posD) const; - // return NED velocity in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getVelNED(int8_t instance, Vector3f &vel) const; + // return NED velocity in m/s + void getVelNED(Vector3f &vel) const; - // return estimate of true airspeed vector in body frame in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance + // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable - bool getAirSpdVec(int8_t instance, Vector3f &vel) const; + bool getAirSpdVec(Vector3f &vel) const; - // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s for the specified instance - // An out of range instance (eg -1) returns data for the primary instance + // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF // but will always be kinematically consistent with the z component of the EKF position state - float getPosDownDerivative(int8_t instance) const; - - // return body axis gyro bias estimates in rad/sec for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getGyroBias(int8_t instance, Vector3f &gyroBias) const; + float getPosDownDerivative() const; - // return body axis gyro scale factor error as a percentage for the specified instance - // An out of range instance (eg -1) returns data for the primary instance - void getGyroScaleErrorPercentage(int8_t instance, Vector3f &gyroScale) const; + // return body axis gyro bias estimates in rad/sec + void getGyroBias(Vector3f &gyroBias) const; // reset body axis gyro bias estimates void resetGyroBias(void); @@ -119,19 +109,19 @@ class NavEKF2 { // return the Z-accel bias estimate in m/s^2 for the specified instance // An out of range instance (eg -1) returns data for the primary instance - void getAccelZBias(int8_t instance, float &zbias) const; + void getAccelZBias(float &zbias) const; // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) // An out of range instance (eg -1) returns data for the primary instance - void getWind(int8_t instance, Vector3f &wind) const; + void getWind(Vector3f &wind) const; // return earth magnetic field estimates in measurement units / 1000 for the specified instance // An out of range instance (eg -1) returns data for the primary instance - void getMagNED(int8_t instance, Vector3f &magNED) const; + void getMagNED(Vector3f &magNED) const; // return body magnetic field estimates in measurement units / 1000 for the specified instance // An out of range instance (eg -1) returns data for the primary instance - void getMagXYZ(int8_t instance, Vector3f &magXYZ) const; + void getMagXYZ(Vector3f &magXYZ) const; // Return estimated magnetometer offsets // Return true if magnetometer offsets are valid @@ -147,7 +137,7 @@ class NavEKF2 { // An out of range instance (eg -1) returns data for the primary instance // All NED positions calculated by the filter are relative to this location // Returns false if the origin has not been set - bool getOriginLLH(int8_t instance, struct Location &loc) const; + bool getOriginLLH(struct Location &loc) const; // set the latitude and longitude and height used to set the NED origin // All NED positions calculated by the filter will be relative to this location @@ -161,7 +151,7 @@ class NavEKF2 { // return the Euler roll, pitch and yaw angle in radians for the specified instance // An out of range instance (eg -1) returns data for the primary instance - void getEulerAngles(int8_t instance, Vector3f &eulers) const; + void getEulerAngles(Vector3f &eulers) const; // return the transformation matrix from XYZ (body) to NED axes void getRotationBodyToNED(Matrix3f &mat) const; @@ -170,15 +160,15 @@ class NavEKF2 { void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const; // return the quaternions defining the rotation from NED to autopilot axes - void getQuaternion(int8_t instance, Quaternion &quat) const; + void getQuaternion(Quaternion &quat) const; // return the innovations for the specified instance // An out of range instance (eg -1) returns data for the primary instance - bool getInnovations(int8_t index, Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; + bool getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; // return the innovation consistency test ratios for the specified instance // An out of range instance (eg -1) returns data for the primary instance - bool getVariances(int8_t instance, float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; + bool getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; // should we use the compass? This is public so it can be used for // reporting via ahrs.use_compass() @@ -211,19 +201,19 @@ class NavEKF2 { 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ - void getFilterFaults(int8_t instance, uint16_t &faults) const; + void getFilterFaults(uint16_t &faults) const; /* return filter gps quality check status for the specified instance An out of range instance (eg -1) returns data for the primary instance */ - void getFilterGpsStatus(int8_t instance, nav_gps_status &faults) const; + void getFilterGpsStatus(nav_gps_status &faults) const; /* return filter status flags for the specified instance An out of range instance (eg -1) returns data for the primary instance */ - void getFilterStatus(int8_t instance, nav_filter_status &status) const; + void getFilterStatus(nav_filter_status &status) const; // send an EKF_STATUS_REPORT message to GCS void send_status_report(mavlink_channel_t chan) const; From 2e20e264035a21c870eefb5a3e2f28f1db104d03 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:50 +1000 Subject: [PATCH 0594/3101] AP_HAL_ChibiOS: allow Volz protocol to be compiled out --- libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 478d4e537aa..f67a6d04eca 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2774,6 +2774,10 @@ def add_apperiph_defaults(f): #define AP_AIRSPEED_AUTOCAL_ENABLE 0 #endif +#ifndef AP_VOLZ_ENABLED +#define AP_VOLZ_ENABLED 0 +#endif + ''') From d97c8479662d93fb74131a2ef74f742a051938a4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:50 +1000 Subject: [PATCH 0595/3101] AP_Volz_Protocol: allow Volz protocol to be compiled out --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp | 12 +++++++----- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 10 ++++++++++ 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index e0084e47981..97851e4b20f 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -4,13 +4,14 @@ * Created on: Oct 31, 2017 * Author: guy */ -#include -#include - #include "AP_Volz_Protocol.h" -#if NUM_SERVO_CHANNELS + +#if AP_VOLZ_ENABLED + +#include #include +#include extern const AP_HAL::HAL& hal; @@ -158,4 +159,5 @@ void AP_Volz_Protocol::update_volz_bitmask(uint32_t new_bitmask) volz_time_frame_micros = channels_micros; } -#endif //NUM_SERVO_CHANNELS + +#endif // AP_VOLZ_ENABLED diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index adbf5edf13f..017d8fc7267 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -33,6 +33,14 @@ #pragma once +#include + +#ifndef AP_VOLZ_ENABLED +#define AP_VOLZ_ENABLED 1 +#endif + +#if AP_VOLZ_ENABLED + #include #include @@ -73,3 +81,5 @@ class AP_Volz_Protocol { AP_Int32 bitmask; bool initialised; }; + +#endif // AP_VOLZ_PROTOCOL From 3339d96573a1671c1f84e13fe9aaf4274a6e3fab Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:50 +1000 Subject: [PATCH 0596/3101] SRV_Channel: allow Volz protocol to be compiled out --- libraries/SRV_Channel/SRV_Channel.h | 4 +++- libraries/SRV_Channel/SRV_Channels.cpp | 18 ++++++++++++++---- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index a4e775dc2de..ed5a52e503b 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -575,11 +575,13 @@ class SRV_Channels { static SRV_Channel *channels; static SRV_Channels *_singleton; -#ifndef HAL_BUILD_AP_PERIPH +#if AP_VOLZ_ENABLED // support for Volz protocol AP_Volz_Protocol volz; static AP_Volz_Protocol *volz_ptr; +#endif +#ifndef HAL_BUILD_AP_PERIPH // support for SBUS protocol AP_SBusOut sbus; static AP_SBusOut *sbus_ptr; diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index e408a22e5f9..dbe565f8372 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -43,8 +43,11 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::_singleton; -#ifndef HAL_BUILD_AP_PERIPH +#if AP_VOLZ_ENABLED AP_Volz_Protocol *SRV_Channels::volz_ptr; +#endif + +#ifndef HAL_BUILD_AP_PERIPH AP_SBusOut *SRV_Channels::sbus_ptr; AP_RobotisServo *SRV_Channels::robotis_ptr; #endif // HAL_BUILD_AP_PERIPH @@ -184,11 +187,13 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { // @Units: Hz AP_GROUPINFO("_RATE", 18, SRV_Channels, default_rate, 50), -#ifndef HAL_BUILD_AP_PERIPH +#if AP_VOLZ_ENABLED // @Group: _VOLZ_ // @Path: ../AP_Volz_Protocol/AP_Volz_Protocol.cpp AP_SUBGROUPINFO(volz, "_VOLZ_", 19, SRV_Channels, AP_Volz_Protocol), +#endif +#ifndef HAL_BUILD_AP_PERIPH // @Group: _SBUS_ // @Path: ../AP_SBusOut/AP_SBusOut.cpp AP_SUBGROUPINFO(sbus, "_SBUS_", 20, SRV_Channels, AP_SBusOut), @@ -258,8 +263,11 @@ SRV_Channels::SRV_Channels(void) fetteconwire_ptr = &fetteconwire; #endif -#ifndef HAL_BUILD_AP_PERIPH +#if AP_VOLZ_ENABLED volz_ptr = &volz; +#endif + +#ifndef HAL_BUILD_AP_PERIPH sbus_ptr = &sbus; robotis_ptr = &robotis; #endif // HAL_BUILD_AP_PERIPH @@ -382,10 +390,12 @@ void SRV_Channels::push() { hal.rcout->push(); -#ifndef HAL_BUILD_AP_PERIPH +#if AP_VOLZ_ENABLED // give volz library a chance to update volz_ptr->update(); +#endif +#ifndef HAL_BUILD_AP_PERIPH // give sbus library a chance to update sbus_ptr->update(); From 73b86ccc0479674d5a87a7177ec8034009f5c7f8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:57 +1000 Subject: [PATCH 0597/3101] AP_HAL_ChibiOS: allow RobotisServo protocol to be compiled out --- libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index f67a6d04eca..7fa212f6e49 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2778,6 +2778,10 @@ def add_apperiph_defaults(f): #define AP_VOLZ_ENABLED 0 #endif +#ifndef AP_ROBOTISSERVO_ENABLED +#define AP_ROBOTISSERVO_ENABLED 0 +#endif + ''') From 9001058d1223dcc09b1927ec54cbdd88d5130433 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:57 +1000 Subject: [PATCH 0598/3101] AP_RobotisServo: allow RobotisServo protocol to be compiled out --- libraries/AP_RobotisServo/AP_RobotisServo.cpp | 11 ++++++----- libraries/AP_RobotisServo/AP_RobotisServo.h | 10 ++++++++++ 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/libraries/AP_RobotisServo/AP_RobotisServo.cpp b/libraries/AP_RobotisServo/AP_RobotisServo.cpp index b48876a62c1..b1c015a321f 100644 --- a/libraries/AP_RobotisServo/AP_RobotisServo.cpp +++ b/libraries/AP_RobotisServo/AP_RobotisServo.cpp @@ -34,15 +34,15 @@ * limitations under the License. */ +#include "AP_RobotisServo.h" + +#if AP_ROBOTISSERVO_ENABLED + #include #include #include #include -#include "AP_RobotisServo.h" - -#if NUM_SERVO_CHANNELS - extern const AP_HAL::HAL& hal; #define BROADCAST_ID 0xFE @@ -406,4 +406,5 @@ void AP_RobotisServo::update() send_command(i+1, REG_GOAL_POSITION, value, 4); } } -#endif //NUM_SERVO_CHANNELS + +#endif // AP_ROBOTISSERVO_ENABLED diff --git a/libraries/AP_RobotisServo/AP_RobotisServo.h b/libraries/AP_RobotisServo/AP_RobotisServo.h index 39e10b31e1a..b850f51b640 100644 --- a/libraries/AP_RobotisServo/AP_RobotisServo.h +++ b/libraries/AP_RobotisServo/AP_RobotisServo.h @@ -18,6 +18,14 @@ #pragma once +#include + +#ifndef AP_ROBOTISSERVO_ENABLED +#define AP_ROBOTISSERVO_ENABLED 1 +#endif + +#if AP_ROBOTISSERVO_ENABLED + #include #include @@ -65,3 +73,5 @@ class AP_RobotisServo { uint32_t last_send_us; uint32_t delay_time_us; }; + +#endif // AP_ROBOTISSERVO_ENABLED From a5be9d0f1810ded6f17dbfe5727071a6d33df53c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 17:20:57 +1000 Subject: [PATCH 0599/3101] SRV_Channel: allow RobotisServo protocol to be compiled out --- libraries/SRV_Channel/SRV_Channel.h | 4 +++- libraries/SRV_Channel/SRV_Channels.cpp | 21 ++++++++++++++++----- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index ed5a52e503b..c3e30c38e3f 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -585,11 +585,13 @@ class SRV_Channels { // support for SBUS protocol AP_SBusOut sbus; static AP_SBusOut *sbus_ptr; +#endif // HAL_BUILD_AP_PERIPH +#if AP_ROBOTISSERVO_ENABLED // support for Robotis servo protocol AP_RobotisServo robotis; static AP_RobotisServo *robotis_ptr; -#endif // HAL_BUILD_AP_PERIPH +#endif #if HAL_SUPPORT_RCOUT_SERIAL // support for BLHeli protocol diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index dbe565f8372..345913b7be0 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -49,8 +49,11 @@ AP_Volz_Protocol *SRV_Channels::volz_ptr; #ifndef HAL_BUILD_AP_PERIPH AP_SBusOut *SRV_Channels::sbus_ptr; +#endif + +#if AP_ROBOTISSERVO_ENABLED AP_RobotisServo *SRV_Channels::robotis_ptr; -#endif // HAL_BUILD_AP_PERIPH +#endif #if AP_FETTEC_ONEWIRE_ENABLED AP_FETtecOneWire *SRV_Channels::fetteconwire_ptr; @@ -205,10 +208,13 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { AP_SUBGROUPINFO(blheli, "_BLH_", 21, SRV_Channels, AP_BLHeli), #endif -#ifndef HAL_BUILD_AP_PERIPH +#if AP_ROBOTISSERVO_ENABLED // @Group: _ROB_ // @Path: ../AP_RobotisServo/AP_RobotisServo.cpp AP_SUBGROUPINFO(robotis, "_ROB_", 22, SRV_Channels, AP_RobotisServo), +#endif + +#ifndef HAL_BUILD_AP_PERIPH #if AP_FETTEC_ONEWIRE_ENABLED // @Group: _FTW_ @@ -269,8 +275,12 @@ SRV_Channels::SRV_Channels(void) #ifndef HAL_BUILD_AP_PERIPH sbus_ptr = &sbus; +#endif + +#if AP_ROBOTISSERVO_ENABLED robotis_ptr = &robotis; -#endif // HAL_BUILD_AP_PERIPH +#endif // AP_ROBOTISSERVO_ENABLED + #if HAL_SUPPORT_RCOUT_SERIAL blheli_ptr = &blheli; #endif @@ -398,11 +408,12 @@ void SRV_Channels::push() #ifndef HAL_BUILD_AP_PERIPH // give sbus library a chance to update sbus_ptr->update(); +#endif // HAL_BUILD_AP_PERIPH +#if AP_ROBOTISSERVO_ENABLED // give robotis library a chance to update robotis_ptr->update(); - -#endif // HAL_BUILD_AP_PERIPH +#endif #if HAL_SUPPORT_RCOUT_SERIAL // give blheli telemetry a chance to update From 961cf659d97d0deeb3b5f32a09ab8d830002c93e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 16:38:11 +1000 Subject: [PATCH 0600/3101] SRV_Channels: stop gating FETTECONEWIRE parameters on HAL_BUILD_BUILD_AP_PERIPH This would break fettech on AP_Periph. The define is off by default on AP_Periph. --- libraries/SRV_Channel/SRV_Channels.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 345913b7be0..511392661ce 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -214,16 +214,12 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = { AP_SUBGROUPINFO(robotis, "_ROB_", 22, SRV_Channels, AP_RobotisServo), #endif -#ifndef HAL_BUILD_AP_PERIPH - #if AP_FETTEC_ONEWIRE_ENABLED // @Group: _FTW_ // @Path: ../AP_FETtecOneWire/AP_FETtecOneWire.cpp AP_SUBGROUPINFO(fetteconwire, "_FTW_", 25, SRV_Channels, AP_FETtecOneWire), #endif -#endif // HAL_BUILD_AP_PERIPH - // @Param: _DSHOT_RATE // @DisplayName: Servo DShot output rate // @Description: This sets the DShot output rate for all outputs as a multiple of the loop rate. 0 sets the output rate to be fixed at 1Khz for low loop rates. This value should never be set below 500Hz. From c805310152d1ec8ac4b72acfc57b013fde424f4c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Apr 2022 16:41:04 +1000 Subject: [PATCH 0601/3101] Tools.py: add Volz, RobotisServo and FETTec to build_options.py Allows for enabling/disabling on custom build server --- Tools/scripts/build_options.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index d07dc19d8d1..12ef3ae716a 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -137,6 +137,10 @@ def __init__(self, Feature('GPS Drivers', 'NOVA', 'AP_GPS_NOVA_ENABLED', 'Enable NOVA GPS', 0, None), Feature('GPS Drivers', 'SBF', 'AP_GPS_SBF_ENABLED', 'Enable SBF GPS', 0, None), Feature('GPS Drivers', 'SIRF', 'AP_GPS_SIRF_ENABLED', 'Enable SiRF GPS', 0, None), + + Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None), + Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo Protocol', 0, None), + Feature('Actuators', 'FETTecOneWire', 'AP_FETTEC_ONEWIRE_ENABLED', 'Enable FETTec OneWire ESCs', 0, None), ] BUILD_OPTIONS.sort(key=lambda x: x.category) From 13464ebb20b1b71be832707101222fd3e86a6371 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 1 Apr 2022 20:57:47 +0100 Subject: [PATCH 0602/3101] AP_RCTelemetry: check EKF checks against failsafe defaults. --- libraries/AP_RCTelemetry/AP_RCTelemetry.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp index 76ccec2d5fd..aca7629fd3c 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp @@ -253,23 +253,23 @@ void AP_RCTelemetry::check_ekf_status(void) uint32_t now = AP_HAL::millis(); if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed // multiple errors can be reported at a time. Same setup as Mission Planner. - if (velVar >= 1) { + if (velVar >= 0.8f) { queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance"); check_ekf_status_timer = now; } - if (posVar >= 1) { + if (posVar >= 0.8f) { queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance"); check_ekf_status_timer = now; } - if (hgtVar >= 1) { + if (hgtVar >= 0.8f) { queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance"); check_ekf_status_timer = now; } - if (magVar.length() >= 1) { + if (magVar.length() >= 0.8f) { queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance"); check_ekf_status_timer = now; } - if (tasVar >= 1) { + if (tasVar >= 0.8f) { queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance"); check_ekf_status_timer = now; } From 4b6590734a2b8eceb4dc7d1702fa1c20c9e38e27 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 10:29:43 +1000 Subject: [PATCH 0603/3101] AP_GPS: move gps blob initialisation out to method --- libraries/AP_GPS/AP_GPS.cpp | 46 ++++++++++++++++++++++--------------- libraries/AP_GPS/AP_GPS.h | 1 + 2 files changed, 28 insertions(+), 19 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e2e0d623d80..e8b7c9b04ec 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -553,6 +553,32 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) initblob_state[instance].remaining = size; } +/* + initialise state for sending binary blob initialisation strings + */ +void AP_GPS::send_blob_start(uint8_t instance) +{ + if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { + static const char blob[] = UBLOX_SET_BINARY_115200; + send_blob_start(instance, blob, sizeof(blob)); + } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || + _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && + ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { + // we use 460800 when doing moving baseline as we need + // more bandwidth. We don't do this if using UART2, as + // in that case the RTCMv3 data doesn't go over the + // link to the flight controller + static const char blob[] = UBLOX_SET_BINARY_460800; + send_blob_start(instance, blob, sizeof(blob)); +#if AP_GPS_NMEA_ENABLED + } else if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); +#endif // AP_GPS_NMEA_ENABLED + } else { + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); + } +} + /* send some more initialisation string bytes if there is room in the UART transmit buffer @@ -684,25 +710,7 @@ void AP_GPS::detect_instance(uint8_t instance) dstate->last_baud_change_ms = now; if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { - if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { - static const char blob[] = UBLOX_SET_BINARY_115200; - send_blob_start(instance, blob, sizeof(blob)); - } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || - _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && - ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { - // we use 460800 when doing moving baseline as we need - // more bandwidth. We don't do this if using UART2, as - // in that case the RTCMv3 data doesn't go over the - // link to the flight controller - static const char blob[] = UBLOX_SET_BINARY_460800; - send_blob_start(instance, blob, sizeof(blob)); -#if AP_GPS_NMEA_ENABLED - } else if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); -#endif // AP_GPS_NMEA_ENABLED - } else { - send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); - } + send_blob_start(instance); } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 65c37b2a680..cf73c3985d5 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -473,6 +473,7 @@ class AP_GPS bool blend_health_check() const; // handle sending of initialisation strings to the GPS - only used by backends + void send_blob_start(uint8_t instance); void send_blob_start(uint8_t instance, const char *_blob, uint16_t size); void send_blob_update(uint8_t instance); From aa72559d009a93bc72f0c7b609614bc598601245 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 10:44:42 +1000 Subject: [PATCH 0604/3101] AP_GPS: tidy initialisation of sending blobs --- libraries/AP_GPS/AP_GPS.cpp | 45 ++++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e8b7c9b04ec..158fd859e14 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -554,29 +554,38 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) } /* - initialise state for sending binary blob initialisation strings + initialise state for sending binary blob initialisation strings. We + may choose different blobs not just based on type but also based on + parameters. */ void AP_GPS::send_blob_start(uint8_t instance) { - if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { - static const char blob[] = UBLOX_SET_BINARY_115200; - send_blob_start(instance, blob, sizeof(blob)); - } else if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || - _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && - ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { - // we use 460800 when doing moving baseline as we need - // more bandwidth. We don't do this if using UART2, as - // in that case the RTCMv3 data doesn't go over the - // link to the flight controller - static const char blob[] = UBLOX_SET_BINARY_460800; - send_blob_start(instance, blob, sizeof(blob)); + if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { + static const char blob[] = UBLOX_SET_BINARY_115200; + send_blob_start(instance, blob, sizeof(blob)); + return; + } + + if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || + _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && + ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { + // we use 460800 when doing moving baseline as we need + // more bandwidth. We don't do this if using UART2, as + // in that case the RTCMv3 data doesn't go over the + // link to the flight controller + static const char blob[] = UBLOX_SET_BINARY_460800; + send_blob_start(instance, blob, sizeof(blob)); + return; + } + #if AP_GPS_NMEA_ENABLED - } else if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); + if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); + return; + } #endif // AP_GPS_NMEA_ENABLED - } else { - send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); - } + + send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } /* From e0a96eaa83a27003d1f28e8a0429dde691c3eab4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 12:31:46 +1000 Subject: [PATCH 0605/3101] AP_GPS: remove more code based on GPS_MOVING_BASELINE --- libraries/AP_GPS/AP_GPS.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 158fd859e14..df1c9a48049 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -566,6 +566,7 @@ void AP_GPS::send_blob_start(uint8_t instance) return; } +#if GPS_MOVING_BASELINE if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { @@ -577,6 +578,7 @@ void AP_GPS::send_blob_start(uint8_t instance) send_blob_start(instance, blob, sizeof(blob)); return; } +#endif #if AP_GPS_NMEA_ENABLED if (_type[instance] == GPS_TYPE_HEMI) { From eb930f830baa15227e524f530b0afe13abd81d0e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 00:31:28 +1000 Subject: [PATCH 0606/3101] Tools: build_ci.sh: set consistent GIT_VERSION_INT in env For consistent compiler output --- Tools/scripts/build_ci.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index a682f74b926..75f64561803 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -14,6 +14,7 @@ cxx_compiler=${CXX:-g++} export BUILDROOT=/tmp/ci.build rm -rf $BUILDROOT export GIT_VERSION="abcdef" +export GIT_VERSION_INT="15" export CHIBIOS_GIT_VERSION="12345667" export CCACHE_SLOPPINESS="include_file_ctime,include_file_mtime" autotest_args="" From fa293fd5f3509e300e96d1a323654082242fe86f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 00:36:38 +1000 Subject: [PATCH 0607/3101] github: set GIT_VERSION_INT to consistent value for checksum tests For consistent git hashes --- .github/workflows/test_size.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index a1ed220ddb6..0a6eeb3d689 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -69,9 +69,9 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then - CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf AP_Periph + CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" GIT_VERSION_INT="15" ./waf AP_Periph else - CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf + CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" GIT_VERSION_INT="15" ./waf fi cp -r build/${{matrix.config}}/bin/* "$NO_VERSIONS_DIR" @@ -114,9 +114,9 @@ jobs: if [ "${{matrix.config}}" = "Hitec-Airspeed" ] || [ "${{matrix.config}}" = "f103-GPS" ]; then - CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf AP_Periph + CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" GIT_VERSION_INT="15" ./waf AP_Periph else - CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf + CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" GIT_VERSION_INT="15" ./waf fi cp -r build/${{matrix.config}}/bin/* "$NO_VERSIONS_DIR" From 7474a31a832708fa302703cf1b3429238865290f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 00:51:58 +1000 Subject: [PATCH 0608/3101] waf: take ap_version.h int items from environment as we do str items This will mean we take the integer version of GIT_VERSION from the env too --- Tools/ardupilotwaf/ardupilotwaf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index eb7452f1c92..dd66b19c585 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -380,7 +380,7 @@ def ap_version_append_str(ctx, k, v): @conf def ap_version_append_int(ctx, k, v): - ctx.env['AP_VERSION_ITEMS'] += [(k,v)] + ctx.env['AP_VERSION_ITEMS'] += [(k, '{}'.format(os.environ.get(k, v)))] @conf def write_version_header(ctx, tgt): From 56513a07619ee00bb1f314d74d1f1c2cee288c8a Mon Sep 17 00:00:00 2001 From: QioTek <71515778+QioTek@users.noreply.github.com> Date: Tue, 12 Apr 2022 19:28:09 +0800 Subject: [PATCH 0609/3101] Tools: added QioTekAdeptF427 board ID Tools: added QioTekAdeptF427 board ID --- Tools/AP_Bootloader/board_types.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index e0fdcb235f5..357caa811f6 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -163,6 +163,7 @@ AP_HW_DEVEBOXH7 1061 AP_HW_MatekL431 1062 AP_HW_CUBEORANGEPLUS 1063 AP_HW_CarbonixF405 1064 +AP_HW_QioTekAdeptF427 1065 AP_HW_CUBEORANGE_PERIPH 1400 AP_HW_CUBEBLACK_PERIPH 1401 From 37e9ce3fb731700d6da162ea4fa7e902fa35cf0c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 12 Apr 2022 20:12:29 +0900 Subject: [PATCH 0610/3101] AP_NavEKF3: correct wind estimate param descriptions EK3_MCOEF becomes EK3_DRAG_MCOEF EK3_BCOEF_X/Y becomes EK3_DRAG_BCOEF_X/Y --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 5d52cfc36fd..e8a9f523dfa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -664,7 +664,7 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Param: DRAG_BCOEF_X // @DisplayName: Ballistic coefficient for X axis drag - // @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter. + // @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter. // @Range: 0.0 1000.0 // @Units: kg/m/m // @User: Advanced @@ -672,7 +672,7 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Param: DRAG_BCOEF_Y // @DisplayName: Ballistic coefficient for Y axis drag - // @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_MCOEF parameter. + // @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a postive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter. // @Range: 50.0 1000.0 // @Units: kg/m/m // @User: Advanced @@ -680,7 +680,7 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Param: DRAG_M_NSE // @DisplayName: Observation noise for drag acceleration - // @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_BCOEF_X and EK3_BCOEF_Y parameters + // @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables esitmation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters // @Range: 0.1 2.0 // @Increment: 0.1 // @User: Advanced @@ -689,7 +689,7 @@ const AP_Param::GroupInfo NavEKF3::var_info2[] = { // @Param: DRAG_MCOEF // @DisplayName: Momentum coefficient for propeller drag - // @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with different propellers. It is higher for ducted rotors. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEF to a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_BCOEF_X and EK3_BCOEF_Y parameters. + // @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with different propellers. It is higher for ducted rotors. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_DRAG_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEF to a postive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters. // @Range: 0.0 1.0 // @Increment: 0.01 // @Units: 1/s From 002bcca7f98670c87a5eaf51063102f5939be4cb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 12 Apr 2022 20:14:05 +0900 Subject: [PATCH 0611/3101] AP_Baro: correct param description refering to EK3 wind est params AP_NavEKF3: correct wind estimate param descriptions mention of EK3_BCOEF_X/Y becomes EK3_DRAG_BCOEF_X/Y --- libraries/AP_Baro/AP_Baro_Wind.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_Wind.cpp b/libraries/AP_Baro/AP_Baro_Wind.cpp index f8694c7050c..5050810d1ff 100644 --- a/libraries/AP_Baro/AP_Baro_Wind.cpp +++ b/libraries/AP_Baro/AP_Baro_Wind.cpp @@ -15,7 +15,7 @@ const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = { // @Param: FWD // @DisplayName: Pressure error coefficient in positive X direction (forward) - // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned. + // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned. // @Range: -1.0 1.0 // @Increment: 0.05 // @User: Advanced @@ -23,7 +23,7 @@ const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = { // @Param: BCK // @DisplayName: Pressure error coefficient in negative X direction (backwards) - // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned. + // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned. // @Range: -1.0 1.0 // @Increment: 0.05 // @User: Advanced @@ -31,7 +31,7 @@ const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = { // @Param: RGT // @DisplayName: Pressure error coefficient in positive Y direction (right) - // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned. + // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned. // @Range: -1.0 1.0 // @Increment: 0.05 // @User: Advanced @@ -39,7 +39,7 @@ const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = { // @Param: LFT // @DisplayName: Pressure error coefficient in negative Y direction (left) - // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_BCOEF_X and EK3_BCOEF_Y parameters have been tuned. + // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned. // @Range: -1.0 1.0 // @Increment: 0.05 // @User: Advanced From 375ab84e5647adbc2e8641531428a5851a6c6fef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 12 Apr 2022 20:15:42 +0900 Subject: [PATCH 0612/3101] SITL: correct suggestion to user re wind estimatation param value EK3_MCOEF becomes EK3_DRAG_MCOEF EK3_BCOEF_ becomes EK3_DRAG_BCOEF_ --- libraries/SITL/SIM_Frame.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 2068a259164..03f88b343e6 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -453,10 +453,10 @@ void Frame::init(const char *frame_str, Battery *_battery) if (momentum_drag > drag_force) { model.mdrag_coef *= drag_force / momentum_drag; areaCd = 0.0; - ::printf("Suggested EK3_BCOEF_* = 0, EK3_MCOEF = %.3f\n", (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density)); + ::printf("Suggested EK3_DRAG_BCOEF_* = 0, EK3_DRAG_MCOEF = %.3f\n", (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density)); } else { areaCd = (drag_force - momentum_drag) / (0.5f * ref_air_density * sq(model.refSpd)); - ::printf("Suggested EK3_BCOEF_* = %.3f, EK3_MCOEF = %.3f\n", model.mass / areaCd, (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density)); + ::printf("Suggested EK3_DRAG_BCOEF_* = %.3f, EK3_DRAG_MCOEF = %.3f\n", model.mass / areaCd, (momentum_drag / (model.mass * airspeed_bf)) * sqrtf(1.225f / ref_air_density)); } terminal_rotation_rate = model.refRotRate; From 04cc45de656e713d8976a81830514f4277de358c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Apr 2022 11:54:22 +1000 Subject: [PATCH 0613/3101] hwdef: remove non-ascii character from hwdef Killing the custom build server --- libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat index eeb063095f0..dca2a2a5788 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini-sd/hwdef.dat @@ -1,4 +1,4 @@ -# Revo-mini target with I²C and SD card support +# Revo-mini target with I2C and SD card support AUTOBUILD_TARGETS From c09f56112ec29e12587e1118da6b08d4b8f5e696 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 11:36:13 +1100 Subject: [PATCH 0614/3101] AP_Arming: move logging of RPM into RPM library --- libraries/AP_Arming/AP_Arming.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 217fcbfa0c7..d8baeead159 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #if HAL_MAX_CAN_PROTOCOL_DRIVERS #include From 23f2055330a2b33b962eb5da45a92e48e4a6cec0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 11:36:13 +1100 Subject: [PATCH 0615/3101] AP_Logger: move logging of RPM into RPM library --- libraries/AP_Logger/AP_Logger.h | 2 -- libraries/AP_Logger/LogFile.cpp | 16 ---------------- libraries/AP_Logger/LogStructure.h | 19 +++---------------- 3 files changed, 3 insertions(+), 34 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 1f826233d09..41f31b8eced 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -55,7 +55,6 @@ #include #include #include -#include #include #include #include @@ -305,7 +304,6 @@ class AP_Logger bool was_command_long=false); void Write_Mission_Cmd(const AP_Mission &mission, const AP_Mission::Mission_Command &cmd); - void Write_RPM(const AP_RPM &rpm_sensor); void Write_RallyPoint(uint8_t total, uint8_t sequence, const RallyLocation &rally_point); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index c52410ca477..dc0700e21de 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -426,22 +426,6 @@ void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) WriteBlock(&pkt, sizeof(pkt)); } -void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor) -{ - float rpm1 = -1, rpm2 = -1; - - rpm_sensor.get_rpm(0, rpm1); - rpm_sensor.get_rpm(1, rpm2); - - const struct log_RPM pkt{ - LOG_PACKET_HEADER_INIT(LOG_RPM_MSG), - time_us : AP_HAL::micros64(), - rpm1 : rpm1, - rpm2 : rpm2 - }; - WriteBlock(&pkt, sizeof(pkt)); -} - // Write beacon sensor (position) data void AP_Logger::Write_Beacon(AP_Beacon &beacon) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 68ba9e40872..0ea2caa4545 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -136,6 +136,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format struct PACKED LogStructure { @@ -502,13 +503,6 @@ struct PACKED log_MAV_Stats { // uint8_t state_retry_max; }; -struct PACKED log_RPM { - LOG_PACKET_HEADER; - uint64_t time_us; - float rpm1; - float rpm2; -}; - struct PACKED log_Rally { LOG_PACKET_HEADER; uint64_t time_us; @@ -1112,12 +1106,6 @@ struct PACKED log_VER { // @Field: Stat: Sensor state // @Field: Orient: Sensor orientation -// @LoggerMessage: RPM -// @Description: Data from RPM sensors -// @Field: TimeUS: Time since system startup -// @Field: rpm1: First sensor's data -// @Field: rpm2: Second sensor's data - // @LoggerMessage: RSSI // @Description: Received Signal Strength Indicator for RC receiver // @Field: TimeUS: Time since system startup @@ -1344,10 +1332,9 @@ LOG_STRUCTURE_FROM_NAVEKF3 \ LOG_STRUCTURE_FROM_NAVEKF \ LOG_STRUCTURE_FROM_AHRS \ LOG_STRUCTURE_FROM_HAL_CHIBIOS \ +LOG_STRUCTURE_FROM_RPM \ { LOG_DF_FILE_STATS, sizeof(log_DSF), \ "DSF", "QIHIIII", "TimeUS,Dp,Blk,Bytes,FMn,FMx,FAv", "s--b---", "F--0---" }, \ - { LOG_RPM_MSG, sizeof(log_RPM), \ - "RPM", "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" , true }, \ { LOG_RALLY_MSG, sizeof(log_Rally), \ "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \ { LOG_MAV_MSG, sizeof(log_MAV), \ @@ -1424,7 +1411,7 @@ enum LogMessages : uint8_t { LOG_DSTL_MSG, LOG_MAG_MSG, LOG_ARSP_MSG, - LOG_RPM_MSG, + LOG_IDS_FROM_RPM, LOG_RFND_MSG, LOG_MAV_STATS, LOG_FORMAT_UNITS_MSG, From ab86dafda7f0e24529812cddc0cf884829bde01d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 11:36:14 +1100 Subject: [PATCH 0616/3101] AP_RPM: move logging of RPM into RPM library --- libraries/AP_RPM/AP_RPM.cpp | 22 +++++++++++++++++++++- libraries/AP_RPM/AP_RPM.h | 2 ++ libraries/AP_RPM/LogStructure.h | 22 ++++++++++++++++++++++ 3 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_RPM/LogStructure.h diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp index 0b86a22b47c..83423955bb3 100644 --- a/libraries/AP_RPM/AP_RPM.cpp +++ b/libraries/AP_RPM/AP_RPM.cpp @@ -189,9 +189,11 @@ void AP_RPM::update(void) } } +#if HAL_LOGGING_ENABLED if (enabled(0) || enabled(1)) { - AP::logger().Write_RPM(*this); + Log_RPM(); } +#endif } /* @@ -259,6 +261,24 @@ bool AP_RPM::arming_checks(size_t buflen, char *buffer) const return true; } +#if HAL_LOGGING_ENABLED +void AP_RPM::Log_RPM() +{ + float rpm1 = -1, rpm2 = -1; + + get_rpm(0, rpm1); + get_rpm(1, rpm2); + + const struct log_RPM pkt{ + LOG_PACKET_HEADER_INIT(LOG_RPM_MSG), + time_us : AP_HAL::micros64(), + rpm1 : rpm1, + rpm2 : rpm2 + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} +#endif + // singleton instance AP_RPM *AP_RPM::_singleton; diff --git a/libraries/AP_RPM/AP_RPM.h b/libraries/AP_RPM/AP_RPM.h index 5d034b8a62a..2b2c4070a24 100644 --- a/libraries/AP_RPM/AP_RPM.h +++ b/libraries/AP_RPM/AP_RPM.h @@ -104,6 +104,8 @@ class AP_RPM uint8_t num_instances; void detect_instance(uint8_t instance); + + void Log_RPM(); }; namespace AP { diff --git a/libraries/AP_RPM/LogStructure.h b/libraries/AP_RPM/LogStructure.h new file mode 100644 index 00000000000..d870fabeda7 --- /dev/null +++ b/libraries/AP_RPM/LogStructure.h @@ -0,0 +1,22 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_RPM \ + LOG_RPM_MSG + +// @LoggerMessage: RPM +// @Description: Data from RPM sensors +// @Field: TimeUS: Time since system startup +// @Field: rpm1: First sensor's data +// @Field: rpm2: Second sensor's data +struct PACKED log_RPM { + LOG_PACKET_HEADER; + uint64_t time_us; + float rpm1; + float rpm2; +}; + +#define LOG_STRUCTURE_FROM_RPM \ + { LOG_RPM_MSG, sizeof(log_RPM), \ + "RPM", "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" , true }, From f023f0443d12d63bf67750dabdeb62827bddd684 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 11:36:14 +1100 Subject: [PATCH 0617/3101] GCS_MAVLink: move logging of RPM into RPM library --- libraries/GCS_MAVLink/GCS_Common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 2dd7a960109..2cfcfb98e51 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include From 3d854c7e922f7ce1471d2fcba0aec875c920ee40 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 11:36:14 +1100 Subject: [PATCH 0618/3101] Rover: move logging of RPM into RPM library --- Rover/Rover.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Rover/Rover.h b/Rover/Rover.h index d9fa99fc76b..15ff437de77 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -44,6 +44,7 @@ #include #include // Range finder library #include // RC input mapping library +#include // RPM input library #include // main loop scheduler #include // statistics library #include From 795427e574c60bbc59536a25f3234214e857d871 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:12:18 +1100 Subject: [PATCH 0619/3101] AP_Logger: move logging of Proximity into Proximity library --- libraries/AP_Logger/AP_Logger.h | 4 -- libraries/AP_Logger/LogFile.cpp | 65 --------------------------- libraries/AP_Logger/LogStructure.h | 71 ++---------------------------- 3 files changed, 3 insertions(+), 137 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 41f31b8eced..4ed38cee1d8 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -59,7 +59,6 @@ #include #include #include -#include #include #include @@ -308,9 +307,6 @@ class AP_Logger uint8_t sequence, const RallyLocation &rally_point); void Write_Beacon(AP_Beacon &beacon); -#if HAL_PROXIMITY_ENABLED - void Write_Proximity(AP_Proximity &proximity); -#endif void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp); void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index dc0700e21de..80d3290c082 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -453,71 +453,6 @@ void AP_Logger::Write_Beacon(AP_Beacon &beacon) WriteBlock(&pkt_beacon, sizeof(pkt_beacon)); } -#if HAL_PROXIMITY_ENABLED -// Write proximity sensor distances -void AP_Logger::Write_Proximity(AP_Proximity &proximity) -{ - // exit immediately if not enabled - if (proximity.get_status() == AP_Proximity::Status::NotConnected) { - return; - } - - AP_Proximity::Proximity_Distance_Array dist_array{}; // raw distances stored here - AP_Proximity::Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here - for (uint8_t i = 0; i < proximity.get_num_layers(); i++) { - const bool active = proximity.get_active_layer_distances(i, dist_array, filt_dist_array); - if (!active) { - // nothing on this layer - continue; - } - float dist_up; - if (!proximity.get_upward_distance(dist_up)) { - dist_up = 0.0f; - } - - float closest_ang = 0.0f; - float closest_dist = 0.0f; - proximity.get_closest_object(closest_ang, closest_dist); - - const struct log_Proximity pkt_proximity{ - LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), - time_us : AP_HAL::micros64(), - instance : i, - health : (uint8_t)proximity.get_status(), - dist0 : filt_dist_array.distance[0], - dist45 : filt_dist_array.distance[1], - dist90 : filt_dist_array.distance[2], - dist135 : filt_dist_array.distance[3], - dist180 : filt_dist_array.distance[4], - dist225 : filt_dist_array.distance[5], - dist270 : filt_dist_array.distance[6], - dist315 : filt_dist_array.distance[7], - distup : dist_up, - closest_angle : closest_ang, - closest_dist : closest_dist - }; - WriteBlock(&pkt_proximity, sizeof(pkt_proximity)); - - if (proximity.get_raw_log_enable()) { - const struct log_Proximity_raw pkt_proximity_raw{ - LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG), - time_us : AP_HAL::micros64(), - instance : i, - raw_dist0 : dist_array.distance[0], - raw_dist45 : dist_array.distance[1], - raw_dist90 : dist_array.distance[2], - raw_dist135 : dist_array.distance[3], - raw_dist180 : dist_array.distance[4], - raw_dist225 : dist_array.distance[5], - raw_dist270 : dist_array.distance[6], - raw_dist315 : dist_array.distance[7], - }; - WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw)); - } - } -} -#endif - void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb) { const struct log_SRTL pkt_srtl{ diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 0ea2caa4545..b6604f26ab2 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -132,6 +132,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -527,38 +528,6 @@ struct PACKED log_Beacon { float posz; }; -// proximity sensor logging -struct PACKED log_Proximity { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t instance; - uint8_t health; - float dist0; - float dist45; - float dist90; - float dist135; - float dist180; - float dist225; - float dist270; - float dist315; - float distup; - float closest_angle; - float closest_dist; -}; -struct PACKED log_Proximity_raw { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t instance; - float raw_dist0; - float raw_dist45; - float raw_dist90; - float raw_dist135; - float raw_dist180; - float raw_dist225; - float raw_dist270; - float raw_dist315; -}; - struct PACKED log_Performance { LOG_PACKET_HEADER; uint64_t time_us; @@ -1005,36 +974,6 @@ struct PACKED log_VER { // @Field: MVmin: MCU Voltage min // @Field: MVmax: MCU Voltage max -// @LoggerMessage: PRX -// @Description: Proximity Filtered sensor data -// @Field: TimeUS: Time since system startup -// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. -// @Field: He: True if proximity sensor is healthy -// @Field: D0: Nearest object in sector surrounding 0-degrees -// @Field: D45: Nearest object in sector surrounding 45-degrees -// @Field: D90: Nearest object in sector surrounding 90-degrees -// @Field: D135: Nearest object in sector surrounding 135-degrees -// @Field: D180: Nearest object in sector surrounding 180-degrees -// @Field: D225: Nearest object in sector surrounding 225-degrees -// @Field: D270: Nearest object in sector surrounding 270-degrees -// @Field: D315: Nearest object in sector surrounding 315-degrees -// @Field: DUp: Nearest object in upwards direction -// @Field: CAn: Angle to closest object -// @Field: CDis: Distance to closest object - -// @LoggerMessage: PRXR -// @Description: Proximity Raw sensor data -// @Field: TimeUS: Time since system startup -// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. -// @Field: D0: Nearest object in sector surrounding 0-degrees -// @Field: D45: Nearest object in sector surrounding 45-degrees -// @Field: D90: Nearest object in sector surrounding 90-degrees -// @Field: D135: Nearest object in sector surrounding 135-degrees -// @Field: D180: Nearest object in sector surrounding 180-degrees -// @Field: D225: Nearest object in sector surrounding 225-degrees -// @Field: D270: Nearest object in sector surrounding 270-degrees -// @Field: D315: Nearest object in sector surrounding 315-degrees - // @LoggerMessage: RAD // @Description: Telemetry radio statistics // @Field: TimeUS: Time since system startup @@ -1293,10 +1232,7 @@ LOG_STRUCTURE_FROM_CAMERA \ "DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \ { LOG_BEACON_MSG, sizeof(log_Beacon), \ "BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000", true }, \ - { LOG_PROXIMITY_MSG, sizeof(log_Proximity), \ - "PRX", "QBBfffffffffff", "TimeUS,Layer,He,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s#-mmmmmmmmmhm", "F--00000000000", true }, \ - { LOG_RAW_PROXIMITY_MSG, sizeof(log_Proximity_raw), \ - "PRXR", "QBffffffff", "TimeUS,Layer,D0,D45,D90,D135,D180,D225,D270,D315", "s#mmmmmmmm", "F-00000000", true }, \ + LOG_STRUCTURE_FROM_PROXIMITY \ { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ "PM", "QHHIIHHIIIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "s---b%------s", "F---0A------F" }, \ { LOG_SRTL_MSG, sizeof(log_SRTL), \ @@ -1430,7 +1366,7 @@ enum LogMessages : uint8_t { LOG_IDS_FROM_VISUALODOM, LOG_IDS_FROM_AVOIDANCE, LOG_BEACON_MSG, - LOG_PROXIMITY_MSG, + LOG_IDS_FROM_PROXIMITY, LOG_DF_FILE_STATS, LOG_SRTL_MSG, LOG_PERFORMANCE_MSG, @@ -1445,7 +1381,6 @@ enum LogMessages : uint8_t { LOG_PSCN_MSG, LOG_PSCE_MSG, LOG_PSCD_MSG, - LOG_RAW_PROXIMITY_MSG, LOG_IDS_FROM_PRECLAND, LOG_IDS_FROM_AIS, LOG_STAK_MSG, From a62e08ee4095b2cffe56e53d614e5a67da2161fc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:12:18 +1100 Subject: [PATCH 0620/3101] AP_Proximity: move logging of Proximity into Proximity library --- libraries/AP_Proximity/AP_Proximity.cpp | 67 +++++++++++++++++++++ libraries/AP_Proximity/AP_Proximity.h | 8 ++- libraries/AP_Proximity/LogStructure.h | 77 +++++++++++++++++++++++++ 3 files changed, 149 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Proximity/LogStructure.h diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 70ba0306524..174c386eb00 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -27,6 +27,8 @@ #include "AP_Proximity_AirSimSITL.h" #include "AP_Proximity_Cygbot_D1.h" +#include + extern const AP_HAL::HAL &hal; // table of user settable parameters @@ -522,6 +524,71 @@ void AP_Proximity::set_rangefinder_alt(bool use, bool healthy, float alt_cm) drivers[primary_instance]->set_rangefinder_alt(use, healthy, alt_cm); } +#if HAL_LOGGING_ENABLED +// Write proximity sensor distances +void AP_Proximity::log() +{ + // exit immediately if not enabled + if (get_status() == AP_Proximity::Status::NotConnected) { + return; + } + + Proximity_Distance_Array dist_array{}; // raw distances stored here + Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here + auto &logger { AP::logger() }; + for (uint8_t i = 0; i < get_num_layers(); i++) { + const bool active = get_active_layer_distances(i, dist_array, filt_dist_array); + if (!active) { + // nothing on this layer + continue; + } + float dist_up; + if (!get_upward_distance(dist_up)) { + dist_up = 0.0f; + } + + float closest_ang = 0.0f; + float closest_dist = 0.0f; + get_closest_object(closest_ang, closest_dist); + + const struct log_Proximity pkt_proximity{ + LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG), + time_us : AP_HAL::micros64(), + instance : i, + health : (uint8_t)get_status(), + dist0 : filt_dist_array.distance[0], + dist45 : filt_dist_array.distance[1], + dist90 : filt_dist_array.distance[2], + dist135 : filt_dist_array.distance[3], + dist180 : filt_dist_array.distance[4], + dist225 : filt_dist_array.distance[5], + dist270 : filt_dist_array.distance[6], + dist315 : filt_dist_array.distance[7], + distup : dist_up, + closest_angle : closest_ang, + closest_dist : closest_dist + }; + logger.WriteBlock(&pkt_proximity, sizeof(pkt_proximity)); + + if (_raw_log_enable) { + const struct log_Proximity_raw pkt_proximity_raw{ + LOG_PACKET_HEADER_INIT(LOG_RAW_PROXIMITY_MSG), + time_us : AP_HAL::micros64(), + instance : i, + raw_dist0 : dist_array.distance[0], + raw_dist45 : dist_array.distance[1], + raw_dist90 : dist_array.distance[2], + raw_dist135 : dist_array.distance[3], + raw_dist180 : dist_array.distance[4], + raw_dist225 : dist_array.distance[5], + raw_dist270 : dist_array.distance[6], + raw_dist315 : dist_array.distance[7], + }; + logger.WriteBlock(&pkt_proximity_raw, sizeof(pkt_proximity_raw)); + } + } +} +#endif AP_Proximity *AP_Proximity::_singleton; diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 43db39d2937..0646e6a2460 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -151,9 +151,6 @@ class AP_Proximity Type get_type(uint8_t instance) const; - // true if raw distances should be logged - bool get_raw_log_enable() const { return _raw_log_enable; } - // parameter list static const struct AP_Param::GroupInfo var_info[]; @@ -168,6 +165,10 @@ class AP_Proximity // set alt as read from downward facing rangefinder. Tilt is already adjusted for void set_rangefinder_alt(bool use, bool healthy, float alt_cm); + // method called by vehicle to have AP_Proximity write onboard log + // messages: + void log(); + private: static AP_Proximity *_singleton; Proximity_State state[PROXIMITY_MAX_INSTANCES]; @@ -195,6 +196,7 @@ class AP_Proximity AP_Float _min_m; // Proximity minimum range void detect_instance(uint8_t instance); + }; namespace AP { diff --git a/libraries/AP_Proximity/LogStructure.h b/libraries/AP_Proximity/LogStructure.h new file mode 100644 index 00000000000..809a35c3c0f --- /dev/null +++ b/libraries/AP_Proximity/LogStructure.h @@ -0,0 +1,77 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_PROXIMITY \ + LOG_PROXIMITY_MSG, \ + LOG_RAW_PROXIMITY_MSG + +// @LoggerMessage: PRX +// @Description: Proximity Filtered sensor data +// @Field: TimeUS: Time since system startup +// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. +// @Field: He: True if proximity sensor is healthy +// @Field: D0: Nearest object in sector surrounding 0-degrees +// @Field: D45: Nearest object in sector surrounding 45-degrees +// @Field: D90: Nearest object in sector surrounding 90-degrees +// @Field: D135: Nearest object in sector surrounding 135-degrees +// @Field: D180: Nearest object in sector surrounding 180-degrees +// @Field: D225: Nearest object in sector surrounding 225-degrees +// @Field: D270: Nearest object in sector surrounding 270-degrees +// @Field: D315: Nearest object in sector surrounding 315-degrees +// @Field: DUp: Nearest object in upwards direction +// @Field: CAn: Angle to closest object +// @Field: CDis: Distance to closest object + +// proximity sensor logging +struct PACKED log_Proximity { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint8_t health; + float dist0; + float dist45; + float dist90; + float dist135; + float dist180; + float dist225; + float dist270; + float dist315; + float distup; + float closest_angle; + float closest_dist; +}; + +// @LoggerMessage: PRXR +// @Description: Proximity Raw sensor data +// @Field: TimeUS: Time since system startup +// @Field: Layer: Pitch(instance) at which the obstacle is at. 0th layer {-75,-45} degrees. 1st layer {-45,-15} degrees. 2nd layer {-15, 15} degrees. 3rd layer {15, 45} degrees. 4th layer {45,75} degrees. Minimum distance in each layer will be logged. +// @Field: D0: Nearest object in sector surrounding 0-degrees +// @Field: D45: Nearest object in sector surrounding 45-degrees +// @Field: D90: Nearest object in sector surrounding 90-degrees +// @Field: D135: Nearest object in sector surrounding 135-degrees +// @Field: D180: Nearest object in sector surrounding 180-degrees +// @Field: D225: Nearest object in sector surrounding 225-degrees +// @Field: D270: Nearest object in sector surrounding 270-degrees +// @Field: D315: Nearest object in sector surrounding 315-degrees + +struct PACKED log_Proximity_raw { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + float raw_dist0; + float raw_dist45; + float raw_dist90; + float raw_dist135; + float raw_dist180; + float raw_dist225; + float raw_dist270; + float raw_dist315; +}; + + +#define LOG_STRUCTURE_FROM_PROXIMITY \ + { LOG_PROXIMITY_MSG, sizeof(log_Proximity), \ + "PRX", "QBBfffffffffff", "TimeUS,Layer,He,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis", "s#-mmmmmmmmmhm", "F--00000000000", true }, \ + { LOG_RAW_PROXIMITY_MSG, sizeof(log_Proximity_raw), \ + "PRXR", "QBffffffff", "TimeUS,Layer,D0,D45,D90,D135,D180,D225,D270,D315", "s#mmmmmmmm", "F-00000000", true }, From de2182b88f1a199333ef30c46f68054f717e9749 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:12:18 +1100 Subject: [PATCH 0621/3101] ArduCopter: move logging of Proximity into Proximity library --- ArduCopter/Copter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 70314c9b565..87c7ae92b3d 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -537,7 +537,7 @@ void Copter::ten_hz_logging_loop() if (should_log(MASK_LOG_CTUN)) { attitude_control->control_monitor_log(); #if HAL_PROXIMITY_ENABLED - logger.Write_Proximity(g2.proximity); // Write proximity sensor distances + g2.proximity.log(); // Write proximity sensor distances #endif #if BEACON_ENABLED == ENABLED logger.Write_Beacon(g2.beacon); From 232721882203cab8932739755f675e0e6e7ce6bb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:12:18 +1100 Subject: [PATCH 0622/3101] Rover: move logging of Proximity into Proximity library --- Rover/Rover.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index afec3bc1a04..670a7648edd 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -380,7 +380,7 @@ void Rover::update_logging1(void) #if HAL_PROXIMITY_ENABLED if (should_log(MASK_LOG_RANGEFINDER)) { - logger.Write_Proximity(g2.proximity); + g2.proximity.log(); } #endif } From bbcf0a096875bee354849f001ad3a91035139b71 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:31:25 +1100 Subject: [PATCH 0623/3101] AP_Beacon: move logging of Beacon into Beacon library --- libraries/AP_Beacon/AP_Beacon.cpp | 27 ++++++++++++++++++++++ libraries/AP_Beacon/AP_Beacon.h | 3 +++ libraries/AP_Beacon/LogStructure.h | 37 ++++++++++++++++++++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 libraries/AP_Beacon/LogStructure.h diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp index 68a3718cfa0..81ca563a773 100644 --- a/libraries/AP_Beacon/AP_Beacon.cpp +++ b/libraries/AP_Beacon/AP_Beacon.cpp @@ -21,6 +21,7 @@ #include "AP_Beacon_SITL.h" #include +#include extern const AP_HAL::HAL &hal; @@ -388,6 +389,32 @@ bool AP_Beacon::device_ready(void) const return ((_driver != nullptr) && (_type != AP_BeaconType_None)); } +// Write beacon sensor (position) data +void AP_Beacon::log() +{ + if (!enabled()) { + return; + } + // position + Vector3f pos; + float accuracy = 0.0f; + get_vehicle_position_ned(pos, accuracy); + + const struct log_Beacon pkt_beacon{ + LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG), + time_us : AP_HAL::micros64(), + health : (uint8_t)healthy(), + count : (uint8_t)count(), + dist0 : beacon_distance(0), + dist1 : beacon_distance(1), + dist2 : beacon_distance(2), + dist3 : beacon_distance(3), + posx : pos.x, + posy : pos.y, + posz : pos.z + }; + AP::logger().WriteBlock(&pkt_beacon, sizeof(pkt_beacon)); +} // singleton instance AP_Beacon *AP_Beacon::_singleton; diff --git a/libraries/AP_Beacon/AP_Beacon.h b/libraries/AP_Beacon/AP_Beacon.h index 3bf766c7ee8..4d0cdb023d9 100644 --- a/libraries/AP_Beacon/AP_Beacon.h +++ b/libraries/AP_Beacon/AP_Beacon.h @@ -104,6 +104,9 @@ class AP_Beacon static const struct AP_Param::GroupInfo var_info[]; + // a method for vehicles to call to make onboard log messages: + void log(); + private: static AP_Beacon *_singleton; diff --git a/libraries/AP_Beacon/LogStructure.h b/libraries/AP_Beacon/LogStructure.h new file mode 100644 index 00000000000..f0faa8f355d --- /dev/null +++ b/libraries/AP_Beacon/LogStructure.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_BEACON \ + LOG_BEACON_MSG + +// @LoggerMessage: BCN +// @Description: Beacon information +// @Field: TimeUS: Time since system startup +// @Field: Health: True if beacon sensor is healthy +// @Field: Cnt: Number of beacons being used +// @Field: D0: Distance to first beacon +// @Field: D1: Distance to second beacon +// @Field: D2: Distance to third beacon +// @Field: D3: Distance to fourth beacon +// @Field: PosX: Calculated beacon position, x-axis +// @Field: PosY: Calculated beacon position, y-axis +// @Field: PosZ: Calculated beacon position, z-axis + +struct PACKED log_Beacon { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t health; + uint8_t count; + float dist0; + float dist1; + float dist2; + float dist3; + float posx; + float posy; + float posz; +}; + +#define LOG_STRUCTURE_FROM_BEACON \ + { LOG_BEACON_MSG, sizeof(log_Beacon), \ + "BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000", true }, From e5c40d0e54b8e070e9b95900c0e5491e4b08772a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:31:25 +1100 Subject: [PATCH 0624/3101] AP_Logger: move logging of Beacon into Beacon library --- libraries/AP_Logger/AP_Logger.h | 2 -- libraries/AP_Logger/LogFile.cpp | 27 ------------------------ libraries/AP_Logger/LogStructure.h | 33 +++--------------------------- 3 files changed, 3 insertions(+), 59 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 4ed38cee1d8..5b9d7442407 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -58,7 +58,6 @@ #include #include #include -#include #include #include @@ -306,7 +305,6 @@ class AP_Logger void Write_RallyPoint(uint8_t total, uint8_t sequence, const RallyLocation &rally_point); - void Write_Beacon(AP_Beacon &beacon); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp); void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 80d3290c082..bb278181be1 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -426,33 +426,6 @@ void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) WriteBlock(&pkt, sizeof(pkt)); } -// Write beacon sensor (position) data -void AP_Logger::Write_Beacon(AP_Beacon &beacon) -{ - if (!beacon.enabled()) { - return; - } - // position - Vector3f pos; - float accuracy = 0.0f; - beacon.get_vehicle_position_ned(pos, accuracy); - - const struct log_Beacon pkt_beacon{ - LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG), - time_us : AP_HAL::micros64(), - health : (uint8_t)beacon.healthy(), - count : (uint8_t)beacon.count(), - dist0 : beacon.beacon_distance(0), - dist1 : beacon.beacon_distance(1), - dist2 : beacon.beacon_distance(2), - dist3 : beacon.beacon_distance(3), - posx : pos.x, - posy : pos.y, - posz : pos.z - }; - WriteBlock(&pkt_beacon, sizeof(pkt_beacon)); -} - void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& breadcrumb) { const struct log_SRTL pkt_srtl{ diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index b6604f26ab2..5ac54933fb8 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -120,6 +120,7 @@ const struct MultiplierStructure log_Multipliers[] = { #define HEAD_BYTE1 0xA3 // Decimal 163 #define HEAD_BYTE2 0x95 // Decimal 149 +#include #include #include #include @@ -514,20 +515,6 @@ struct PACKED log_Rally { int16_t altitude; }; -struct PACKED log_Beacon { - LOG_PACKET_HEADER; - uint64_t time_us; - uint8_t health; - uint8_t count; - float dist0; - float dist1; - float dist2; - float dist3; - float posx; - float posy; - float posz; -}; - struct PACKED log_Performance { LOG_PACKET_HEADER; uint64_t time_us; @@ -738,19 +725,6 @@ struct PACKED log_VER { // @Field: Hp: Probability sensor is healthy // @Field: Pri: True if sensor is the primary sensor -// @LoggerMessage: BCN -// @Description: Beacon information -// @Field: TimeUS: Time since system startup -// @Field: Health: True if beacon sensor is healthy -// @Field: Cnt: Number of beacons being used -// @Field: D0: Distance to first beacon -// @Field: D1: Distance to second beacon -// @Field: D2: Distance to third beacon -// @Field: D3: Distance to fourth beacon -// @Field: PosX: Calculated beacon position, x-axis -// @Field: PosY: Calculated beacon position, y-axis -// @Field: PosZ: Calculated beacon position, z-axis - // @LoggerMessage: CMD // @Description: Executed mission command information // @Field: TimeUS: Time since system startup @@ -1230,8 +1204,7 @@ LOG_STRUCTURE_FROM_CAMERA \ "RFND", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--", true }, \ { LOG_MAV_STATS, sizeof(log_MAV_Stats), \ "DMS", "QIIIIBBBBBBBBB", "TimeUS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "F-------------" }, \ - { LOG_BEACON_MSG, sizeof(log_Beacon), \ - "BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--0000000", true }, \ + LOG_STRUCTURE_FROM_BEACON \ LOG_STRUCTURE_FROM_PROXIMITY \ { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ "PM", "QHHIIHHIIIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "s---b%------s", "F---0A------F" }, \ @@ -1365,7 +1338,7 @@ enum LogMessages : uint8_t { LOG_IDS_FROM_VISUALODOM, LOG_IDS_FROM_AVOIDANCE, - LOG_BEACON_MSG, + LOG_IDS_FROM_BEACON, LOG_IDS_FROM_PROXIMITY, LOG_DF_FILE_STATS, LOG_SRTL_MSG, From 39eca59938780ee2e330c5ccb4b33e6c24613a8e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:31:26 +1100 Subject: [PATCH 0625/3101] ArduCopter: move logging of Beacon into Beacon library --- ArduCopter/Copter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 87c7ae92b3d..dc9a2556eb5 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -540,7 +540,7 @@ void Copter::ten_hz_logging_loop() g2.proximity.log(); // Write proximity sensor distances #endif #if BEACON_ENABLED == ENABLED - logger.Write_Beacon(g2.beacon); + g2.beacon.log(); #endif } #if FRAME_CONFIG == HELI_FRAME From 134e4ff705d9aec0eea2228a4bfb8db50597f949 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 4 Mar 2022 12:31:26 +1100 Subject: [PATCH 0626/3101] Rover: move logging of Beacon into Beacon library --- Rover/Rover.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 670a7648edd..d93eca3bd56 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -366,7 +366,7 @@ void Rover::update_logging1(void) if (should_log(MASK_LOG_THR)) { Log_Write_Throttle(); - logger.Write_Beacon(g2.beacon); + g2.beacon.log(); } if (should_log(MASK_LOG_NTUN)) { From c33f6f3a8d4d94f4603eeace31e3cc38eb991fc1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 7 Apr 2022 12:52:48 +1000 Subject: [PATCH 0627/3101] autotest: add tests to ensure we get BCN, RPM and PRX/PRXR log msgs --- Tools/autotest/arducopter.py | 7 +++++++ Tools/autotest/common.py | 2 ++ 2 files changed, 9 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f75aeb88eae..93a8dd2a14c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6042,12 +6042,17 @@ def fly_proximity_avoidance_test_corners(self): self.set_parameters({ "FENCE_ENABLE": 1, "PRX_TYPE": 10, + "PRX_LOG_RAW": 1, "RC10_OPTION": 40, # proximity-enable }) self.reboot_sitl() self.progress("Enabling proximity") self.set_rc(10, 2000) self.check_avoidance_corners() + + self.assert_current_onboard_log_contains_message("PRX") + self.assert_current_onboard_log_contains_message("PRXR") + except Exception as e: self.print_exception_caught(e) ex = e @@ -6386,6 +6391,8 @@ def fly_beacon_position(self): self.wait_groundspeed(0, 0.3, timeout=120) self.land_and_disarm() + self.assert_current_onboard_log_contains_message("BCN") + except Exception as e: self.print_exception_caught(e) ex = e diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index ce70a9474f1..5f0ce974454 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -11267,6 +11267,8 @@ def test_frsky_sport(self): self.set_rc(3, 1050) self.wait_rpm1(timeout=10, min_rpm=200) + self.assert_current_onboard_log_contains_message("RPM") + self.drain_mav_unparsed() # anything with a lambda in here needs a proper test written. # This, at least makes sure we're getting some of each From f4548daa5aa741e490399c67bfb56214254fdf5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 12 Apr 2022 08:13:06 +1000 Subject: [PATCH 0628/3101] AC_AttitudeControl: added an option for pitch weathervaning when nose-in or tail-in, if the aircraft has significant pitch asymmetry in hover then we would spin around in no wind if we use pitch as an input this makes pitch input for nose-in and tail-in optional and off by default to preserve existing behaviour --- libraries/AC_AttitudeControl/AC_WeatherVane.cpp | 17 ++++++++++++++--- libraries/AC_AttitudeControl/AC_WeatherVane.h | 7 ++++++- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp index e1978c80112..cd6a9c8ac26 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp @@ -87,6 +87,13 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = { // @User: Standard AP_GROUPINFO("LAND", 8, AC_WeatherVane, _landing_direction, -1), + // @Param: OPTIONS + // @DisplayName: Weathervaning options + // @Description: Options impacting weathervaning behaviour + // @Bitmask: 0:Use pitch when nose or tail-in for faster weathervaning + // @User: Standard + AP_GROUPINFO("OPTIONS", 9, AC_WeatherVane, _options, 0), + AP_GROUPEND }; @@ -97,7 +104,7 @@ AC_WeatherVane::AC_WeatherVane(void) AP_Param::setup_object_defaults(this, var_info); } -bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing) +bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, float pitch_cdeg, const bool is_takeoff, const bool is_landing) { Direction dir = (Direction)_direction.get(); if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) { @@ -159,13 +166,17 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con const float deadzone_cdeg = _min_dz_ang_deg*100.0; float output = 0.0; const char* dir_string = ""; + + // should we enable pitch input for nose-in and tail-in? + const bool pitch_enable = (uint8_t(_options.get()) & uint8_t(Options::PITCH_ENABLE)) != 0; + switch (dir) { case Direction::OFF: reset(); return false; case Direction::NOSE_IN: - if (is_positive(pitch_cdeg)) { + if (pitch_enable && is_positive(pitch_cdeg)) { output = fabsf(roll_cdeg) + pitch_cdeg; } else { output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); @@ -193,7 +204,7 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con break; case Direction::TAIL_IN: - if (is_negative(pitch_cdeg)) { + if (pitch_enable && is_negative(pitch_cdeg)) { output = fabsf(roll_cdeg) - pitch_cdeg; } else { output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.h b/libraries/AC_AttitudeControl/AC_WeatherVane.h index b0cb57f8a2d..d8e66498da7 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.h +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.h @@ -10,7 +10,7 @@ class AC_WeatherVane { CLASS_NO_COPY(AC_WeatherVane); // Calculate and return the yaw output to weathervane the vehicle - bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing); + bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, float pitch_cdeg, const bool is_takeoff, const bool is_landing); // Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted void reset(void); @@ -31,6 +31,10 @@ class AC_WeatherVane { TAIL_IN = 4, // backwards, for tailsitters, makes it easier to descend }; + enum class Options { + PITCH_ENABLE = (1<<0), + }; + // Paramaters AP_Int8 _direction; AP_Float _gain; @@ -40,6 +44,7 @@ class AC_WeatherVane { AP_Float _max_vel_z; AP_Int8 _landing_direction; AP_Int8 _takeoff_direction; + AP_Int16 _options; float last_output; bool active_msg_sent; From cdfa682be03f7574b1bd7c767cf741b1e94b947c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 12 Apr 2022 08:17:12 +1000 Subject: [PATCH 0629/3101] AC_AttitudeControl: use deadzone for pitch when pitch for nose-in and tail-in is enabled we should use the deadzone --- libraries/AC_AttitudeControl/AC_WeatherVane.cpp | 10 +++++----- libraries/AC_AttitudeControl/AC_WeatherVane.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp index cd6a9c8ac26..ea8cde608a2 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp @@ -104,7 +104,7 @@ AC_WeatherVane::AC_WeatherVane(void) AP_Param::setup_object_defaults(this, var_info); } -bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, float pitch_cdeg, const bool is_takeoff, const bool is_landing) +bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing) { Direction dir = (Direction)_direction.get(); if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) { @@ -176,8 +176,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con return false; case Direction::NOSE_IN: - if (pitch_enable && is_positive(pitch_cdeg)) { - output = fabsf(roll_cdeg) + pitch_cdeg; + if (pitch_enable && is_positive(pitch_cdeg - deadzone_cdeg)) { + output = fabsf(roll_cdeg) + (pitch_cdeg - deadzone_cdeg); } else { output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); } @@ -204,8 +204,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con break; case Direction::TAIL_IN: - if (pitch_enable && is_negative(pitch_cdeg)) { - output = fabsf(roll_cdeg) - pitch_cdeg; + if (pitch_enable && is_negative(pitch_cdeg + deadzone_cdeg)) { + output = fabsf(roll_cdeg) - (pitch_cdeg + deadzone_cdeg); } else { output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); } diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.h b/libraries/AC_AttitudeControl/AC_WeatherVane.h index d8e66498da7..fdfa765ab13 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.h +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.h @@ -10,7 +10,7 @@ class AC_WeatherVane { CLASS_NO_COPY(AC_WeatherVane); // Calculate and return the yaw output to weathervane the vehicle - bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, float pitch_cdeg, const bool is_takeoff, const bool is_landing); + bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing); // Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted void reset(void); From ccca0e8e0f2bc89801a1e9e200f83d89eb37097a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 09:16:54 +1000 Subject: [PATCH 0630/3101] AP_GPS: stop using HAL_BUILD_AP_PERIPH to gate GPS backends --- libraries/AP_GPS/AP_GPS.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index df1c9a48049..950a0ab75d2 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -632,13 +632,11 @@ void AP_GPS::detect_instance(uint8_t instance) // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there case GPS_TYPE_MAV: -#ifndef HAL_BUILD_AP_PERIPH #if AP_GPS_MAV_ENABLED dstate->auto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); goto found_gps; #endif //AP_GPS_MAV_ENABLED -#endif break; // user has to explicitly set the UAVCAN type, do not use AUTO @@ -676,8 +674,6 @@ void AP_GPS::detect_instance(uint8_t instance) // the correct baud rate, and should have the selected baud broadcast dstate->auto_detected_baud = true; - // don't build the less common GPS drivers on F1 AP_Periph -#if !defined(HAL_BUILD_AP_PERIPH) || !defined(STM32F1) switch (_type[instance]) { #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. @@ -705,7 +701,6 @@ void AP_GPS::detect_instance(uint8_t instance) default: break; } -#endif // HAL_BUILD_AP_PERIPH if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { // try the next baud rate @@ -762,7 +757,6 @@ void AP_GPS::detect_instance(uint8_t instance) } new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], role); } -#ifndef HAL_BUILD_AP_PERIPH #if AP_GPS_SBP2_ENABLED else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { @@ -796,7 +790,6 @@ void AP_GPS::detect_instance(uint8_t instance) } #endif //AP_GPS_NMEA_ENABLED -#endif // HAL_BUILD_AP_PERIPH if (new_gps) { goto found_gps; } From e788941f7094c72ecbe991f808b0c53f5784e086 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Apr 2022 09:17:23 +1000 Subject: [PATCH 0631/3101] hwdef: turn off various GPS backends on on AP_Periph --- .../AP_HAL_ChibiOS/hwdef/f103-GPS/hwdef.dat | 8 +++- .../hwdef/scripts/chibios_hwdef.py | 46 +++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/f103-GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/f103-GPS/hwdef.dat index 7348b44ce41..97f3199056b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/f103-GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/f103-GPS/hwdef.dat @@ -10,8 +10,14 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES undef HAL_UART_MIN_TX_SIZE define HAL_UART_MIN_TX_SIZE 256 -# GPS+MAG +# GPS define HAL_PERIPH_ENABLE_GPS +# restrict backends available on f103: +define AP_GPS_NOVA_ENABLED 0 +define AP_GPS_SBF_ENABLED 0 +define AP_GPS_GSOF_ENABLED 0 + +# MAG define HAL_PERIPH_ENABLE_MAG #define HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY define HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 7fa212f6e49..171a23f1378 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -2782,6 +2782,52 @@ def add_apperiph_defaults(f): #define AP_ROBOTISSERVO_ENABLED 0 #endif +/* + * GPS Backends - we selectively turn backends on. + * Note also that f103-GPS explicitly disables some of these backends. + */ +#define AP_GPS_BACKEND_DEFAULT_ENABLED 0 + +#ifndef AP_GPS_ERB_ENABLED +#define AP_GPS_ERB_ENABLED 0 +#endif + +#ifndef AP_GPS_GSOF_ENABLED +#define AP_GPS_GSOF_ENABLED defined(HAL_PERIPH_ENABLE_GPS) +#endif + +#ifndef AP_GPS_NMEA_ENABLED +#define AP_GPS_NMEA_ENABLED 0 +#endif + +#ifndef AP_GPS_SBF_ENABLED +#define AP_GPS_SBF_ENABLED defined(HAL_PERIPH_ENABLE_GPS) +#endif + +#ifndef AP_GPS_SBP_ENABLED +#define AP_GPS_SBP_ENABLED 0 +#endif + +#ifndef AP_GPS_SBP2_ENABLED +#define AP_GPS_SBP2_ENABLED 0 +#endif + +#ifndef AP_GPS_SIRF_ENABLED +#define AP_GPS_SIRF_ENABLED 0 +#endif + +#ifndef AP_GPS_MAV_ENABLED +#define AP_GPS_MAV_ENABLED 0 +#endif + +#ifndef AP_GPS_NOVA_ENABLED +#define AP_GPS_NOVA_ENABLED defined(HAL_PERIPH_ENABLE_GPS) +#endif + +#ifndef HAL_SIM_GPS_ENABLED +#define HAL_SIM_GPS_ENABLED (AP_SIM_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) +#endif + ''') From 69666d19eeaa3d26a24a27cb554b98d950ef0a65 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 16:31:25 +1000 Subject: [PATCH 0632/3101] Tools: fix cygwin build error --- Tools/scripts/cygwin_build.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Tools/scripts/cygwin_build.sh b/Tools/scripts/cygwin_build.sh index 62767fe8ea9..55e8bf6f9d8 100755 --- a/Tools/scripts/cygwin_build.sh +++ b/Tools/scripts/cygwin_build.sh @@ -8,6 +8,8 @@ set -x +git config --global --add safe.directory /cygdrive/d/a/ardupilot/ardupilot + rm -rf artifacts mkdir artifacts From 16a8400f8520e9593d1b0c3f5236853beb9ea783 Mon Sep 17 00:00:00 2001 From: mateksys Date: Sat, 9 Apr 2022 11:17:44 +0800 Subject: [PATCH 0633/3101] AP_HAL_ChibiOS:add icm42688p as substitution --- libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat index 6f930300db3..9be823f0886 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH743/hwdef.dat @@ -181,11 +181,13 @@ define HAL_STORAGE_SIZE 16384 STORAGE_FLASH_PAGE 14 # spi devices +SPIDEV icm42688 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ SPIDEV mpu6000 SPI1 DEVID1 IMU1_CS MODE3 1*MHZ 4*MHZ SPIDEV icm20602 SPI4 DEVID1 IMU2_CS MODE3 1*MHZ 4*MHZ SPIDEV icm42605 SPI4 DEVID1 IMU3_CS MODE3 2*MHZ 8*MHZ SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + DMA_NOSHARE SPI1* SPI4* # SPI3 external connections @@ -198,9 +200,13 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES define HAL_I2C_INTERNAL_MASK 0 define HAL_COMPASS_AUTO_ROT_DEFAULT 2 -# two IMUs. We put icm20602 first as we can sample accel at 4kHz -IMU Invensense SPI:icm20602 ROTATION_ROLL_180_YAW_270 +# two IMUs +# H743-V1, ICM20602, MPU6000 +# H743-V1.5/V2, ICM42605, MPU6000 +# H743-V3, ICM42688P, ICM42605 +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 IMU Invensensev3 SPI:icm42605 ROTATION_YAW_270 +IMU Invensense SPI:icm20602 ROTATION_ROLL_180_YAW_270 IMU Invensense SPI:mpu6000 ROTATION_ROLL_180_YAW_270 define HAL_DEFAULT_INS_FAST_SAMPLE 1 From e23e2cb63f4de3ed5358092bf18a6ae36336b6e1 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 4 Sep 2020 00:28:25 -0300 Subject: [PATCH 0634/3101] Autotest: Sub: Add depth hold cases of large buoyancies and small inputs --- Tools/autotest/ardusub.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 8f2a46839d6..68ea3b2f0b1 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -152,9 +152,21 @@ def test_alt_hold(self): # let the vehicle settle (momentum / stopping point shenanigans....) self.delay_sim_time(1) + self.watch_altitude_maintained() + # Make sure the code can handle buoyancy changes + self.set_parameter("SIM_BUOYANCY", 10) + self.watch_altitude_maintained() + self.set_parameter("SIM_BUOYANCY", -10) self.watch_altitude_maintained() + # Make sure that the ROV will dive with a small input down even if there is a 10N buoyancy force upwards + self.set_parameter("SIM_BUOYANCY", 10) + self.set_rc(Joystick.Throttle, 1450) + self.wait_altitude(altitude_min=-6, altitude_max=-5.5) + + self.set_rc(Joystick.Throttle, 1500) + self.watch_altitude_maintained() self.disarm_vehicle() def test_pos_hold(self): From cd9cc1419bc721b1a310b6358896d6a91eabddd9 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 4 Sep 2020 00:31:33 -0300 Subject: [PATCH 0635/3101] Sub: Improve althold to handle small inputs with payload/buyoancy better --- ArduSub/control_althold.cpp | 41 +++++++++++++------------------------ 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 2e49e3c046d..a420cfe3149 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -37,7 +37,7 @@ void Sub::althold_run() if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) - attitude_control.set_throttle_out(0,true,g.throttle_filt); + attitude_control.set_throttle_out(0.5,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); pos_control.relax_z_controller(motors.get_throttle_hover()); last_pilot_heading = ahrs.yaw_sensor; @@ -101,33 +101,20 @@ void Sub::althold_run() } void Sub::control_depth() { - // Hold actual position until zero derivative is detected - static bool engageStopZ = true; - // Get last user velocity direction to check for zero derivative points - static bool lastVelocityZWasNegative = false; - if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5% - // output pilot's throttle - attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); - // reset z targets to current values - pos_control.relax_z_controller(channel_throttle->norm_input()); - engageStopZ = true; - lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z_up_cms()); - } else { // hold z - - if (ap.at_bottom) { - pos_control.init_z_controller(); - pos_control.set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 10.0); // set target to 10 cm above bottom + float target_climb_rate_cm_s = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -get_pilot_speed_dn(), g.pilot_speed_up); + + // desired_climb_rate returns 0 when within the deadzone. + //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom + if (fabsf(target_climb_rate_cm_s) < 0.05f) { + if (ap.at_surface) { + pos_control.set_pos_target_z_cm(MIN(pos_control.get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level + } else if (ap.at_bottom) { + pos_control.set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, pos_control.get_pos_target_z_cm())); // set target to 10 cm above bottom } + } - // Detects a zero derivative - // When detected, move the altitude set point to the actual position - // This will avoid any problem related to joystick delays - // or smaller input signals - if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z_up_cms()))) { - engageStopZ = false; - pos_control.init_z_controller(); - } + pos_control.set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); + pos_control.update_z_controller(); - pos_control.update_z_controller(); - } } From 81cd49213a293173f7887b0c3eeca2368bbb4534 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 4 Sep 2020 00:41:36 -0300 Subject: [PATCH 0636/3101] Autotest: Sub: tighten default watch_altitude_maintened delta --- Tools/autotest/ardusub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 68ea3b2f0b1..9183ab8be06 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -81,7 +81,7 @@ def default_frame(self): def is_sub(self): return True - def watch_altitude_maintained(self, delta=1, timeout=5.0): + def watch_altitude_maintained(self, delta=0.3, timeout=5.0): """Watch and wait for the actual altitude to be maintained Keyword Arguments: From 6e326ee912b85142f31f35538089071d2bee8959 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Tue, 10 Aug 2021 14:28:00 -0300 Subject: [PATCH 0637/3101] AC_PosControl_Sub.h: do not use our own input_vel_accel_z --- .../AC_AttitudeControl/AC_PosControl_Sub.cpp | 35 ------------------- .../AC_AttitudeControl/AC_PosControl_Sub.h | 6 ---- 2 files changed, 41 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index 0b88e82afbb..d309c137fb1 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -1,37 +1,2 @@ #include "AC_PosControl_Sub.h" -/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. -/// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. -/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. -/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. -void AC_PosControl_Sub::input_vel_accel_z(float &vel, const float accel, bool force_descend, bool limit_output) -{ - // check for ekf z position reset - handle_ekf_z_reset(); - - // limit desired velocity to prevent breeching altitude limits - if (_alt_min < 0 && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) { - vel = constrain_float(vel, - sqrt_controller(_alt_min-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f), - sqrt_controller(_alt_max-_pos_target.z, 0.0f, _accel_max_z_cmss, 0.0f)); - } - - // calculated increased maximum acceleration and jerk if over speed - float accel_max_z_cmss = _accel_max_z_cmss * calculate_overspeed_gain(); - float jerk_max_xy_cmsss = _jerk_max_xy_cmsss * calculate_overspeed_gain(); - - // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); - - // prevent altitude target from breeching altitude limits - if (is_negative(_alt_min) && _alt_min < _alt_max && _alt_max < 100 && _pos_target.z < _alt_min) { - _pos_target.z = constrain_float(_pos_target.z, _alt_min, _alt_max); - } - - shape_vel_accel(vel, accel, - _vel_desired.z, _accel_desired.z, - -accel_max_z_cmss, accel_max_z_cmss, - jerk_max_xy_cmsss, _dt, limit_output); - - update_vel_accel(vel, accel, _dt, 0.0, 0.0); -} diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h index 3d9c4f5f59a..03c73aded49 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.h +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.h @@ -18,12 +18,6 @@ class AC_PosControl_Sub : public AC_PosControl { /// set to zero to disable limit void set_alt_min(float alt) { _alt_min = alt; } - /// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. - /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. - /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. - /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. - void input_vel_accel_z(float &vel, float accel, bool force_descend, bool limit_output = true) override; - private: float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence float _alt_min; // min altitude - should be updated from the main code with altitude limit from fence From c63d12b6c9c131f7873460fdeab713e5882f238d Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 12 Aug 2021 14:17:25 -0300 Subject: [PATCH 0638/3101] Autotest: Sub: fix sub tests for new althold implementation With this implementation (same as copter's) 1450 is within the pilot deadzone, which breaks the test. --- Tools/autotest/ardusub.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 9183ab8be06..b9a94fd41b6 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -162,7 +162,7 @@ def test_alt_hold(self): # Make sure that the ROV will dive with a small input down even if there is a 10N buoyancy force upwards self.set_parameter("SIM_BUOYANCY", 10) - self.set_rc(Joystick.Throttle, 1450) + self.set_rc(Joystick.Throttle, 1350) self.wait_altitude(altitude_min=-6, altitude_max=-5.5) self.set_rc(Joystick.Throttle, 1500) From cb4f5ac5783a5adb70ce26686f5a4b390130f29f Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 12 Aug 2021 20:18:53 -0300 Subject: [PATCH 0639/3101] Autotest: Sub: tweak params --- Tools/autotest/default_params/sub.parm | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index 31ce6df1a6d..f9295726c46 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -84,3 +84,6 @@ SERVO8_FUNCTION 7 SERVO8_MAX 1900 SERVO8_MIN 1100 BARO_EXT_BUS 1 +PILOT_ACCEL_Z 200 +PILOT_SPEED_UP 200 +PSC_JERK_Z 8 From 836ae879556ed0f23c5b56557fc2d2bcc977c30d Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Tue, 5 Apr 2022 23:58:38 -0400 Subject: [PATCH 0640/3101] Copter: properly set feedforward enabled before exit --- ArduCopter/mode.h | 1 + ArduCopter/mode_systemid.cpp | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8f41a3ff581..a4008e3beb8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1471,6 +1471,7 @@ class ModeSystemId : public Mode { bool init(bool ignore_checks) override; void run() override; + void exit() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 0ce41da15ac..60cb56718b1 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -104,6 +104,13 @@ bool ModeSystemId::init(bool ignore_checks) return true; } +// systemId_exit - clean up systemId controller before exiting +void ModeSystemId::exit() +{ + // reset the feedfoward enabled parameter to the initialized state + attitude_control->bf_feedforward(att_bf_feedforward); +} + // systemId_run - runs the systemId controller // should be called at 100hz or more void ModeSystemId::run() @@ -179,9 +186,9 @@ void ModeSystemId::run() switch (systemid_state) { case SystemIDModeState::SYSTEMID_STATE_STOPPED: + attitude_control->bf_feedforward(att_bf_feedforward); break; case SystemIDModeState::SYSTEMID_STATE_TESTING: - attitude_control->bf_feedforward(att_bf_feedforward); if (copter.ap.land_complete) { systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED; From fe740681af91f127eae97d8af913eaedbc8e7ad3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Apr 2022 09:49:56 +1000 Subject: [PATCH 0641/3101] autotest: test_build_options.py: undef variables before setting them --- Tools/autotest/test_build_options.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index b130ba3e52a..c3ce942894d 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -31,7 +31,10 @@ def get_build_options_from_ardupilot_tree(self): return mod.BUILD_OPTIONS def write_defines_to_file(self, defines, filepath): - content = "\n".join(["define %s %s" % (a, b) for (a, b) in defines.items()]) + lines = [] + lines.extend(["undef %s\n" % (a, ) for (a, b) in defines.items()]) + lines.extend(["define %s %s\n" % (a, b) for (a, b) in defines.items()]) + content = "".join(lines) with open(filepath, "w") as f: f.write(content) From fbc9b771d1f8355fb774942977f879f68265dba5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 13:46:22 +1000 Subject: [PATCH 0642/3101] AP_Periph: updated release nodes for 1.3.1 --- Tools/AP_Periph/ReleaseNotes.txt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index 3f317063e3a..ab4c6b38fb3 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,10 @@ +Release 1.3.1 15th Apr 2022 +--------------------------- + +This is a minor release with a single bug fix: + + - fixed intermittent loss of GPS packets on F4 and L4 GPS nodes which caused loss of GPS lock on the flight controller + Release 1.3.0 18th Mar 2022 --------------------------- From ab5e8419d845a56ffecddb87965a8b61b3515671 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 15:03:31 +1000 Subject: [PATCH 0643/3101] AP_InertialSensor: move INS_HNTC2 to a new parameter table ID --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 8 +++++--- libraries/AP_InertialSensor/AP_InertialSensor.h | 3 +++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6de297ffd99..f368cf56f4a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -533,9 +533,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, HAL_DEFAULT_INS_FAST_SAMPLE), - // @Group: NOTCH_ - // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_notch_filter, "NOTCH_", 37, AP_InertialSensor, HarmonicNotchFilterParams), + // index 37 was NOTCH_ // @Group: LOG_ // @Path: ../AP_InertialSensor/BatchSampler.cpp @@ -552,6 +550,10 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Path: ../Filter/HarmonicNotchFilter.cpp AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), + // @Group: HNTC2_ + // @Path: ../Filter/HarmonicNotchFilter.cpp + AP_SUBGROUPINFO(_notch_filter, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), + // @Param: GYRO_RATE // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used. diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 284c93d6fec..d9d90d955e9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -302,6 +302,9 @@ class AP_InertialSensor : AP_AccelCal_Client // check for vibration movement. True when all axis show nearly zero movement bool is_still(); + // return true if notch enabled + bool gyro_notch_enabled(void) const { return _notch_filter.enabled(); } + // return true if harmonic notch enabled bool gyro_harmonic_notch_enabled(void) const { return _harmonic_notch_filter.enabled(); } From 47e95c2a886cf692b6a3151d839cfeccb43522fc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 15:02:48 +1000 Subject: [PATCH 0644/3101] Copter: param conversion for INS_NOTCH to INS_HNTC2 --- ArduCopter/Parameters.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 7c365efe643..841e2b081f9 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1387,14 +1387,22 @@ void Copter::convert_pid_parameters(void) AP_Param::convert_old_parameters(&ff_and_filt_conversion_info[i], 1.0f); } - // notch filter parameter conversions (changed position and type) for Copter-3.7 - const AP_Param::ConversionInfo notchfilt_conversion_info[] = { - { Parameters::k_param_ins, 165, AP_PARAM_INT16, "INS_NOTCH_FREQ" }, - { Parameters::k_param_ins, 229, AP_PARAM_INT16, "INS_NOTCH_BW" }, - }; - uint8_t notchfilt_table_size = ARRAY_SIZE(notchfilt_conversion_info); - for (uint8_t i=0; i Date: Wed, 13 Apr 2022 20:10:37 +1000 Subject: [PATCH 0645/3101] Plane: param conversion for INS_NOTCH to INS_HNTC2 --- ArduPlane/Parameters.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index f287fddfd89..76fbda72e7b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1502,5 +1502,23 @@ void Plane::load_parameters(void) } #endif + if (!ins.gyro_notch_enabled()) { + // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch + const AP_Param::ConversionInfo notchfilt_conversion_info[] { + { Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" }, + { Parameters::k_param_ins, 293, AP_PARAM_FLOAT, "INS_HNTC2_ATT" }, + { Parameters::k_param_ins, 357, AP_PARAM_FLOAT, "INS_HNTC2_FREQ" }, + { Parameters::k_param_ins, 421, AP_PARAM_FLOAT, "INS_HNTC2_BW" }, + }; + uint8_t notchfilt_table_size = ARRAY_SIZE(notchfilt_conversion_info); + for (uint8_t i=0; iprintf("load_all took %uus\n", (unsigned)(micros() - before)); } From f5c3b5642678cd0bc24baed531f94af046aef45d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:44:27 +1000 Subject: [PATCH 0646/3101] AP_HAL_ChibiOS: update for changed INS_NOTCH parameter name --- .../AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm index 375c5df8ec0..701276db1f3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/defaults.parm @@ -126,10 +126,10 @@ DISARM_DELAY 8 INS_FAST_SAMPLE 0 INS_GYRO_FILTER 20 LOG_BITMASK 65534 -INS_NOTCH_ENABLE 1 -INS_NOTCH_ATT 50 -INS_NOTCH_BW 80 -INS_NOTCH_FREQ 80 +INS_HNTC2_ENABLE 1 +INS_HNTC2_ATT 50 +INS_HNTC2_BW 80 +INS_HNTC2_FREQ 80 # profile parameters TMODE_P1_ANG_MAX 2500 From db9d796dd4688c58f366c7db75bcefe368426c8d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:44:27 +1000 Subject: [PATCH 0647/3101] Tools: update for changed INS_NOTCH parameter name --- Tools/FilterTestTool/FilterTest.py | 8 ++++---- Tools/FilterTestTool/run_filter_test.py | 18 +++++++++--------- .../SkyViper-2450GPS/defaults.parm | 8 ++++---- Tools/Frame_params/Solo_Copter-4.param | 8 ++++---- .../Frame_params/Solo_Copter-4_GreenCube.param | 8 ++++---- Tools/Frame_params/Sub/bluerov2-4_0_0.params | 2 +- .../Sub/bluerov2-heavy-4_0_0.params | 2 +- Tools/autotest/arducopter.py | 9 +++++---- 8 files changed, 32 insertions(+), 31 deletions(-) diff --git a/Tools/FilterTestTool/FilterTest.py b/Tools/FilterTestTool/FilterTest.py index 5b70641ad6b..dedffbe76df 100644 --- a/Tools/FilterTestTool/FilterTest.py +++ b/Tools/FilterTestTool/FilterTest.py @@ -455,10 +455,10 @@ def print_filter_param_info(self): filt_type = f.get_type() if filt_type == BiquadFilterType.PEAK: # NOTCH - print("INS_NOTCH_ENABLE,", 1) - print("INS_NOTCH_FREQ,", f.get_center_freq()) - print("INS_NOTCH_BW,", f.get_bandwidth()) - print("INS_NOTCH_ATT,", f.get_attenuation()) + print("INS_HNTC2_ENABLE,", 1) + print("INS_HNTC2_FREQ,", f.get_center_freq()) + print("INS_HNTC2_BW,", f.get_bandwidth()) + print("INS_HNTC2_ATT,", f.get_attenuation()) else: # LPF print("INS_GYRO_FILTER,", f.get_center_freq()) diff --git a/Tools/FilterTestTool/run_filter_test.py b/Tools/FilterTestTool/run_filter_test.py index 7a1f67859e6..a754b5fc64c 100755 --- a/Tools/FilterTestTool/run_filter_test.py +++ b/Tools/FilterTestTool/run_filter_test.py @@ -72,7 +72,7 @@ PARAMS_TO_CHECK = [ "INS_LOG_BAT_OPT", "INS_GYRO_FILTER", "INS_ACCEL_FILTER", - "INS_NOTCH_ENABLE", "INS_NOTCH_FREQ", "INS_NOTCH_BW", "INS_NOTCH_ATT", + "INS_HNTC2_ENABLE", "INS_HNTC2_FREQ", "INS_HNTC2_BW", "INS_HNTC2_ATT", "INS_NOTCA_ENABLE", "INS_NOTCA_FREQ", "INS_NOTCA_BW", "INS_NOTCA_ATT", "LOG_BITMASK" ] @@ -226,18 +226,18 @@ def set_bit(number, bit_index, bit_value): if "INS_ACCEL_FILTER" in params: DEFAULT_ACC_FILTER = params["INS_ACCEL_FILTER"] -if "INS_NOTCH_ENABLE" in params: - if params["INS_NOTCH_ENABLE"] != 0: - if "INS_NOTCH_ATT" in params: - DEFAULT_GYR_NOTCH_ATTENUATION = params["INS_NOTCH_ATT"] +if "INS_HNTC2_ENABLE" in params: + if params["INS_HNTC2_ENABLE"] != 0: + if "INS_HNTC2_ATT" in params: + DEFAULT_GYR_NOTCH_ATTENUATION = params["INS_HNTC2_ATT"] else: DEFAULT_GYR_NOTCH_ATTENUATION = 0 - if "INS_NOTCH_BW" in params: - DEFAULT_GYR_NOTCH_BANDWIDTH = params["INS_NOTCH_BW"] + if "INS_HNTC2_BW" in params: + DEFAULT_GYR_NOTCH_BANDWIDTH = params["INS_HNTC2_BW"] - if "INS_NOTCH_FREQ" in params: - DEFAULT_GYR_NOTCH_FREQ = params["INS_NOTCH_FREQ"] + if "INS_HNTC2_FREQ" in params: + DEFAULT_GYR_NOTCH_FREQ = params["INS_HNTC2_FREQ"] if "INS_NOTCA_ENABLE" in params: if params["INS_NOTCA_ENABLE"] != 0: diff --git a/Tools/Frame_params/SkyViper-2450GPS/defaults.parm b/Tools/Frame_params/SkyViper-2450GPS/defaults.parm index fbc2a3fac09..320e4d15e4e 100644 --- a/Tools/Frame_params/SkyViper-2450GPS/defaults.parm +++ b/Tools/Frame_params/SkyViper-2450GPS/defaults.parm @@ -117,10 +117,10 @@ DISARM_DELAY 8 INS_FAST_SAMPLE 1 INS_GYRO_FILTER 20 LOG_BITMASK 65534 -INS_NOTCH_ENABLE 1 -INS_NOTCH_ATT 30 -INS_NOTCH_BW 60 -INS_NOTCH_FREQ 80 +INS_HNTC2_ENABLE 1 +INS_HNTC2_ATT 30 +INS_HNTC2_BW 60 +INS_HNTC2_FREQ 80 # rate controllers ANGLE_MAX 3500 diff --git a/Tools/Frame_params/Solo_Copter-4.param b/Tools/Frame_params/Solo_Copter-4.param index b1ecf7c488f..0d9f0961108 100644 --- a/Tools/Frame_params/Solo_Copter-4.param +++ b/Tools/Frame_params/Solo_Copter-4.param @@ -83,10 +83,10 @@ INS_HNTCH_ENABLE,1 INS_HNTCH_REF,0.26 INS_HNTCH_FREQ,200 INS_HNTCH_BW,100 -INS_NOTCH_ATT,40 -INS_NOTCH_ENABLE,1 -INS_NOTCH_FREQ,98 -INS_NOTCH_BW,49 +INS_HNTC2_ATT,40 +INS_HNTC2_ENABLE,1 +INS_HNTC2_FREQ,98 +INS_HNTC2_BW,49 INS_TRIM_OPTION,2 LAND_ALT_LOW,600 LAND_SPEED,45 diff --git a/Tools/Frame_params/Solo_Copter-4_GreenCube.param b/Tools/Frame_params/Solo_Copter-4_GreenCube.param index f58d904ef6f..221f9f39c8d 100644 --- a/Tools/Frame_params/Solo_Copter-4_GreenCube.param +++ b/Tools/Frame_params/Solo_Copter-4_GreenCube.param @@ -82,10 +82,10 @@ INS_HNTCH_ENABLE,1 INS_HNTCH_REF,0.26 INS_HNTCH_FREQ,200 INS_HNTCH_BW,100 -INS_NOTCH_ATT,40 -INS_NOTCH_ENABLE,1 -INS_NOTCH_FREQ,98 -INS_NOTCH_BW,49 +INS_HNTC2_ATT,40 +INS_HNTC2_ENABLE,1 +INS_HNTC2_FREQ,98 +INS_HNTC2_BW,49 INS_TRIM_OPTION,2 LAND_ALT_LOW,600 LAND_SPEED,45 diff --git a/Tools/Frame_params/Sub/bluerov2-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-4_0_0.params index 08bf4748d1e..bc3f9053906 100644 --- a/Tools/Frame_params/Sub/bluerov2-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-4_0_0.params @@ -292,7 +292,7 @@ INS_LOG_BAT_LGCT,32 INS_LOG_BAT_LGIN,20 INS_LOG_BAT_MASK,0 INS_LOG_BAT_OPT,0 -INS_NOTCH_ENABLE,0 +INS_HNTC2_ENABLE,0 INS_POS1_X,0.0 INS_POS1_Y,0.0 INS_POS1_Z,0.0 diff --git a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params index 9c76f4e1d13..4cbb5e830f8 100644 --- a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params @@ -292,7 +292,7 @@ INS_LOG_BAT_LGCT,32 INS_LOG_BAT_LGIN,20 INS_LOG_BAT_MASK,0 INS_LOG_BAT_OPT,0 -INS_NOTCH_ENABLE,0 +INS_HNTC2_ENABLE,0 INS_POS1_X,0.0 INS_POS1_Y,0.0 INS_POS1_Z,0.0 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 93a8dd2a14c..3253c57f06c 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2679,10 +2679,11 @@ def fly_motor_vibration(self): # now add a notch and check that post-filter the peak is squashed below 40dB self.set_parameters({ "INS_LOG_BAT_OPT": 2, - "INS_NOTCH_ENABLE": 1, - "INS_NOTCH_FREQ": freq, - "INS_NOTCH_ATT": 50, - "INS_NOTCH_BW": freq/2, + "INS_HNTC2_ENABLE": 1, + "INS_HNTC2_FREQ": freq, + "INS_HNTC2_ATT": 50, + "INS_HNTC2_BW": freq/2, + "INS_HNTC2_MODE": 0, "SIM_VIB_MOT_MAX": 350, }) self.reboot_sitl() From 4c9a851fc6bafc7ffba2a3eff5011015bb8629a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:44:27 +1000 Subject: [PATCH 0648/3101] SITL: update for changed INS_NOTCH parameter name --- .../examples/SilentWings/Params/Rolladen-Schneider-LS8b.param | 2 +- .../SITL/examples/SilentWings/Params/Schleicher-ASW27B.param | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 1c308a3397a..bb7306c1071 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -245,7 +245,7 @@ INS_LOG_BAT_LGCT,32 INS_LOG_BAT_LGIN,20 INS_LOG_BAT_MASK,0 INS_LOG_BAT_OPT,0 -INS_NOTCH_ENABLE,0 +INS_HNTC2_ENABLE,0 INS_POS1_X,0 INS_POS1_Y,0 INS_POS1_Z,0 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 735181a2f21..5bf01819c15 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -249,7 +249,7 @@ INS_LOG_BAT_LGCT,32 INS_LOG_BAT_LGIN,20 INS_LOG_BAT_MASK,0 INS_LOG_BAT_OPT,0 -INS_NOTCH_ENABLE,0 +INS_HNTC2_ENABLE,0 INS_POS1_X,0 INS_POS1_Y,0 INS_POS1_Z,0 From 973befa5d82920b56c2abcf6b4d791763e42f4ea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Apr 2022 08:11:08 +1000 Subject: [PATCH 0649/3101] GCS_MAVLink: prevent devop larger than buffer --- libraries/GCS_MAVLink/GCS_DeviceOp.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp index 2d93f687987..b7418ad78c6 100644 --- a/libraries/GCS_MAVLink/GCS_DeviceOp.cpp +++ b/libraries/GCS_MAVLink/GCS_DeviceOp.cpp @@ -48,6 +48,10 @@ void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg) retcode = 2; goto fail; } + if (packet.count > sizeof(data)) { + retcode = 5; + goto fail; + } if (!dev->get_semaphore()->take(10)) { retcode = 3; goto fail; From 95ae150a213c61deda0ed0d15a55fa88783e3cea Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Apr 2022 08:13:09 +1000 Subject: [PATCH 0650/3101] AP_InertialSensor: catch FIFO overruns on BMI088 we have seen errors where the BMI088 gets out of sync, so that the 3 axes are rotated. The data is shifted by 4 bytes, so that X=Z, Y=X and Z=Y this changes the BMI088 to "stop on full" mode, which is what Bosch use in their example drivers, and also catches FIFO overrun events and triggers a full FIFO reset. This should fix the problem with the FIFO sync --- .../AP_InertialSensor_BMI088.cpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index f6df2e247fe..11afa0bbec6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -254,8 +254,8 @@ bool AP_InertialSensor_BMI088::gyro_init() return false; } - // setup FIFO for streaming X,Y,Z - if (!dev_gyro->write_register(REGG_FIFO_CONFIG_1, 0x80, true)) { + // setup FIFO for streaming X,Y,Z, with stop-at-full + if (!dev_gyro->write_register(REGG_FIFO_CONFIG_1, 0x40, true)) { return false; } @@ -374,38 +374,41 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void) _inc_gyro_error_count(gyro_instance); return; } + const float scale = radians(2000.0f) / 32767.0f; + const uint8_t max_frames = 8; + Vector3i data[max_frames]; + + if (num_frames & 0x80) { + // fifo overrun, reset, likely caused by scheduling error + dev_gyro->write_register(REGG_FIFO_CONFIG_1, 0x40, true); + goto check_next; + } + num_frames &= 0x7F; // don't read more than 8 frames at a time - if (num_frames > 8) { - num_frames = 8; - } + num_frames = MIN(num_frames, max_frames); if (num_frames == 0) { - return; + goto check_next; } - uint8_t data[6*num_frames]; - if (!dev_gyro->read_registers(REGG_FIFO_DATA, data, num_frames*6)) { + + if (!dev_gyro->read_registers(REGG_FIFO_DATA, (uint8_t *)data, num_frames*6)) { _inc_gyro_error_count(gyro_instance); - return; + goto check_next; } // data is 16 bits with 2000dps range - const float scale = radians(2000.0f) / 32767.0f; for (uint8_t i = 0; i < num_frames; i++) { - const uint8_t *d = &data[i*6]; - int16_t xyz[3] { - int16_t(uint16_t(d[0] | d[1]<<8)), - int16_t(uint16_t(d[2] | d[3]<<8)), - int16_t(uint16_t(d[4] | d[5]<<8)) }; - Vector3f gyro(xyz[0], xyz[1], xyz[2]); + Vector3f gyro(data[i].x, data[i].y, data[i].z); gyro *= scale; _rotate_and_correct_gyro(gyro_instance, gyro); _notify_new_gyro_raw_sample(gyro_instance, gyro); } +check_next: AP_HAL::Device::checkreg reg; - if (!dev_gyro->check_next_register()) { + if (!dev_gyro->check_next_register(reg)) { log_register_change(dev_gyro->get_bus_id(), reg); _inc_gyro_error_count(gyro_instance); } From bca4fc9608ab2c5b1ac5236efb6dc1c95d513d41 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Apr 2022 14:57:13 +1000 Subject: [PATCH 0651/3101] hwdef: disable serially-attached actuators on SkyViper --- .../AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index 15430fe5cfc..e1129f4989f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -67,7 +67,6 @@ env BUILD_ABIN True # Disable un-needed hardware drivers define AP_AIRSPEED_ENABLED 0 -define AP_FETTEC_ONEWIRE_ENABLED 0 define AP_OPTICALFLOW_ENABLED 0 define BEACON_ENABLED 0 define GPS_MOVING_BASELINE 0 @@ -92,14 +91,18 @@ define HAL_RUNCAM_ENABLED 0 define HAL_SMARTAUDIO_ENABLED 0 define HAL_SPEKTRUM_TELEM_ENABLED 0 define HAL_SPRAYER_ENABLED 0 -define HAL_SUPPORT_RCOUT_SERIAL 0 -define HAL_TORQEEDO_ENABLED 0 -define HAL_WITH_ESC_TELEM 0 define HAL_WITH_ESC_TELEM 0 define LANDING_GEAR_ENABLED 0 define MODE_TURTLE_ENABLED 0 define WINCH_ENABLED 0 +# serially-attached actuators aren't relevant on SkyViper: +define AP_FETTEC_ONEWIRE_ENABLED 0 +define AP_ROBOTISSERVO_ENABLED 0 +define AP_VOLZ_ENABLED 0 +define HAL_SUPPORT_RCOUT_SERIAL 0 +define HAL_TORQEEDO_ENABLED 0 + # SkyViper has only one physical GPS but can also take from mavlink: define AP_GPS_BACKEND_DEFAULT_ENABLED 0 define AP_GPS_UBLOX_ENABLED 1 From 9f58c73780b2c6d6408fdebba461acc3b7235965 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 07:21:19 +1000 Subject: [PATCH 0652/3101] hwdef: Added MatekL431-Rangefinder --- .../hwdef/MatekL431-Rangefinder/hwdef-bl.dat | 3 +++ .../hwdef/MatekL431-Rangefinder/hwdef.dat | 13 +++++++++++++ 2 files changed, 16 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef-bl.dat new file mode 100644 index 00000000000..ffb40904b01 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef-bl.dat @@ -0,0 +1,3 @@ +include ../MatekL431/hwdef-bl.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Rangefinder" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef.dat new file mode 100644 index 00000000000..97d9233199d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Rangefinder/hwdef.dat @@ -0,0 +1,13 @@ +include ../MatekL431/hwdef.inc + +define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Rangefinder" + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE + +# support all rangefinder types +define HAL_PERIPH_ENABLE_RANGEFINDER + +define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 2 +define RANGEFINDER_MAX_INSTANCES 1 From ea3ff853c75c81ecd1cd35901a293e20abed1f47 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 07:21:45 +1000 Subject: [PATCH 0653/3101] Tools: added MatekL431-Rangefinder bootloader --- Tools/bootloaders/MatekL431-Rangefinder_bl.bin | Bin 0 -> 21324 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100755 Tools/bootloaders/MatekL431-Rangefinder_bl.bin diff --git a/Tools/bootloaders/MatekL431-Rangefinder_bl.bin b/Tools/bootloaders/MatekL431-Rangefinder_bl.bin new file mode 100755 index 0000000000000000000000000000000000000000..dea889eabdb15e85fae2e5141b1ffc3809129c29 GIT binary patch literal 21324 zcmd74dw5jU)jz(^WoAw$Loy)&LlWR*1~4#Oh6FW;mC0l{2^la5+N!j#lLYD+K#8C& zmMRm1HGDPBJ4#tfzXAJvgH3>_t~jL_z>X}1lr#G|E132rT_PK#sDT8;TnW}|4SSHj_d!Q z^SX2*knjCl(OS|O?RJ!QXBB6g-WpUd zPmVqo5eCO!M0ufp&=B71GpqqD)i^DXgK1qL=8hr zx;c{=HS{x4eP34L0Hx$&f^^g1fGV0OCJBS-F${#kBw)Zs^}UHPEQGLjy-%s})qqA1 z`woU#q0AM>Wk5-Y<5EBdlSY0U!-Zi$(|PO8{C3fr4mQ8c<>(e1`v=+G&-NrS&pJ;@ ze~^BBc17T4O-FhJwhU!V)DW=%kFaBMA!v0knHHxj=ALVJ(3#JUa(%lTOyL!Z9f{N6 z+C)=fx4sZPUw+!jN7;T#364YW`E$-Z?VN?)D_jkkk7Dj4mkopC^S~(u`oW~W9i!9* zr{s@6v%{Iz!QNQrI-MbP6DdD5Tgs7iA-#0*i*!l-!oBFFGp#+Wu&T$D#N;0cNm6%6 zly-+4QZN*dHiy^+Wv<+iT}lX1-f@O1i-@1?BnnYSLK|MczS!3%)Xc|k0)B70EZqFp zUcv|-M=v2CAl!6$-o5b>e_!e2{=W5((=W$+6?~}~g}w5nR3YTkZ_LJU!4eQ zh=^A$PX8VZulxjUeLj%3ByH&*o!o_6R6et&XdpzC0ySkx$`8tTWnn7>0OT}DK_HL^ z3}WPuFGm_>k0I0|+%TVr`xzoU|Gj|qUpnh+^Y#;}DR5HWC$)WL-j3DwII&jurPSm| z`-_OkDg4Mth*KUMCjT4>5u$_uJN;+4y)PTGHLvilk5+yIkDeKN=8G=b%cI+tdN@2AR?x;w5IjY12fe5>vU z@80ITtw3lqv=sD?<=AZI;43O`XB1848h@tpgXR`7Ho~#RC*{N@PDva|DM>CSR~c1euw;Ai(F&Q5eU$Xu9IaF@iwdgrV&yppN%$}E+j zy??0V-K>~i$tbrDjC0rDO)T}R_pYzMYw!AwyDzNnSbyQJj`b7n$@uwM)n`7b`b5O` z4X>9fomMgXq$(^u*F|!xFn9Z;YVf|f2$=0Bf`~wQf#OBXtb@*bUE?3!q*kO zJM-Lf)v6Hxd4UMS2)h!n&m+DPrW%P5Nrb+Duo0mF;RFKxes(loWFo@-qxBv|`cr(n zlBac8QH%79kQ|3HSVbDx?4x0lEcmk|-9hk5-YaSvcFZ1aUkJQVNt9EXVN%aWv`!Fp zz6dZ2b#q8>XVpKyG$@>Lj5+mMomE%IXX_3UC7@l^`IBNkdmywDzn5dbeW6k%E1?)% z+@~dk`2<~U9QF;ZH9Rx9H^_(t61b>4G4ammjzLHnBcq&Br!^%iiCm%$JNXnX^3=$W zLyOVkhaocig+Tnvvq6f}BO?Zh49Xa^+JB_hB6uFAO zrY~f5lc^Icv)xCfpLuSSUIb1H`&Ygi$7)Oek6+D<@h?#>X}Ka%aApu;I_l$^KGwn-;GpZN*3Au3ntwP< zTm9EzqPR4g`s6UVx>c$KwT?N{J0{ObX*V`7c}an*6^M;Kxvd15G=-*Ad!?rQ1bdfTozG0wTNXn ztC&+hjHWim_9O7v^ut$pY|3FuCEY{p}e*KHw;NANtGA(ddc4AOcNPo z@=`r|ce?EAT~@ZWnV6VCmZW=p&6P!IfH%|7LbA;k9m;oRK&qNmK0F7J=;3HNK0 zA!`dQOOG`&%JPvzvZMEF%gKy7XKsi7)q@_Jlw%>;`4X#G26f>iLX=;OkZHVhXVf=< zywKMiCVLk#;T*=$A@t}u!9rn8gUq$zkYHsjA9k_1x_Kt7o#&hFR#Nw27k{7bz}YTh zoxa&n%qqzP3n=}nU+p8lO_%eYseA9kou~4Mcpl*c3lS?@#z<+9*sM~kE^B3Hn}f{E zq~O?C3(y9wjA-c0nQ0E_+L@UN0o}~x0P3{aXATAvV=!NZ;_;WG@n@rPC-!C9r(@!? z;%2KVqQ6;S}pcf(INQ)^P)pyl=rkTpf_F1lrOIE z!E2D^!p%f5w;0{Vk_-=b$+x0T^@!KEeth@LCK2_n9TX)_X&q_%((K@h8D;h{6?`i| zw{?Jr8FzXm)%lO39i=7DowfPl#(7P-?j%b_6`3MRx<;jKPKjB*S!NG$Ck5-Q#j}gY z^l&Vv{A)xf*;}u6+lq~VWfWiZixA^&x^&w@eTT(8tvE4OdVloeP~i?o?=)wf(-6p7 zY!Zd436cmrU=9Ba>av11oVNoa9l^o7tE#0#R#lxMMi=sKnJX#;&ezLO(V zkDG~fK0*nC;L##7;)f9qBgA;RQU9t>GNHGF7$3>GBWQhywPgk`Zf9)iK}Ja#(o0(8 zx5FH?=TnvcR0B^O8B}`pm&X5s#p!%}w7T!KO6(UO%4<+Gr|-uPZ2zX2 zS#ZRu3p2XJjs+7Zxw;5>l2`=`qo}db{YLvLWC*7bE%M7@j5?ggE7$_})B;L`Yww2xqDFiWjuBvOZUEK&Nb)&@3FPAMEY8M?SV$2KisZf9(h0y^8V zAU%a`Tl}#5J(=zAEKeh~f{V`ba(3Yi4-kQxHHP<=2Brf=`hisecovQ;M zN>7KfB?m@q7}*;-u$@`Z(0tmqyZyeUvo}vWrVE=mQYTpQU`e3;Pt`VN^)`2qNfL#u zcj_)A<<4-CC+H}@!G23DR3Z}VAvr|zS8bf%n>I831d8C>=9xkCb62Yj8JXxtI+S%zhmENj-JPe zcqy|ro_`~j&vQKm-lioCwBp8SY4?+f6ce-@-m&OB&Ke0F#-196>!~4(g(z1K(l);h z^`$$*gay5FX{)rpzK0V)GapeNjQSQOoJE-(-w2aOGDqo2H1G+rf}YCnqgv#u5ur(o z{B*!3CzLc8u@6^K+NXuC>>cky4v?=&Cr_ffD{JKx%FHdm@D`KXBG)@pf{E_(2Qw;- z#nuP8jmE{NElC*18_`ZkczUw@OOz3@ABaO(Z}qRZ-lcMcwl$s*@w%4urRj?&6tmc8 z*WOE%A8FVR>&CmsFD7;4i?712;9Jfpe;yj=p0FvUX)IRm7eiR{Q6BvE$spF(#>T?cCKD>tJ{?1ZcaW$1@+&Dsh~a`X zUDva|W_Kj_uA1J_MIPO`q^@!2O}{OrvtNys>^VZFe%O_KYtrvtQC-W}%{x_>xL0<_ z|5R^yF8O8d?M%5)A%#hGjFJ}JI-wv;IP$xsm#F^94jy#sKB4VY3~yJ-vh$&OtW1NA z^uF$6@=#(=Jp9bp_v|3n4W<87`=2$|y@K5$v2LRW6i!?z?fjbTV5W`O5I zY^OT05oh(0eQK7S*c{4}KH7RYqz=hh6f7@gl)%6O$iIbW8Rh+ft)NtKj8eu+<9~I9 zS{aZ+3p}@#{$3@G?}yFCV>q9s`ST~A4Zac9C+EP*sW&Da;(8L9 z<>2}0;Nf)@RYfidApoba+SHe**0KKUg_k`GnmcOT{g zb8R;^B-JIo^huWxTh-EteN^{?EhzTyiVjBk>I$#XbwbzAI|v8o#d)6c;vbO?|M|re z0a-|X1!=;QVxdJ>Oq6YdZC?rwu7qDqEkZqS5I^Nvat#r^X1e#NrIaTBRB!Vk)as=% z5lS5pnkB@6w~Ci5;P@3l)}sm;@Swi33V%fPP@fmJrV zMZ&~GE{P5zLo-482|{zM(gnR!=gyFI?#!BojA|#_VRo^~laM}@MYJw$BfrJ4l$Ua3 z5nmo3&S|BowlDKhyS^szhrg<)LI>KM89~KI>nyXguQA_6;gsli=6N? z6w?11>OfDT*!YRacUHC}xqm9x4G13RB%_PDAyI;Eb0%$ox%0NHSxrP`e#%LgLg|vO zbLNz-&a6tNV^7w9RXQ^0II}PZY!2&ZA^l*PDvPxV9-(E7TZos$@{FG?S>dJJyn;fZ z4Cf?k9V~ty{Pdk$n7n}y)x$Z7l>!-K2bfGUJsPYmVqF}tVFNhtXGUSo1z#0TUYaKT zPMsneq#9}Yv45(SwND*E-fnB9=fz@aPPrDjsG)=$4-4#b&K$o*`tRlf=?S9i@j)Ls zNx#{j&z$7C+$_w#%2n(7todPeisz%C7OB9w^Y1~S)3%1|*~(Qr{}#|9x1;R+AoQ*; z)oMl@3{m>A?u<uvb~>a+jixU#xW{hdJq)xgvttF86svsQO8m4Q?T7MW+zwa%I;cqE zcFB0!{2D9YB4^;-1dFCSy(l)@bjA@VYz!XhBwBXKx2#q@MUMA5;^j0ya5|R8{!(C) z+tl>RH0+y`r1inX&4O)RyB2v{TjV_?uVhT;4#}B}aPE(Cn&P|VXQ10LO{98Z6*DDK z#(Bj=Y)Abck3^R`u`M9Vt8cPXUVtv8 z*`Eoewty(yJzJ}so~@qBBJj6`SdYhaL|)>mOpr|9f=-mrl@@5@Xu%|57VN1CpM0{<#|Zuu zsjP1+$A`D+eJI!UNU4uDWOXEhxi`K9U=TsYmpy==})mv5KT zB<>v{@0imfMS|(F=xL}q;QOR22`4XTDb`n1!ETeiQs%lGX-^T_9cOzW!|+qgjlkLN zWje`>{$K*mMwt5|tgA)?C?UgcTZ8`oGO`Q5do)Uyx}nKA(>o|tywDe(9u{*fLGBfo zq&qZQ;{N)yliE72k#NH1F9+?w;z$e&Vm%Ek+JM8$&>^(Q^CKnF3aLQKmjr5!C?ZP7 zF)gxp1m`%Ke}taL)ly13CU2^nmmF&Pf@I*ZOyk2jSi#E?7YqEQt+z^&aE)COuZ7;A z9fm!fzvR0Wde{o+BMI+zrOtm>E%q_iZ&ve{9=Q!C;UB-N-ctLpdYk=W)ngZ~6~cl5 zNF&u=q%PsmAHwNqeWE5_E2%k8NsIb}538puuWF+E-7kiDcSRZHauw$cdfx2BS|qE8 z!`#9{U)t+@%x|=&SZ|xuGT1l74Q8!) zr`guT=0Li(Q|Y2SVhU^5zJAHKK62)ro_CUx!W>uHeKFx=-(R@rp)KQlZN#2>KkT@; z=hNMKN3=$58B{N|Tq@gg!k>hjQwrIcalHAbS<9l^`b~p;|DOi?`=53GEyyhRs5#m8 zemj$YyxEZRmv&gF?j7&jRe83O5l@5!$&WpCA@ol2+Bo3hrsQsxwfJA$(= zddB`cW`Mb&FDOctZ4A|siM_??t@EZTmC54+u=p>jE91i3SkmJhoASU?L&Et-4Tkdt z-dj}bvYnok2MQFPNx7HF<7_=ads{!~x3OiSROlTq)yroLyp(XhUS_iQdfx7Hu;AO( z1iST`kqU3U9MmP^8)o+2*jL!O$Z7@}yB4`4_WcYu`h9NfyOVxrW8X_gM2Q7gRbqV} z^y98dlDuU4x}ZL6GUUrX!*mf`5rzzk5EewAD*JaP2gGw!b{-jgc`>8PM6V|EfK5rH#e&}3q-8qJw@R3 z<)ZWiqy&|(TM>gZF~)2C`mousE2Or8FR?}~LsWxZ-pw&=fVsW`+CINnkj19#*bbvb ze&L5a@~hxi6*8^*=1HLGUx)swx{eET$rMN;9qCyFX>q3^>D)(p)A?W*+4y_skpjb+ zLhn!2DV5K7Y!6TjAVr|3G4K%hEEvb6=}qWQm}8+ui@Y$TMY>?Kp;GdNH|d>Wp;ssE zk%`i|tTEhqXRG{)wh`Ek!kwyHcr=5n3@SWr9aPH%gyG)BHxGm5ku!n z!{u|?kRC)yh>{!y`_JYfHO`TcskFblhp1e>0jI`$h6)o*(0m|UAa_;p8?mNVCQEwQ zIkiYArVHj*PHmzVIxTWU6Dw7~&FVT@V3Nj3lO-Q|@eL+Q*_&7?p*3gIvLK z*V@Epr;1u+98QWwLu_^$Zodi)*{tP{tgZL9o6KUcHbK~GbIEIdIQiWCJx^$RRQaz!+%jp zZM^=^0`b-K494}h!E$e^Y{ib8>Y*u$L8_<1ORm1NEDrU~pceTG_IsQHd|!gjzkv0y z`t~=Zgs_l+{W6>GQkFsXAZ$NFAFAp5*GLIH1`Za8q}|e+IH~k6O84CEDVI1{VlM?h zRtYTV;&o4{yqIyXXU-?V-(kG#u@91~A4X~ob_!8?MJ1K5sBGmtK(bQ{%e{$(XUi(Qsg^s+;`E_Z znGejg$j@>M+3zW*-^;ykt7*CP zm&b3xW$oU#Rnu6Scf?9vxfNcS`(#EEj$K(8=`9%dWkYU zx~!YwScwfdCzXW>16@!yHzfkk;^CMss;p}rg$3t*HCk5Qio0^;KO4>8(VF}kbXE8S zrM9e-wtcB5+{E3OyXr4>gb}G8w6W!3nb~z~Q)bOdgW+7Di(LB=orC%l&35S^v`@CN zvQ26Ejr?e%4i?D&YHCyS?JEsN3V9ucv`8nK^QA^WQq6o=NF^F*Jo5<1w%YUkzPZs$y#j2JN-GPiKQ z=B3nDBC|>>EYDPTBUXRk%EwQ_;dL_hiprdenwUY)r?iek-qVB{M2oberfIb18Eu=>F75W;Et9EQ zwVvlQ7s!V^ zqU%c!D_(-{O<(8=4$q9@U;_3J{4Y3s^#dHVf*Q#SEVRgBwh~j-09exQTg*&ck_)X~=8MmEY}R^Khz|!ic8Mr=0nI zW_ON<*&}>=51D&R*;ObXpsO$LNJo!Ix_8#7|A)6_uZlj znMWr$@%0f^!bt3+BcO9fXRZ^s@eFuZi%fv#h#4ZJrJ?cLzJw~-LgnZ>_1ivOHSexp zdtpuecmEsK!1D&S&L6`5_&eBtgY^F=tRn`?x$jte1@=mt)Tbx;HKB#w!zix~j)WLx z*;|Z~gD~wadrQJnqidIQOdv&N7To3#td2gyGYe)qQWcHc*1e2e4t^W5w~5t9zovZl zu6JDPi84B4ZtN(YqLY2wrPA)R4`HQk8sBZz=lgGjwZ$ggA-~mgW{U-@)fgZ%(l0!3 zdd+!NaGx_dIL7mjP^$Dz=tZX?$SOY9+kL(b&P9oVTbjNN31_|zeV`I^qUXcVYfdKc zn)CDa4EZUE%=kPw!E-V;H`JezQ4D-(_j$qM(kTY$GeJ_?zh&`mCj(7Id4yLC29z6r zvgk%B-~VIj&sec3nZL!lNRmDkgr4(B1wTzyla9AKrbqRW?5HuUH9ywPFhx;gWWEC_ z6Nj(!T-UhHhkV_PC|C=6vstFuLCADpGkgZ~j{29y=+h62MtsfgOn<>ig86M}uxmi( zLJ=W^l}nmZWzKW5hIy39YiQoB z*0dHfKsXFJ`N3<>9FNyy#T}ENyBsv*d{x;dNUaD72u6fN1lqKZ8}d&# z>t;`m`qbq1+C^NL;q+$*w;NsMeF@y)Rz}R|%utEvi(43bcc^wz_{_6mePWjN*YYvg z*SW#5$0kR2TqZLY1>MZo&G>Z{6fsudsxk-LRhv@RpdS_iXaK(u`c8(K5Hv&fu|I}OzqShPqVX7u6q!R^459p_pN9Nr8uxlc4_NmlqW4FnHOPIP5u z9~lhy(OIi^o;^b?)PY#<<214L9oVh?A;uC49Y^ZFB+rtPLzv3jPeKRDL%}3gnan!7i^m>z3edJ3 z#+A{ka7j071QyOJRoK5nHIw(DlieQM1I`UI^A0Vik z?aRhIy-&?|w5g6tAGoIdTFtQzM;!T zhDF$;XR!``Q8&NJVRCglrr^uQ>`q6wPBhGMp!9MFnM%vxwM1Tk=hBR*E}RITNT$#h zWqVT!)1}jx)#Jm$HI7-F)DvqR`z3CJz(2pw#Z1zj{oF|2CMNHr=E=4iKe6Ujf>UtY zupS;z1C(OqGSv-AA8vd?`UG~9^%xQLjH4qe4o;pSwdZXjHYs3Y zUES;|jNI(%K0S~6L1z^j=C~9epW6ZdNl^D>STr2~-ZP>ca5lkn+{D;oxSwt|OI)26 zSvAZx2zd@0D}4#f3xMg}am*i~=PSB5|8k@?%p~iBw(sF@tJKBtr*!#p6n{FFKFrl| zFKOcKl|?7L8S>j@Z_63q$)A{ydqF@=_f5DdO`3?_$khq0bcYJU2mZN?)$mz<()mmL zPC8TV-QgUA)8&)H)!t0Gx(vRLkYXo1RbDE|YNUUKV6z=gluuz5UHL}8v|~>?7pjpG zWa^LV+cpXEPAT2dI0kfGR+|P&;#7&#(}XI@BSaZjwPMmj+_I%qF*CRzl^zZy5x1Kk zb_sb$are{Gb27QK`=o(8OT@(3m~9iDakj&vdcA*3rq+BlZfV<9Gj7;^6__*YCiu45 z^?!0=y>c`Cmz&|sS2ta1SK;<}C>ON6f9S;?z400sy*;C?b78VVzu+Un*U=WaI(s(P zklI8~j5C91l{jK)U4T3(99k}UGayrk@|($=qk*3ri~-!O^_*kMohhnt1FIO=)B*-6 zVdf1T=9!M(7N3aUKl$jGi1L(Au+HmzLn71O4)G10wj6(_RbQ9rI^@*VrFGnq$?YLe zKCx(?f4Ov~*_$C8;_ZefV2>Lz)Q@+)+`}5j!D3Ib<{R|&y1fkz-2DRXsy2oc~; z$9KInT{;}1y`=^i>uaFfx_~fqZ2LY{w3kn^Vy&EqH39bCTsF%0TMKuhoPK*0Gk9~z z49K?cAcH@Ed0+fSD0;%(Mk?3%9y# z(mUa7_W5u&Hw|wmbiiRMYNZrTD16^A_B>;G9^T3Cz;42#)B=d-9Lt@BIKfr)6mq$R zwnA`$mEPu28>;pNU8h6%70AFB!p!}k@{ta4<~QvpW21g0z*?(gsbc}&`cTN;!pum6 zR>Mqj%zU^p$M2XzrZO|&C3b6id*-H{kkb{IL!Qo|uG~3pbPnqrI;6-hPO?DTpy&9N zgF4(!3o}0JFwfxHGi#_l`O}a%W2c|`mSxH;Jk3~ZJI+h*%bBe^Jc(sH{$uBV&!fcT z3r@$ry2f#4ruV<-6WyWmuxep;!*CXSEE8LI2}dV^Kz%JD*7okRJcDiQaj z@V~7(7G{&V0JEvd=}_LO5|k~Be%G2Kr}_*fHkZD>4nD=d&`aHkcp9)(y0g0@Rq8hC z4e$)C^~KMaMytMo)ZHbol$J}zmqh6(>;n%4Ui3cGIv2Yn-A#)J4N@oFO-bC_@Kj&& zjC5rSyn>DJ@qItE!22{b;Ds`oGZ4H|XEy4PoX-LWybC-pmVE+?9hnBnREZZKNqA2Y z=Aua>e}E*k#w1`8L#5#9X5rd|Cj>~qi&4jH%yDigcy#OFg9*xH{mH&*Ia7h;Qeu1g?-S_aG-VYLA1{@x#fMo;Rf&LJJ&0Tq$c#D_bQCqM5<#Wd;=rA z9Gajwl5+!G!!ZxvFzBJX{Fs9yk-R9^V65XC664xBCoxZJSHG_l_mg=LOCf3eIshM{mG)1t;6)!xK65oh!{BoxcFAf@3a- z$ZL_K(SPrW6H`NnY8F(_*vwsjbo-g+xq!L`P)E03D0~~-Sl;)S)WQ2-53OmPJ&}&- zf7*Q>?VkN;#%AXFe}x9NpMd7dEYMBA(0qG)&O~B0gfmR_xDht1Opi%rI+?jlo__jA z&HX`cuW=W@H*pv4Rl^yKX;;3V&wD@~vumFJR`@m$Yf|0dc1l5Lm|TYIf9MBlidX1I ztdyFRij;<%Ju@~R-OgM;eWDim`@n)7N4Hixx!?ggPFoRtjk~E4j0sxnw(n7S!Hzx7 zG=B=Lp}W4=4DGckpck3WEPDm+-^uYeAve`vLWVF=7v@c*hfC1y*@qp1=^;sPg)@`u z+v}ZsI%AHFn+vvIY{#NbbV#7_th#Nu&(F$>hJji0^+m&sOUem#nqG zDppb<=T+kcPWWG7xj5;4J; z?68a3myqe{e)tyJl7q9|dz|y}n{kh`-{*|IMiFW1%8L|>%-wV1?rlHkdX{rX27CL; z<#D{zZ^dbxQw&^T_ag3nNQXkj!MQybV;1Kf9%uiP8>#I%5#B{wUSj#pI^FrPirF$b z&>Z&NIVs>%SJ~}tW6^VBAO*i41e2)Uz2AoyGWF|eZ0Jmgm5|(c3F5mN)*lURv3728pK$)=ds4kenxNqpE%T-q(H@RG_B(ZzIY*k{Aj$^=+fmZ*eLduB zwm)1GOSgsq{Rjzr3b{*Vg$dZF;ER(={rM(y^^U@X2EvVg!~AAvU>(qN)>c{Xec72P zReAIM56i#EXmRpV;+b^Zkqk!?v;$t7>p*y;tC^q5*GqQS zaOCpHMR|d%Cls&qEpnejm#>!VoDsB-6E4~fbn%{R(RSh)+U_T~{j<3$&~A=ITjpR% z9@CL4F}sE%e;dL5z5HtC_0n9leJa*=SBTbn8)^9EwisPx%E>aeVVeiH^yGM#F6D%W zE#u0~Z9+?+OP5;*3(;12YwK2-$=&L?Bg%zmRU5UH+NGWaWDrt`tP_t^mCq_C~;GV>B((0xeF+Wu;QlP=-ua(}B;Ee_9 zhXe6eH)Y1pcl3+_eW_NIP8Bf?JJ7<{8IxSJrH@1LF`q0;agm0tF)A<^Kb=pie~p*o zJp7{kB1S^TQQ^HKtGsMt<+)KRT$C&RXDl%peq|(lY$T7M7ixC}HNmIm7D~@weMir& zWm%gzZ=9OdF=`IreV=)hn*UG7|Lw^CjQ=#&&HtP6|GR8YgU!CsPY9JbH2ou_^AWd> zmeKSPr0KVWc-dT<=0@Qv;$^uM?q^6--y1D5@+R-*up*DcvRvq4tlWKc-&zIPL-#8_ z#w}Xp)?wCay)Qm@^qwKV<)oL+LS`$UEx5GEOWJhkH`JpV?}Bpl7sQFB{XW=M^Bu6+ z`}5macxa#OB6;;#kHU43)97a_^)8e5%DbF{XYPP)(djXJZX5kBpl`~j+^;#CF2LKy z1Z=G|FSHeU;p-ifx`kz0x(9{sE z_u)eLxwn{>7L?h%RdPMX1CN^a_}F*&sqYltZCPiQF_QH@>Z6hG7>ewLPa>_$!^bYu zTPruf&vbX}UMEf;bBloYoFXg)7SAmn#$qkYxKE4JYK$a$srS`TWK#@hdY}A>mJSI@ zZ?8Y|(|cst?fukFZf-Fx(IQTbzDYGM_F3o}{E^C7U23|CLu++|@%b!&oiC0Hah!?5w|?7~;uGDuVLl^_`OsX4d~+hNO=H0<{Gt`(fgJ72;z5NtUJ?tTUoNGMNBc;#yJSK zMDRN94@c#NfbYi9dP3V`=vYq7KsvZ5)l=x<@pbSmDly~pz+qnWfee}TEc4u6M(6Um zx9C`PZZ4e0p6t?D06UlRwqYz?H*`)bTe5CCj?*K(S)_VZp0q+un^O;KjlKNcE@sLM zk8vjM@Fp1Tf9}%VO1(db^7l99t|`p8n4XWfoptg7_*)7Sh;kvi5&k8+ohI<{A3ZkD z8qYXSz5FxJ*v$Ia3g|1VY|E6N&-g0Fhxsj;ODep3aQeHl9*Ct%ej7E6t~mU~vivrS z#K}%IZ6V37bKc=O;o)%4kPi%=exqKllBceruta$_NLk`F(YA?xMq)^ z2^(1oZN^)~$71|P?@`j1kgKuw^{c%^c{pla^Rj#*W4m))d#cM?PQC8w%B=97S53Ze zJ@D=i61+#DvNy+jyeK|LpN7i3O}LXvb!BE?cP}v7X8Mn(8{z$a`y15y6{o8Sw$J#? z#AO%daXJ@Q%IGT#=)2yJxS?}&E{MoFgq8%nfZo`uMHJ28zQapq`2G5ldJh|}$*uQb zb@ltoGdx0DZ9ZfY;H}JEi-eX$)Tu+A|JD*v#|c~S9c39Zw6^6L@j9^>`5)BXl2h*| zmwfi)ES0RltDiuc5#mE`OAcCvm*UqNxkM^h?Ad&fw^zf526r-XpMJ6>Cm%c8G(atb z4SEl(;QO@{cb_GrjK#e^@aTSrjm7=G+&jv2vD^KJHS_{Gcmk_@@c_N4+mCwJ!>=eV ze@=o5xVMYZ!qE+{323xbhornYUeX70-HzTtONJ-z<5iVD2QT|g9kWVG`yKbDK%Uwr zX3FKzafG07qi~Hd&oFyJ#|+tu*9zf)78x4WA~&HwCw$%nCVNR~-0q#g%zid37zB*u zc52HXh7aHD5Jvhz=p8l9_p)>|EI1`9zW}~OH>~x*A45ZV?F^Zg*E^E}V}RAP+zd~W zdm_Rlgvo%)Lt7sXzXZ>7vY*zh$6G^z`7(ytyO2$f(K#f%KflwjZ8G1M_aTd4)?9`wtj-P;MukTXC`SdA`jD6ue51yVO@H>7O2Hdow?1G zMVeQ@lZ*Zhht6i|gT5?>I8xm9ma}TDV+x;ej>abuXzc1IE}AxChbip$ZM1q=moVCv zJ=PYeyRuLfm&MznK0Tz8IB1)O7RJKh6|5F{OT(MT*xx6h?;*8FFYdQ>f%sQuH}!L; zei)ShuLOg8QSYr-IpAiG1Ge{i&yvd@IFWZVc)bNEJ8?V@dU05NnN6I#~pf0_}nw8@rl>sf72Z}6FS~6`WzjVE)x{UqIAMP_zjmSg!Ws)wG`_Ho zmR(qN;htFh^HQbO`O!V4%FA7 zY4F5xm0phA2M--rsq&5MZB=v?_3lM}iYp0F1SNbvAkzVfI86Wq;wcCupL$`#=6#gX zY8gYO>}N=hdD${J>c5P=5`O~&-f91gy`qTgoOE2tG0!oEjh$Cv>8QWURMN1%E_NcW zgJkvqY~%iGtG(KvnWgo^aF83=pUdKBNX zJr#S_!YeKox3BG}XkU8)A${G1RTI`_And)nK=IwY7k{TB7UOSKU`?1)`W;&)`KbiJ z%D~^MsKwu^s1<2Ulq~}swAVYZON@>IYhk|o`jYz9;9KkmwT7Ydzz+NDSjCh)7@P;` zNgzFM_Ab^9or~=okYyS26Wq}GS&LyM>i2ChdS(Kuvn&PWPQ}C0?@mRC#k9REZJh;2 zJNu89Q_dDkF2taG8z}THX6XFHk_@^pDRdvokjJ8g;xP_?pXH=l1A7$ZG^fJDCS`<= zkb&iAM1%E!=U9p@-e$8Vl)x^6*&MZtAa%tq^4*;AuwG%zRIYMqgdrv~x%F0No}ol& zS>fei1zAyMZPLN=l-RUP-cv;7>PV#KE%;tI4r1M1fn!JE>Hrt`4(`7Hfa9;UhjZvM z@0IpE{{cr|YtVk{02lZUZpnYZnXk0B2JNl6(q8^|aO3gL;iq1{f76CL$`0uH^Qk3u zfD8cAgPge}buH?_CoCH|Q%N0i0p$Lrkn806^T{Qt_u$(-dy1Y#4m~0lK&~7)p(J%Z zzO6?+0p#L6{X|l7%e;6M(t_wvU8(lhuf%) ziOSAdqa`okJfY9V>k-^fFVghc25|&udXymk5#j{I^AQ^nUxWB6#B&knAfBZeva=9R z*G$<{5l=-t4RN+cImDEGHPV?e-5lrD5%|lh@V7!Z9rNAp zSlrpG)KA4v`6>~~ryiv;IsYh=gS#Y=dKS}@PL67Cg^W-I{F+o>QGe_W58V;s zUU0;p1Gz4==&{0c2dH=9-=p*lqg#?H&1^EFZC0eOzs>NZqTb~2fx+vmbeSlR9z|XH zwrDhe&1fDLg!!zimTI`7u9Mq@TmRAn@cc>7G`BJK-k5LOfu_?LS8ZZkI3b3$8y<7w zHC+c@+H~qRwK@4P!(R%TdU)M`!viPg|5G(iWp<8NnXi>amjWSRaa{|U?g~)thp8pD zXTv;y)c;vJ{(gx%sf5{+D&uecc+y~LYWvdeAc}JI0BrtK<+2GooQ!{hyi#5~A=~qH zCnP)sx>g58o$Gr~3F9iSHOcdh*C`bFLfPv|P=$ zyIU!b+0_}~len%;l&g+1He=xSEG|gl84>d}#i>qpP~T4?c822of;TeIH!1(n_jB?7 zQ(_o2`~U;ur#NVl?X8>yPO{OzDnn)rfcu$ZtQGKFKEBhmZZwT`L10P(}zS&F5?pwWf$-KO|vzC|LbJyyhd+%AbdIPy>y;xRRDQ@)MWhBnBRpQToDXv?6 z&t3P);?(=&CFSeU#+}|(C=fTw>+fH;N-SUf-B-Y`TD`GigSYa&^&9@ZDs8!Nd7)_E zG}WHJ>H2u|!<>;6tzVCBy&LZSW!Z++M&fwxv+yc;mmceE#Vt$oa+l0oxDaV7x30WL zqj(k;+_d=TWfiMMI*d)Ti4QQ}`=L3qFOvw5TJg6UEkt+)>CX{9ok)Zx#P1@sAhaT! zKxjiqz_-(gFX3-Bb|C&F3wH}o#+v$He}->F+Xz!(_envRh>(DA31y!m%tiX{GVeXK zvUkdrC2E1N2hMYl}Hd=fN?-eLb NW`cJ{i~c{0{tt3>xnBSP literal 0 HcmV?d00001 From 9909499a81a5b90bb0f7061150fb5d429399074a Mon Sep 17 00:00:00 2001 From: QioTek <71515778+QioTek@users.noreply.github.com> Date: Sun, 17 Apr 2022 13:46:37 +0800 Subject: [PATCH 0654/3101] AP_HAL_ChibiOS: ICM4 series sensors as an alternative with IMU3. AP_HAL_ChibiOS: ICM4 series sensors as an alternative with IMU3. --- libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat index d2470c5a1c4..dfca39149e9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/QioTekZealotH743/hwdef.dat @@ -167,6 +167,7 @@ IMU Invensensev3 SPI:imu1 ROTATION_YAW_270 IMU Invensense SPI:imu2 ROTATION_ROLL_180 IMU Invensensev3 SPI:imu2 ROTATION_YAW_270 IMU Invensense SPI:imu3 ROTATION_ROLL_180 +IMU Invensensev3 SPI:imu3 ROTATION_YAW_270 # two Baro sensors From ecd5cb21fccf70460c3cfd4d876383f3d4970f41 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 17 Apr 2022 23:01:28 +1000 Subject: [PATCH 0655/3101] autotest: tidy richenpower test using new methods --- Tools/autotest/arducopter.py | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 3253c57f06c..4babcc72b23 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -6569,19 +6569,9 @@ def test_richenpower(self): self.wait_generator_speed_and_state(0, 0, mavutil.mavlink.MAV_GENERATOR_STATUS_FLAG_OFF) - messages = [] - - def my_message_hook(mav, m): - if m.get_type() != 'STATUSTEXT': - return - messages.append(m) - self.install_message_hook(my_message_hook) - try: - self.set_rc(9, 2000) # remember this is a switch position - run - finally: - self.remove_message_hook(my_message_hook) - if "Generator HIGH" not in [x.text for x in messages]: - self.wait_statustext("Generator HIGH", timeout=60) + self.context_collect('STATUSTEXT') + self.set_rc(9, 2000) # remember this is a switch position - run + self.wait_statustext("Generator HIGH", check_context=True) self.set_rc(9, 1000) # remember this is a switch position - stop self.wait_statustext("requested state is not RUN", timeout=200) From 227e82053aecd25620776facf4d15662bba3256c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 13:31:29 +1000 Subject: [PATCH 0656/3101] AP_Periph: fixed GPS dropout on F4 and L4 GPS nodes we need more time for other threads on these nodes or we will end up dropping GPS frames --- Tools/AP_Periph/can.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 3099e8b494b..93d42785216 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -59,7 +59,7 @@ extern AP_Periph_FW periph; #if defined(STM32H7) #define HAL_PERIPH_LOOP_DELAY_US 64 #else -#define HAL_PERIPH_LOOP_DELAY_US 512 +#define HAL_PERIPH_LOOP_DELAY_US 1024 #endif #endif From 35a2ca62ee8e35889beae70068edc914def6eda4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 13:41:37 +1000 Subject: [PATCH 0657/3101] Tools: added test script for checking for Fix2 missed frames used to track down the F405 and L431 lost frames --- Tools/scripts/CAN/fix2_gap.py | 44 +++++++++++++++++++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100755 Tools/scripts/CAN/fix2_gap.py diff --git a/Tools/scripts/CAN/fix2_gap.py b/Tools/scripts/CAN/fix2_gap.py new file mode 100755 index 00000000000..2fa6011d5e3 --- /dev/null +++ b/Tools/scripts/CAN/fix2_gap.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +''' +test script to check if all CAN GPS nodes are producing Fix2 frames at the expected rate +''' + +import dronecan, time +from dronecan import uavcan + +# get command line arguments +from argparse import ArgumentParser +parser = ArgumentParser(description='Fix2 gap example') +parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate") +parser.add_argument("--node-id", default=100, type=int, help="CAN node ID") +parser.add_argument("--max-gap", default=0.25, type=int, help="max gap in seconds") +parser.add_argument("port", default=None, type=str, help="serial port or mavcan URI") +args = parser.parse_args() + +# Initializing a DroneCAN node instance. +node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate) + +# Initializing a node monitor +node_monitor = dronecan.app.node_monitor.NodeMonitor(node) + +last_fix2 = {} + +def handle_fix2(msg): + nodeid = msg.transfer.source_node_id + tstamp = msg.transfer.ts_real + if not nodeid in last_fix2: + last_fix2[nodeid] = tstamp + return + dt = tstamp - last_fix2[nodeid] + last_fix2[nodeid] = tstamp + if dt > args.max_gap: + print("Node %u gap=%.3f" % (nodeid, dt)) + +# callback for printing ESC status message to stdout in human-readable YAML format. +node.add_handler(dronecan.uavcan.equipment.gnss.Fix2, handle_fix2) + +while True: + try: + node.spin() + except Exception as ex: + print(ex) From b0c3fae6628ca99743111f0f0fa322f76531dfc9 Mon Sep 17 00:00:00 2001 From: Walter Dunckel Date: Sun, 27 Mar 2022 17:24:48 -0700 Subject: [PATCH 0658/3101] AP_HAL_ChibiOS: add support for NucleoH755 board This addition allows for cheap testing of a STM32H755 (dual core). This borrows the setup file STM32H757xx.py, as there are almost no changes between the chips. CRSF in and out, gps, ICM20948, BMP388, settings saving to on board flash all working. PWM(3) pin change Changed PWM(3) pin to one exposed on connector --- .../hwdef/NucleoH755/defaults.parm | 9 ++ .../hwdef/NucleoH755/hwdef-bl.dat | 43 +++++ .../AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef.dat | 147 ++++++++++++++++++ 3 files changed, 199 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/defaults.parm create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef-bl.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/defaults.parm new file mode 100644 index 00000000000..8dd37bd7f1a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/defaults.parm @@ -0,0 +1,9 @@ +SERIAL3_BAUD 57 +SERIAL3_PROTOCOL 5 +SERIAL3_OPTIONS 256 + +SERIAL4_BAUD 115 +SERIAL4_PROTOCOL 23 +SERIAL4_OPTIONS 0 + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef-bl.dat new file mode 100644 index 00000000000..c74391c2bda --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef-bl.dat @@ -0,0 +1,43 @@ +# hw definition file for processing by chibios_hwdef.py +# for H755 bootloader - H755 is almost identical to H757, so utilizing its STM32H757xx.py file + +# MCU class and specific type +MCU STM32H7xx STM32H757xx + +# crystal frequency +OSCILLATOR_HZ 8000000 + +define CORE_CM7 +define SMPS_PWR + +# board ID for firmware load +APJ_BOARD_ID 139 + +# the nucleo seems to have trouble with flashing the last sector? +FLASH_SIZE_KB 2048 + +# the location where the bootloader will put the firmware +# the H755 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 256 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 + +#LED setup +PB0 LED_BOOTLOADER OUTPUT LOW +PB14 LED_ACTIVITY OUTPUT LOW +define HAL_LED_ON 0 + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# Add CS pins to ensure they are high in bootloader +PA4 MPU_CS CS +PC7 BARO_CS CS + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef.dat new file mode 100644 index 00000000000..b14421a04ca --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/NucleoH755/hwdef.dat @@ -0,0 +1,147 @@ +# hw definition file for processing by chibios_hwdef.py +# for H755 - H755 is almost identical to H757, so utilizing its STM32H757xx.py file + +AUTOBUILD_TARGETS None + +# MCU class and specific type +MCU STM32H7xx STM32H757xx + +# crystal frequency +OSCILLATOR_HZ 8000000 + +STM32_ST_USE_TIMER 5 + +define CORE_CM7 +define SMPS_PWR + +# board ID for firmware load +APJ_BOARD_ID 139 + +# the nucleo seems to have trouble with flashing the last sector? +FLASH_SIZE_KB 2048 + +FLASH_BOOTLOADER_LOAD_KB 256 +FLASH_RESERVE_START_KB 256 + +define HAL_STORAGE_SIZE 32768 +STORAGE_FLASH_PAGE 14 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART2 UART7 USART1 UART4 UART5 + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# order of UARTs and suggested uses +# USART2 TELEM1 +# UART7 TELEM2 +# USART1 GPS +# UART4 RCIN/OUT Telem +# UART5 DEBUG + +# telem1 USART2 +PD3 USART2_CTS USART2 +PD4 USART2_RTS USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + +# telem2 UART7 +PF9 UART7_CTS UART7 +PF8 UART7_RTS UART7 +PF7 UART7_TX UART7 +PF6 UART7_RX UART7 + +# GPS USART1 +PB7 USART1_RX USART1 +PB6 USART1_TX USART1 + +# RCIN/OUT UART4 +PB9 UART4_TX UART4 +PB8 UART4_RX UART4 + +# debug uart5 +PB13 UART5_TX UART5 +PB12 UART5_RX UART5 + +# Now setup the pins for the microSD card, if available. +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# CAN Busses +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +# PWM Output +PB1 TIM8_CH3N TIM8 PWM(1) GPIO(50) +PA0 TIM2_CH1 TIM2 PWM(2) GPIO(51) +PE9 TIM1_CH1 TIM1 PWM(3) GPIO(52) +PA2 TIM2_CH3 TIM2 PWM(4) GPIO(53) +PA3 TIM2_CH4 TIM2 PWM(5) GPIO(54) +PD12 TIM4_CH1 TIM4 PWM(6) GPIO(55) +PD13 TIM4_CH2 TIM4 PWM(7) GPIO(56) +PD14 TIM4_CH3 TIM4 PWM(8) GPIO(57) +PD15 TIM4_CH4 TIM4 PWM(9) GPIO(58) +PE5 TIM15_CH1 TIM15 PWM(10) GPIO(59) +PE6 TIM15_CH2 TIM15 PWM(11) GPIO(60) +PA8 TIM1_CH1 TIM1 PWM(12) GPIO(61) # for WS2812 LED + +define HAL_SPI_CHECK_CLOCK_FREQ + +# sensor CS +PC7 MPU_CS CS + +# status LEDs +define HAL_LED_ON 0 +PB0 LED OUTPUT HIGH GPIO(0) #Green +PE1 LED1 OUTPUT HIGH GPIO(2) #Yellow +PB14 LED2 OUTPUT HIGH GPIO(1) #Red +define HAL_GPIO_A_LED_PIN 0 +define HAL_GPIO_B_LED_PIN 1 +define HAL_GPIO_C_LED_PIN 2 +define HAL_GPIO_LED_OFF 1 +define HAL_GPIO_LED_ON 0 + +# I2C +I2C_ORDER I2C2 +define HAL_I2C_INTERNAL_MASK 0 +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# Enable FAT filesystem support (needs a microSD defined via SDMMC). +define HAL_OS_FATFS_IO 1 +# Now some defines for logging and terrain data files. +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# barometers +BARO BMP388 I2C:0:0x77 + +# SPI1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI3 +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PB5 SPI3_MOSI SPI3 + +# SPI devices +SPIDEV icm20948 SPI3 DEVID1 MPU_CS MODE3 4*MHZ 8*MHZ + +# analog in +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC3 BATT_CURRENT_SENS ADC1 SCALE(1) + +# probe for an invensense IMU +IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 + +# compass as part of ICM20948 on newer cubes +COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 From 594160daf39cb48a193f70b4aacfebd171136732 Mon Sep 17 00:00:00 2001 From: Sanket Sharma Date: Sun, 17 Apr 2022 23:04:57 +0530 Subject: [PATCH 0659/3101] Plane: Updated highest airspeed limit when armed --- ArduPlane/Attitude.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 23230d3b37c..1cdd3a50e2d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -9,7 +9,7 @@ float Plane::calc_speed_scaler(void) { float aspeed, speed_scaler; if (ahrs.airspeed_estimate(aspeed)) { - if (aspeed > auto_state.highest_airspeed) { + if (aspeed > auto_state.highest_airspeed && hal.util->get_soft_armed()) { auto_state.highest_airspeed = aspeed; } if (aspeed > 0.0001f) { From b4059d3745ac56b876dc9719c8169df4b18c1ee2 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Thu, 31 Mar 2022 16:52:40 -0400 Subject: [PATCH 0660/3101] AC_AutoTune: use generic load gain method to save flash --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 206 +++++++-------------- libraries/AC_AutoTune/AC_AutoTune_Heli.h | 3 + 2 files changed, 75 insertions(+), 134 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 5cb31d2adb4..a9b153a5352 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -424,35 +424,13 @@ void AC_AutoTune_Heli::load_orig_gains() { attitude_control->bf_feedforward(orig_bf_feedforward); if (roll_enabled()) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_ri); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); - attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); - attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax); - attitude_control->get_angle_roll_p().kP(orig_roll_sp); - attitude_control->set_accel_roll_max_cdss(orig_roll_accel); + load_gain_set(ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); } if (pitch_enabled()) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); - attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); - attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax); - attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); - attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel); + load_gain_set(PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); } if (yaw_enabled()) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); - attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); - attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax); - attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); - attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel); + load_gain_set(YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); } } @@ -465,30 +443,14 @@ void AC_AutoTune_Heli::load_tuned_gains() attitude_control->set_accel_pitch_max_cdss(0.0f); } if (roll_enabled()) { - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().ff(tune_roll_rff); - attitude_control->get_angle_roll_p().kP(tune_roll_sp); - attitude_control->set_accel_roll_max_cdss(tune_roll_accel); + load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); } if (pitch_enabled()) { - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff); - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); - attitude_control->set_accel_pitch_max_cdss(tune_pitch_accel); + load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); } if (yaw_enabled()) { if (!is_zero(tune_yaw_rp)) { - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); - attitude_control->set_accel_yaw_max_cdss(tune_yaw_accel); + load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax); } } } @@ -501,35 +463,13 @@ void AC_AutoTune_Heli::load_intra_test_gains() // sanity check the gains attitude_control->bf_feedforward(true); if (roll_enabled()) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); - attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); - attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax); - attitude_control->get_angle_roll_p().kP(orig_roll_sp); - attitude_control->set_accel_roll_max_cdss(orig_roll_accel); + load_gain_set(ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); } if (pitch_enabled()) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); - attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); - attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax); - attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); - attitude_control->set_accel_pitch_max_cdss(orig_pitch_accel); + load_gain_set(PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); } if (yaw_enabled()) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); - attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax); - attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); - attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); - attitude_control->set_accel_yaw_max_cdss(orig_yaw_accel); + load_gain_set(YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); } } @@ -537,55 +477,81 @@ void AC_AutoTune_Heli::load_intra_test_gains() // called by control_attitude() just before it beings testing a gain (i.e. just before it twitches) void AC_AutoTune_Heli::load_test_gains() { + float rate_p, rate_i, rate_d; switch (axis) { case ROLL: if (tune_type == SP_UP) { - attitude_control->get_rate_roll_pid().kI(orig_roll_ri); + rate_i = orig_roll_ri; } else { // freeze integrator to hold trim by making i term small during rate controller tuning - attitude_control->get_rate_roll_pid().kI(0.01f * orig_roll_ri); + rate_i = 0.01f * orig_roll_ri; } if (tune_type == MAX_GAINS && !is_zero(tune_roll_rff)) { - attitude_control->get_rate_roll_pid().kP(0.0f); - attitude_control->get_rate_roll_pid().kD(0.0f); + rate_p = 0.0f; + rate_d = 0.0f; } else { - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); + rate_p = tune_roll_rp; + rate_d = tune_roll_rd; } - attitude_control->get_rate_roll_pid().ff(tune_roll_rff); - attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); - attitude_control->get_rate_roll_pid().slew_limit(0.0f); - attitude_control->get_angle_roll_p().kP(tune_roll_sp); + load_gain_set(ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, 0.0f); break; case PITCH: if (tune_type == SP_UP) { - attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); + rate_i = orig_pitch_ri; } else { // freeze integrator to hold trim by making i term small during rate controller tuning - attitude_control->get_rate_pitch_pid().kI(0.01f * orig_pitch_ri); + rate_i = 0.01f * orig_pitch_ri; } if (tune_type == MAX_GAINS && !is_zero(tune_pitch_rff)) { - attitude_control->get_rate_pitch_pid().kP(0.0f); - attitude_control->get_rate_pitch_pid().kD(0.0f); + rate_p = 0.0f; + rate_d = 0.0f; } else { - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); + rate_p = tune_pitch_rp; + rate_d = tune_pitch_rd; } - attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff); - attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); - attitude_control->get_rate_pitch_pid().slew_limit(0.0f); - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); + load_gain_set(PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, 0.0f); break; case YAW: // freeze integrator to hold trim by making i term small during rate controller tuning - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); - attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); - attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); - attitude_control->get_rate_yaw_pid().slew_limit(0.0f); - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*0.01f, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, tune_yaw_rLPF, 0.0f); + break; + } +} + +// load gains +void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax) +{ + switch (axis) { + case ROLL: + attitude_control->get_rate_roll_pid().kP(rate_p); + attitude_control->get_rate_roll_pid().kI(rate_i); + attitude_control->get_rate_roll_pid().kD(rate_d); + attitude_control->get_rate_roll_pid().ff(rate_ff); + attitude_control->get_rate_roll_pid().filt_T_hz(rate_fltt); + attitude_control->get_rate_roll_pid().slew_limit(smax); + attitude_control->get_angle_roll_p().kP(angle_p); + attitude_control->set_accel_roll_max_cdss(max_accel); + break; + case PITCH: + attitude_control->get_rate_pitch_pid().kP(rate_p); + attitude_control->get_rate_pitch_pid().kI(rate_i); + attitude_control->get_rate_pitch_pid().kD(rate_d); + attitude_control->get_rate_pitch_pid().ff(rate_ff); + attitude_control->get_rate_pitch_pid().filt_T_hz(rate_fltt); + attitude_control->get_rate_pitch_pid().slew_limit(smax); + attitude_control->get_angle_pitch_p().kP(angle_p); + attitude_control->set_accel_pitch_max_cdss(max_accel); + break; + case YAW: + attitude_control->get_rate_yaw_pid().kP(rate_p); + attitude_control->get_rate_yaw_pid().kI(rate_i); + attitude_control->get_rate_yaw_pid().kD(rate_d); + attitude_control->get_rate_yaw_pid().ff(rate_ff); + attitude_control->get_rate_yaw_pid().filt_T_hz(rate_fltt); + attitude_control->get_rate_yaw_pid().slew_limit(smax); + attitude_control->get_rate_yaw_pid().filt_E_hz(rate_flte); + attitude_control->get_angle_yaw_p().kP(angle_p); + attitude_control->set_accel_yaw_max_cdss(max_accel); break; } } @@ -607,22 +573,13 @@ void AC_AutoTune_Heli::save_tuning_gains() // sanity check the rate P values if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) { - // rate roll gains - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().ff(tune_roll_rff); - attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt); - attitude_control->get_rate_roll_pid().slew_limit(orig_roll_smax); + load_gain_set(ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel, orig_roll_fltt, 0.0f, orig_roll_smax); + // save rate roll gains attitude_control->get_rate_roll_pid().save_gains(); - // stabilize roll - attitude_control->get_angle_roll_p().kP(tune_roll_sp); + // save stabilize roll attitude_control->get_angle_roll_p().save_gains(); - // acceleration roll - attitude_control->save_accel_roll_max_cdss(tune_roll_accel); - // resave pids to originals in case the autotune is run again orig_roll_rp = attitude_control->get_rate_roll_pid().kP(); orig_roll_ri = attitude_control->get_rate_roll_pid().kI(); @@ -633,22 +590,13 @@ void AC_AutoTune_Heli::save_tuning_gains() } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) { - // rate pitch gains - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(tune_pitch_rff); - attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt); - attitude_control->get_rate_pitch_pid().slew_limit(orig_pitch_smax); + load_gain_set(PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel, orig_pitch_fltt, 0.0f, orig_pitch_smax); + // save rate pitch gains attitude_control->get_rate_pitch_pid().save_gains(); - // stabilize pitch - attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); + // save stabilize pitch attitude_control->get_angle_pitch_p().save_gains(); - // acceleration pitch - attitude_control->save_accel_pitch_max_cdss(tune_pitch_accel); - // resave pids to originals in case the autotune is run again orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP(); orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI(); @@ -659,23 +607,13 @@ void AC_AutoTune_Heli::save_tuning_gains() } if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) { - // rate yaw gains - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(tune_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(tune_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt); - attitude_control->get_rate_yaw_pid().slew_limit(orig_yaw_smax); - attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); + load_gain_set(YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax); + // save rate yaw gains attitude_control->get_rate_yaw_pid().save_gains(); - // stabilize yaw - attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); + // save stabilize yaw attitude_control->get_angle_yaw_p().save_gains(); - // acceleration yaw - attitude_control->save_accel_yaw_max_cdss(tune_yaw_accel); - // resave pids to originals in case the autotune is run again orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP(); orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI(); diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 72a18f16876..07c791afe5f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -42,6 +42,9 @@ class AC_AutoTune_Heli : public AC_AutoTune // backup original gains and prepare for start of tuning void backup_gains_and_initialise() override; + // load gains + void load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel, float rate_fltt, float rate_flte, float smax); + // switch to use original gains void load_orig_gains() override; From 8e35fd26584d912d0daadf8c00e11f44530edea5 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Sat, 9 Apr 2022 23:17:28 -0400 Subject: [PATCH 0661/3101] AC_AutoTune: use failed state to exit --- libraries/AC_AutoTune/AC_AutoTune.h | 18 +++++++++--------- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 15 ++++++++------- 2 files changed, 17 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index 6cc7ac16ac0..c6e0a3d537f 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -231,6 +231,15 @@ class AC_AutoTune }; void load_gains(enum GainType gain_type); + // autotune modes (high level states) + enum TuneMode { + UNINITIALISED = 0, // autotune has never been run + TUNING = 1, // autotune is testing gains + SUCCESS = 2, // tuning has completed, user is flight testing the new gains + FAILED = 3, // tuning has failed, user is flying on original gains + }; + TuneMode mode; // see TuneMode for what modes are allowed + // copies of object pointers to make code a bit clearer AC_AttitudeControl *attitude_control; AC_PosControl *pos_control; @@ -301,15 +310,6 @@ class AC_AutoTune // returns true if vehicle is close to level bool currently_level(); - // autotune modes (high level states) - enum TuneMode { - UNINITIALISED = 0, // autotune has never been run - TUNING = 1, // autotune is testing gains - SUCCESS = 2, // tuning has completed, user is flight testing the new gains - FAILED = 3, // tuning has failed, user is flying on original gains - }; - - TuneMode mode; // see TuneMode for what modes are allowed bool pilot_override; // true = pilot is overriding controls so we suspend tuning temporarily bool use_poshold; // true = enable position hold bool have_position; // true = start_position is value diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index a9b153a5352..8ba89d7685d 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -217,7 +217,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f) || ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq))){ - load_gains(GAIN_INTRA_TEST); + load_gains(GAIN_ORIGINAL); attitude_control->use_sqrt_controller(true); @@ -227,14 +227,15 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true); if ((tune_type == RP_UP && max_rate_p.max_allowed <= 0.0f) || (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f)) { - gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gains Failed, Skip Rate P/D Tuning"); - counter = AUTOTUNE_SUCCESS_COUNT; - step = UPDATE_GAINS; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); + mode = FAILED; + AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED); + update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){ gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range"); - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); - counter = AUTOTUNE_SUCCESS_COUNT; - step = UPDATE_GAINS; + mode = FAILED; + AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED); + update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if (tune_type == TUNE_COMPLETE) { counter = AUTOTUNE_SUCCESS_COUNT; step = UPDATE_GAINS; From b8690ba109531055fb235b0e7f20fefaabe61962 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Wed, 13 Apr 2022 23:41:58 -0400 Subject: [PATCH 0662/3101] AC_AutoTune: fix gain determination fail logic --- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 8ba89d7685d..a6c193971dd 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -213,8 +213,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) { // if tune complete or beyond frequency range or no max allowed gains then exit testing if (tune_type == TUNE_COMPLETE || - (tune_type == RP_UP && max_rate_p.max_allowed <= 0.0f) || - (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f) || + ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) || ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq))){ load_gains(GAIN_ORIGINAL); @@ -226,7 +225,7 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) // hold level attitude attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true); - if ((tune_type == RP_UP && max_rate_p.max_allowed <= 0.0f) || (tune_type == RD_UP && max_rate_d.max_allowed <= 0.0f)) { + if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); mode = FAILED; AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED); From 55517450bc35c12f57a85a1611defafc67fc89a6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 18 Apr 2022 12:39:56 +1000 Subject: [PATCH 0663/3101] AP_Scripting: correct script restart send_text message --- libraries/AP_Scripting/AP_Scripting.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 340dc60f162..e27c5ae8128 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -247,7 +247,7 @@ void AP_Scripting::thread(void) { } // must be enabled to get this far if (cleared || _restart) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Scripting restated"); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Scripting restarted"); break; } if ((_debug_options.get() & uint8_t(lua_scripts::DebugLevel::NO_SCRIPTS_TO_RUN)) != 0) { From 9d73cfb28f7a8a7177099a944e534f9c1c63fb56 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 16 Apr 2022 10:11:32 +0900 Subject: [PATCH 0664/3101] AP_NavEKF: GSF logging in deg from 0 to 360 --- libraries/AP_NavEKF/EKFGSF_Logging.cpp | 14 +++++++------- libraries/AP_NavEKF/LogStructure.h | 16 ++++++++-------- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AP_NavEKF/EKFGSF_Logging.cpp b/libraries/AP_NavEKF/EKFGSF_Logging.cpp index 31cc095d929..64673085ce9 100644 --- a/libraries/AP_NavEKF/EKFGSF_Logging.cpp +++ b/libraries/AP_NavEKF/EKFGSF_Logging.cpp @@ -16,13 +16,13 @@ void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, u LOG_PACKET_HEADER_INIT(id0), time_us : time_us, core : core_index, - yaw_composite : GSF.yaw, - yaw_composite_variance : sqrtF(MAX(GSF.yaw_variance, 0.0f)), - yaw0 : EKF[0].X[2], - yaw1 : EKF[1].X[2], - yaw2 : EKF[2].X[2], - yaw3 : EKF[3].X[2], - yaw4 : EKF[4].X[2], + yaw_composite : wrap_360(degrees(GSF.yaw)), + yaw_composite_variance : sqrtF(MAX(degrees(GSF.yaw_variance), 0.0f)), + yaw0 : wrap_360(degrees(EKF[0].X[2])), + yaw1 : wrap_360(degrees(EKF[1].X[2])), + yaw2 : wrap_360(degrees(EKF[2].X[2])), + yaw3 : wrap_360(degrees(EKF[3].X[2])), + yaw4 : wrap_360(degrees(EKF[4].X[2])), wgt0 : GSF.weights[0], wgt1 : GSF.weights[1], wgt2 : GSF.weights[2], diff --git a/libraries/AP_NavEKF/LogStructure.h b/libraries/AP_NavEKF/LogStructure.h index 85beb5e9791..119ea0f9ead 100644 --- a/libraries/AP_NavEKF/LogStructure.h +++ b/libraries/AP_NavEKF/LogStructure.h @@ -6,13 +6,13 @@ // @Description: EKF Yaw Estimator States // @Field: TimeUS: Time since system startup // @Field: C: EKF core this data is for -// @Field: YC: GSF yaw estimate (rad) -// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (rad) -// @Field: Y0: Yaw estimate from individual EKF filter 0 (rad) -// @Field: Y1: Yaw estimate from individual EKF filter 1 (rad) -// @Field: Y2: Yaw estimate from individual EKF filter 2 (rad) -// @Field: Y3: Yaw estimate from individual EKF filter 3 (rad) -// @Field: Y4: Yaw estimate from individual EKF filter 4 (rad) +// @Field: YC: GSF yaw estimate (deg) +// @Field: YCS: GSF yaw estimate 1-Sigma uncertainty (deg) +// @Field: Y0: Yaw estimate from individual EKF filter 0 (deg) +// @Field: Y1: Yaw estimate from individual EKF filter 1 (deg) +// @Field: Y2: Yaw estimate from individual EKF filter 2 (deg) +// @Field: Y3: Yaw estimate from individual EKF filter 3 (deg) +// @Field: Y4: Yaw estimate from individual EKF filter 4 (deg) // @Field: W0: Weighting applied to yaw estimate from individual EKF filter 0 // @Field: W1: Weighting applied to yaw estimate from individual EKF filter 1 // @Field: W2: Weighting applied to yaw estimate from individual EKF filter 2 @@ -69,7 +69,7 @@ struct PACKED log_KY1 { #define KY0_FMT "QBffffffffffff" #define KY0_LABELS "TimeUS,C,YC,YCS,Y0,Y1,Y2,Y3,Y4,W0,W1,W2,W3,W4" -#define KY0_UNITS "s#rrrrrrr-----" +#define KY0_UNITS "s#hdhhhhh-----" #define KY0_MULTS "F-000000000000" #define KY1_FMT "QBffffffffff" From 7cc2382a78471a2c6271b47c7fe863e4e3f11ede Mon Sep 17 00:00:00 2001 From: Joshua Henderson Date: Fri, 15 Apr 2022 19:59:48 -0400 Subject: [PATCH 0665/3101] AP_AHRS_DCM: NFC comment on check_matrix() normalization reset --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 7ffd3c75456..6f447fba80f 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -265,7 +265,10 @@ AP_AHRS_DCM::check_matrix(void) normalize(); if (_dcm_matrix.is_nan() || - fabsf(_dcm_matrix.c.x) > 10) { + fabsf(_dcm_matrix.c.x) > 10.0) { + // See Issue #20284: regarding the selection of 10.0 for DCM reset + // This won't be lowered without evidence of an issue or mathematical proof & testing of a lower bound + // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", From 622ad2a088edb689416d95949cd04dbdfc2d22eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:17 +1000 Subject: [PATCH 0666/3101] AP_GyroFFT: support two full harmonic notch filters --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index eb3272f021a..201672e1ca7 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -248,10 +248,16 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) return; } - const uint8_t harmonics = _ins->get_gyro_harmonic_notch_harmonics(); + // get the set of harmonics across all harmonic notch filters + uint8_t harmonics = 0; + uint8_t num_notches = 0; + for (uint8_t i=0; iget_gyro_harmonic_notch_harmonics(i); + num_notches = MAX(num_notches, _ins->get_num_gyro_dynamic_notches(i)); + } // count the number of active harmonics or dynamic notchs _tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics), - _ins->get_num_gyro_dynamic_notches()), 1, FrequencyPeak::MAX_TRACKED_PEAKS); + num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS); // calculate harmonic multiplier. this assumes the harmonics configured on the // harmonic notch reflect the multiples of the fundamental harmonic that should be tracked @@ -374,7 +380,11 @@ void AP_GyroFFT::update() if (!_rpy_health.x && !_rpy_health.y) { _health = 0; } else { - _health = MIN(_global_state._health, _ins->get_num_gyro_dynamic_notches()); + uint8_t num_notches = 0; + for (uint8_t i=0; iget_num_gyro_dynamic_notches(i)); + } + _health = MIN(_global_state._health, num_notches); } } From e67d9b41795690fde7ddaa287259586815d5be53 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH 0667/3101] AP_InertialSensor: support two full harmonic notch filters --- .../AP_InertialSensor/AP_InertialSensor.cpp | 89 ++++++++++--------- .../AP_InertialSensor/AP_InertialSensor.h | 43 +++++---- .../AP_InertialSensor_Backend.cpp | 75 +++++++--------- .../AP_InertialSensor_Backend.h | 32 ++----- .../AP_InertialSensor_Logging.cpp | 18 +++- 5 files changed, 124 insertions(+), 133 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f368cf56f4a..fb9232a2298 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -548,11 +548,13 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Group: HNTCH_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 // @Group: HNTC2_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_notch_filter, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), +#endif // @Param: GYRO_RATE // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled @@ -874,45 +876,44 @@ AP_InertialSensor::init(uint16_t loop_rate) // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value - _calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz(); - _num_calculated_harmonic_notch_frequencies = 1; - _num_dynamic_harmonic_notches = 1; - uint8_t num_filters = 0; - const bool double_notch = _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch); + bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {}; + for (uint8_t i=0; iget_motor_mask()); + if (motors != nullptr) { + _num_dynamic_harmonic_notches[i] = __builtin_popcount(motors->get_motor_mask()); + } } // avoid harmonics unless actually configured by the user - _harmonic_notch_filter.set_default_harmonics(1); - } + _harmonic_notch_filter[i].set_default_harmonics(1); + } #endif + } // count number of used sensors uint8_t sensors_used = 0; for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { sensors_used += _use[i]; } - // calculate number of notches we might want to use for harmonic notch - if (_harmonic_notch_filter.enabled()) { - num_filters = __builtin_popcount( _harmonic_notch_filter.harmonics()) - * _num_dynamic_harmonic_notches * (double_notch ? 2 : 1) - * sensors_used; - } - // add filters used by static notch - if (_notch_filter.enabled()) { - _notch_filter.set_default_harmonics(1); - num_filters += __builtin_popcount(_notch_filter.harmonics()) - * (_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch) ? 2 : 1) - * sensors_used; + for (uint8_t i=0; i HAL_HNF_MAX_FILTERS) { @@ -923,18 +924,14 @@ AP_InertialSensor::init(uint16_t loop_rate) for (uint8_t i=0; i= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { + return; + } // protect against zero as the scaled frequency if (is_positive(scaled_freq)) { - _calculated_harmonic_notch_freq_hz[0] = scaled_freq; + _calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq; } - _num_calculated_harmonic_notch_frequencies = 1; + _num_calculated_harmonic_notch_frequencies[idx] = 1; } // Update the harmonic notch frequency -void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { +void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]) { + if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { + return; + } // protect against zero as the scaled frequency for (uint8_t i = 0; i < num_freqs; i++) { if (is_positive(scaled_freq[i])) { - _calculated_harmonic_notch_freq_hz[i] = scaled_freq[i]; + _calculated_harmonic_notch_freq_hz[idx][i] = scaled_freq[i]; } } // any uncalculated frequencies will float at the previous value or the initialized freq if none - _num_calculated_harmonic_notch_frequencies = num_freqs; + _num_calculated_harmonic_notch_frequencies[idx] = num_freqs; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index d9d90d955e9..dc408c7ff61 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -35,6 +35,9 @@ #define HAL_INS_TEMPERATURE_CAL_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024 #endif +#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS +#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2 +#endif #include @@ -240,9 +243,9 @@ class AP_InertialSensor : AP_AccelCal_Client uint8_t get_primary_gyro(void) const { return _primary_gyro; } // Update the harmonic notch frequency - void update_harmonic_notch_freq_hz(float scaled_freq); + void update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq); // Update the harmonic notch frequencies - void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]); + void update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]); // get the gyro filter rate in Hz uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; } @@ -251,32 +254,32 @@ class AP_InertialSensor : AP_AccelCal_Client uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; } // harmonic notch current center frequency - float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz[0]; } + float get_gyro_dynamic_notch_center_freq_hz(uint8_t idx) const { return idx 1) { - _imu._gyro_harmonic_notch_filter[instance].update(num_gyro_harmonic_notch_center_frequencies(), gyro_harmonic_notch_center_frequencies_hz()); - } else { - _imu._gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz()); + for (uint8_t i=0; i 1) { + _imu._gyro_harmonic_notch_filter[i][instance].update(num_gyro_harmonic_notch_center_frequencies(i), gyro_harmonic_notch_center_frequencies_hz(i)); + } else { + _imu._gyro_harmonic_notch_filter[i][instance].update(gyro_harmonic_notch_center_freq_hz(i)); + } + _last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i); } - _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz(); - } - // possily update the notch filter parameters - if (!is_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) || - !is_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) || - !is_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) || - sensors_converging()) { - _imu._gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB()); - _last_notch_center_freq_hz = _gyro_notch_center_freq_hz(); - _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz(); - _last_notch_attenuation_dB = _gyro_notch_attenuation_dB(); } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index db5d226269f..2e54ded740c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -257,33 +257,22 @@ class AP_InertialSensor_Backend return (uint16_t)_imu._loop_rate; } - // return the notch filter center in Hz for the sample rate - float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); } - - // return the notch filter bandwidth in Hz for the sample rate - float _gyro_notch_bandwidth_hz(void) const { return _imu._notch_filter.bandwidth_hz(); } - - // return the notch filter attenuation in dB for the sample rate - float _gyro_notch_attenuation_dB(void) const { return _imu._notch_filter.attenuation_dB(); } - - bool _gyro_notch_enabled(void) const { return _imu._notch_filter.enabled(); } - // return the harmonic notch filter center in Hz for the sample rate - float gyro_harmonic_notch_center_freq_hz() const { return _imu.get_gyro_dynamic_notch_center_freq_hz(); } + float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH 0668/3101] AP_RPM: support two full harmonic notch filters --- libraries/AP_RPM/RPM_HarmonicNotch.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.cpp b/libraries/AP_RPM/RPM_HarmonicNotch.cpp index 883c45eb570..677ef01baee 100644 --- a/libraries/AP_RPM/RPM_HarmonicNotch.cpp +++ b/libraries/AP_RPM/RPM_HarmonicNotch.cpp @@ -32,11 +32,14 @@ AP_RPM_HarmonicNotch::AP_RPM_HarmonicNotch(AP_RPM &_ap_rpm, uint8_t _instance, A void AP_RPM_HarmonicNotch::update(void) { AP_InertialSensor& ins = AP::ins(); - if (ins.get_gyro_harmonic_notch_tracking_mode() != HarmonicNotchDynamicMode::Fixed) { - state.rate_rpm = ins.get_gyro_dynamic_notch_center_freq_hz() * 60.0f; - state.rate_rpm *= ap_rpm._params[state.instance].scaling; - state.signal_quality = 0.5f; - state.last_reading_ms = AP_HAL::millis(); + for (uint8_t i=0; i Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH 0669/3101] AP_Vehicle: support two full harmonic notch filters --- libraries/AP_Vehicle/AP_Vehicle.cpp | 21 +++++++++++---------- libraries/AP_Vehicle/AP_Vehicle.h | 4 ++-- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index d75d7f36e7d..07910f49af8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -398,17 +398,18 @@ bool AP_Vehicle::is_crashed() const // run notch update at either loop rate or 200Hz void AP_Vehicle::update_dynamic_notch_at_specified_rate() { - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate)) { - update_dynamic_notch(); - return; - } - - // decimated update at 200Hz - const uint32_t now = AP_HAL::millis(); + for (uint8_t i=0; i 5) { - _last_notch_update_ms = now; - update_dynamic_notch(); + if (now - _last_notch_update_ms[i] > 5) { + _last_notch_update_ms[i] = now; + update_dynamic_notch(i); + } + } } } diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 7fc49a93c3e..2652baf5c25 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -249,7 +249,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { #endif // AP_SCRIPTING_ENABLED // update the harmonic notch - virtual void update_dynamic_notch() {}; + virtual void update_dynamic_notch(uint8_t idx) {}; // zeroing the RC outputs can prevent unwanted motor movement: virtual bool should_zero_rc_outputs_on_reboot() const { return false; } @@ -412,7 +412,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { bool likely_flying; // true if vehicle is probably flying uint32_t _last_flying_ms; // time when likely_flying last went true - uint32_t _last_notch_update_ms; // last time update_dynamic_notch() was run + uint32_t _last_notch_update_ms[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; // last time update_dynamic_notch() was run static AP_Vehicle *_singleton; From 8d187f4bc64e1121630de566fc71a5e6095e2e9a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH 0670/3101] Copter: support two full harmonic notch filters --- ArduCopter/Copter.h | 2 +- ArduCopter/system.cpp | 38 +++++++++++++++++++------------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5bbdd75a52e..5d1960da143 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -879,7 +879,7 @@ class Copter : public AP_Vehicle { // system.cpp void init_ardupilot() override; void startup_INS_ground(); - void update_dynamic_notch() override; + void update_dynamic_notch(uint8_t idx) override; bool position_ok() const; bool ekf_has_absolute_position() const; bool ekf_has_relative_position() const; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 8375c440093..263a93026d5 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -234,24 +234,24 @@ void Copter::startup_INS_ground() } // update the harmonic notch filter center frequency dynamically -void Copter::update_dynamic_notch() +void Copter::update_dynamic_notch(uint8_t idx) { - if (!ins.gyro_harmonic_notch_enabled()) { + if (!ins.gyro_harmonic_notch_enabled(idx)) { return; } - const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); - const float ref = ins.get_gyro_harmonic_notch_reference(); + const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx); + const float ref = ins.get_gyro_harmonic_notch_reference(idx); if (is_zero(ref)) { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); return; } const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)); - switch (ins.get_gyro_harmonic_notch_tracking_mode()) { + switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle - ins.update_harmonic_notch_freq_hz(throttle_freq); + ins.update_harmonic_notch_freq_hz(idx, throttle_freq); break; #if RPM_ENABLED == ENABLED @@ -259,48 +259,48 @@ void Copter::update_dynamic_notch() float rpm; if (rpm_sensor.get_rpm(0, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref / 60.0f)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref / 60.0f)); } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; #endif #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); for (uint8_t i = 0; i < num_notches; i++) { notches[i] = MAX(ref_freq, notches[i]); } if (num_notches > 0) { - ins.update_harmonic_notch_frequencies_hz(num_notches, notches); + ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches); } else { // throttle fallback - ins.update_harmonic_notch_freq_hz(throttle_freq); + ins.update_harmonic_notch_freq_hz(idx, throttle_freq); } } else { - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); } break; #endif #if HAL_GYROFFT_ENABLED case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - ins.update_harmonic_notch_frequencies_hz(peaks, notches); + ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches); } else { - ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); + ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz()); } break; #endif case HarmonicNotchDynamicMode::Fixed: // static default: - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); break; } } From c1d841dd934f3d4151edbb85df0201b0d9f02fd2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:36:18 +1000 Subject: [PATCH 0671/3101] Plane: support two full harmonic notch filters --- ArduPlane/Plane.h | 2 +- ArduPlane/system.cpp | 40 ++++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 05905a4f469..7f76ace2c87 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1054,7 +1054,7 @@ class Plane : public AP_Vehicle { void startup_INS_ground(void); bool should_log(uint32_t mask); int8_t throttle_percentage(void); - void update_dynamic_notch() override; + void update_dynamic_notch(uint8_t idx) override; void notify_mode(const Mode& mode); // takeoff.cpp diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 1c4570e3c06..9bf33a7320f 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -447,25 +447,25 @@ int8_t Plane::throttle_percentage(void) } // update the harmonic notch filter center frequency dynamically -void Plane::update_dynamic_notch() +void Plane::update_dynamic_notch(uint8_t idx) { - if (!ins.gyro_harmonic_notch_enabled()) { + if (!ins.gyro_harmonic_notch_enabled(idx)) { return; } - const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(); - const float ref = ins.get_gyro_harmonic_notch_reference(); + const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx); + const float ref = ins.get_gyro_harmonic_notch_reference(idx); if (is_zero(ref)) { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); return; } - switch (ins.get_gyro_harmonic_notch_tracking_mode()) { + switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle #if HAL_QUADPLANE_ENABLED if (quadplane.available()) { - ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); + ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); } #endif break; @@ -474,51 +474,51 @@ void Plane::update_dynamic_notch() float rpm; if (rpm_sensor.get_rpm(0, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm * ref * (1/60.0))); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0))); } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); for (uint8_t i = 0; i < num_notches; i++) { notches[i] = MAX(ref_freq, notches[i]); } if (num_notches > 0) { - ins.update_harmonic_notch_frequencies_hz(num_notches, notches); + ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches); #if HAL_QUADPLANE_ENABLED } else if (quadplane.available()) { // throttle fallback - ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); + ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); #endif } else { - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); } } else { - ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); } break; #endif #if HAL_GYROFFT_ENABLED case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { float notches[INS_MAX_NOTCHES]; - const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(), notches); + const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - ins.update_harmonic_notch_frequencies_hz(peaks, notches); + ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches); } else { - ins.update_harmonic_notch_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); + ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz()); } break; #endif case HarmonicNotchDynamicMode::Fixed: // static default: - ins.update_harmonic_notch_freq_hz(ref_freq); + ins.update_harmonic_notch_freq_hz(idx, ref_freq); break; } } From c03436b5e4802bd54e420b79d09d5a853fcbeb48 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:44:27 +1000 Subject: [PATCH 0672/3101] Copter: update for changed INS_NOTCH parameter name --- ArduCopter/Parameters.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 841e2b081f9..9bd31301a29 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1387,7 +1387,8 @@ void Copter::convert_pid_parameters(void) AP_Param::convert_old_parameters(&ff_and_filt_conversion_info[i], 1.0f); } - if (!ins.gyro_notch_enabled()) { +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 + if (!ins.gyro_harmonic_notch_enabled(1)) { // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch const AP_Param::ConversionInfo notchfilt_conversion_info[] { { Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" }, @@ -1399,11 +1400,12 @@ void Copter::convert_pid_parameters(void) for (uint8_t i=0; i Date: Wed, 13 Apr 2022 13:51:56 +1000 Subject: [PATCH 0673/3101] Filter: added RPM2 harmonic notch type --- libraries/Filter/HarmonicNotchFilter.cpp | 2 +- libraries/Filter/HarmonicNotchFilter.h | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 6dc93d5e7d8..f20b218e14f 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -78,7 +78,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @DisplayName: Harmonic Notch Filter dynamic frequency tracking mode // @Description: Harmonic Notch Filter dynamic frequency tracking mode. Dynamic updates can be throttle, RPM sensor, ESC telemetry or dynamic FFT based. Throttle-based updates should only be used with multicopters. // @Range: 0 4 - // @Values: 0:Disabled,1:Throttle,2:RPM Sensor,3:ESC Telemetry,4:Dynamic FFT + // @Values: 0:Disabled,1:Throttle,2:RPM Sensor,3:ESC Telemetry,4:Dynamic FFT,5:Second RPM Sensor // @User: Advanced AP_GROUPINFO("MODE", 7, HarmonicNotchFilterParams, _tracking_mode, 1), diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 4cfd18e48fd..636d0fcc952 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -73,6 +73,7 @@ enum class HarmonicNotchDynamicMode { UpdateRPM = 2, UpdateBLHeli = 3, UpdateGyroFFT = 4, + UpdateRPM2 = 5, }; /* From 13107dfa6dee4589bbfa8ff2df6891d39b71fd2e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:52:42 +1000 Subject: [PATCH 0674/3101] Copter: support harmonic notch on 2nd RPM sensor --- ArduCopter/system.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 263a93026d5..36df04162d3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -256,14 +256,17 @@ void Copter::update_dynamic_notch(uint8_t idx) #if RPM_ENABLED == ENABLED case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking + case HarmonicNotchDynamicMode::UpdateRPM2: { + uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1); float rpm; - if (rpm_sensor.get_rpm(0, rpm)) { + if (rpm_sensor.get_rpm(sensor, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref / 60.0f)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60))); } else { ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; + } #endif #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking From d57abb1230e88e263737e69acdb9b46485be3587 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:52:42 +1000 Subject: [PATCH 0675/3101] Plane: support harmonic notch on 2nd RPM sensor --- ArduPlane/system.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 9bf33a7320f..5be4f6db9ba 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -471,14 +471,17 @@ void Plane::update_dynamic_notch(uint8_t idx) break; case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking + case HarmonicNotchDynamicMode::UpdateRPM2: { + uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1); float rpm; - if (rpm_sensor.get_rpm(0, rpm)) { + if (rpm_sensor.get_rpm(sensor, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0))); } else { ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; + } #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking // set the harmonic notch filter frequency scaled on measured frequency From 1cd1519a4f770218567642c3cc5d66c56bb41659 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:09:28 +1000 Subject: [PATCH 0676/3101] Filter: clarify meaning of 1st harmonic --- libraries/Filter/HarmonicNotchFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index f20b218e14f..661e0628923 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -60,7 +60,7 @@ const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { // @Param: HMNCS // @DisplayName: Harmonic Notch Filter harmonics - // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. + // @Description: Bitmask of harmonic frequencies to apply Harmonic Notch Filter to. This option takes effect on the next reboot. A value of 0 disables this filter. The first harmonic refers to the base frequency. // @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic // @User: Advanced // @RebootRequired: True From 7aafd5cf9869884efa353915ae180b9abc0878bb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:10:37 +1000 Subject: [PATCH 0677/3101] Plane: param conversion for INS_NOTCH to INS_HNTC2 --- ArduPlane/Parameters.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 76fbda72e7b..484b40d82f6 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1502,7 +1502,8 @@ void Plane::load_parameters(void) } #endif - if (!ins.gyro_notch_enabled()) { +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 + if (!ins.gyro_harmonic_notch_enabled(1)) { // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch const AP_Param::ConversionInfo notchfilt_conversion_info[] { { Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" }, @@ -1514,11 +1515,12 @@ void Plane::load_parameters(void) for (uint8_t i=0; iprintf("load_all took %uus\n", (unsigned)(micros() - before)); } From ae1e9e06c184ec3916fdaf78ef04228fa1f12b32 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:16:30 +1000 Subject: [PATCH 0678/3101] AP_Arming: added arming check for conflicting notch modes --- libraries/AP_Arming/AP_Arming.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index d8baeead159..73a048e4e46 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -394,6 +394,14 @@ bool AP_Arming::ins_checks(bool report) check_failed(ARMING_CHECK_INS, report, "%s", failure_msg); return false; } +#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 + if (ins.gyro_harmonic_notch_enabled(0) && + ins.gyro_harmonic_notch_enabled(1) && + ins.get_gyro_harmonic_notch_tracking_mode(0) != HarmonicNotchDynamicMode::Fixed && + ins.get_gyro_harmonic_notch_tracking_mode(0) != ins.get_gyro_harmonic_notch_tracking_mode(1)) { + check_failed(ARMING_CHECK_INS, report, "conflicting notch filters"); + } +#endif } #if HAL_GYROFFT_ENABLED From 782e4887bc093cb27c7f67147614c2cc28462e6e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:21:51 +1000 Subject: [PATCH 0679/3101] AP_GyroFFT: only allow one harmonic notch filter to be linked to FFT --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 201672e1ca7..28b57e63f9c 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -248,12 +248,16 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) return; } - // get the set of harmonics across all harmonic notch filters + // check for harmonics across all harmonic notch filters + // note that we only allow one harmonic notch filter linked to the FFT code uint8_t harmonics = 0; uint8_t num_notches = 0; for (uint8_t i=0; iget_gyro_harmonic_notch_harmonics(i); - num_notches = MAX(num_notches, _ins->get_num_gyro_dynamic_notches(i)); + if (_ins->get_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) { + harmonics = _ins->get_gyro_harmonic_notch_harmonics(i); + num_notches = _ins->get_num_gyro_dynamic_notches(i); + break; + } } // count the number of active harmonics or dynamic notchs _tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics), From d7d04bc7cfdee6984f1e01657f90428a5f4f9d75 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 20:48:18 +1000 Subject: [PATCH 0680/3101] Filter: removed parameters from the old notch filter saves some flash space --- libraries/Filter/NotchFilter.cpp | 47 -------------------------------- libraries/Filter/NotchFilter.h | 3 -- 2 files changed, 50 deletions(-) diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index fb586d17db5..ea818ec68b3 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -100,54 +100,7 @@ void NotchFilter::reset() signal2 = signal1 = T(); } -// table of user settable parameters -const AP_Param::GroupInfo NotchFilterParams::var_info[] = { - - // @Param: ENABLE - // @DisplayName: Enable - // @Description: Enable notch filter - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE), - - // Slots 2 and 3 are reserved - they were integer versions of FREQ and BW which have since been converted to float - - // @Param: ATT - // @DisplayName: Attenuation - // @Description: Notch attenuation in dB - // @Range: 5 30 - // @Units: dB - // @User: Advanced - AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15), - - // @Param: FREQ - // @DisplayName: Frequency - // @Description: Notch center frequency in Hz - // @Range: 10 400 - // @Units: Hz - // @User: Advanced - AP_GROUPINFO("FREQ", 5, NotchFilterParams, _center_freq_hz, 80), - - // @Param: BW - // @DisplayName: Bandwidth - // @Description: Notch bandwidth in Hz - // @Range: 5 100 - // @Units: Hz - // @User: Advanced - AP_GROUPINFO("BW", 6, NotchFilterParams, _bandwidth_hz, 20), - - AP_GROUPEND -}; - /* - a notch filter with enable and filter parameters - constructor - */ -NotchFilterParams::NotchFilterParams(void) -{ - AP_Param::setup_object_defaults(this, var_info); -} - -/* instantiate template classes */ template class NotchFilter; diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 290210a630b..632f738d855 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -50,9 +50,6 @@ class NotchFilter { */ class NotchFilterParams { public: - NotchFilterParams(void); - static const struct AP_Param::GroupInfo var_info[]; - float center_freq_hz(void) const { return _center_freq_hz; } float bandwidth_hz(void) const { return _bandwidth_hz; } float attenuation_dB(void) const { return _attenuation_dB; } From e2e1e74da5e570a4fcc65fbf68e2b0a482c2cedb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:38:56 +1000 Subject: [PATCH 0681/3101] AP_InertialSensor: switch to HarmonicNotch class this makes the logic much easier to follow, without indexes into arrays --- .../AP_InertialSensor/AP_InertialSensor.cpp | 99 ++++++++++++------- .../AP_InertialSensor/AP_InertialSensor.h | 87 +++++++--------- .../AP_InertialSensor_Backend.cpp | 42 ++------ .../AP_InertialSensor_Backend.h | 24 +---- .../AP_InertialSensor_Logging.cpp | 12 +-- 5 files changed, 117 insertions(+), 147 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index fb9232a2298..affac1b8464 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -548,12 +548,12 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Group: HNTCH_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter[0], "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(harmonic_notches[0].params, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams), #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 // @Group: HNTC2_ // @Path: ../Filter/HarmonicNotchFilter.cpp - AP_SUBGROUPINFO(_harmonic_notch_filter[1], "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), + AP_SUBGROUPINFO(harmonic_notches[1].params, "HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams), #endif // @Param: GYRO_RATE @@ -876,28 +876,25 @@ AP_InertialSensor::init(uint16_t loop_rate) // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value - uint8_t num_filters = 0; - bool double_notch[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS] {}; - for (uint8_t i=0; iget_motor_mask()); + notch.num_dynamic_notches = __builtin_popcount(motors->get_motor_mask()); } } // avoid harmonics unless actually configured by the user - _harmonic_notch_filter[i].set_default_harmonics(1); + notch.params.set_default_harmonics(1); } #endif } @@ -907,11 +904,13 @@ AP_InertialSensor::init(uint16_t loop_rate) sensors_used += _use[i]; } - for (uint8_t i=0; i 1) { + filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); + } else { + filter[instance].update(center_freq); + } + last_center_freq_hz = center_freq; + } +} /* update gyro and accel values from backends @@ -1606,6 +1631,13 @@ void AP_InertialSensor::update(void) for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } + for (uint8_t i=0; i<_gyro_count; i++) { + const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; + const float gyro_rate = _gyro_raw_sample_rates[i]; + for (auto ¬ch : harmonic_notches) { + notch.update_params(i, converging, gyro_rate); + } + } if (!_startup_error_counts_set) { for (uint8_t i=0; i= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { - return; - } +void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq) +{ // protect against zero as the scaled frequency if (is_positive(scaled_freq)) { - _calculated_harmonic_notch_freq_hz[idx][0] = scaled_freq; + calculated_notch_freq_hz[0] = scaled_freq; } - _num_calculated_harmonic_notch_frequencies[idx] = 1; + num_calculated_notch_frequencies = 1; } // Update the harmonic notch frequency -void AP_InertialSensor::update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]) { - if (idx >= HAL_INS_NUM_HARMONIC_NOTCH_FILTERS) { - return; - } +void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) { // protect against zero as the scaled frequency for (uint8_t i = 0; i < num_freqs; i++) { if (is_positive(scaled_freq[i])) { - _calculated_harmonic_notch_freq_hz[idx][i] = scaled_freq[i]; + calculated_notch_freq_hz[i] = scaled_freq[i]; } } // any uncalculated frequencies will float at the previous value or the initialized freq if none - _num_calculated_harmonic_notch_frequencies[idx] = num_freqs; + num_calculated_notch_frequencies = num_freqs; } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index dc408c7ff61..a3fb8d1259e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -39,6 +39,11 @@ #define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2 #endif +// time for the estimated gyro rates to converge +#ifndef HAL_INS_CONVERGANCE_MS +#define HAL_INS_CONVERGANCE_MS 30000 +#endif + #include #include @@ -242,46 +247,12 @@ class AP_InertialSensor : AP_AccelCal_Client uint8_t get_primary_accel(void) const { return _primary_accel; } uint8_t get_primary_gyro(void) const { return _primary_gyro; } - // Update the harmonic notch frequency - void update_harmonic_notch_freq_hz(uint8_t idx, float scaled_freq); - // Update the harmonic notch frequencies - void update_harmonic_notch_frequencies_hz(uint8_t idx, uint8_t num_freqs, const float scaled_freq[]); - // get the gyro filter rate in Hz uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; } // get the accel filter rate in Hz uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; } - // harmonic notch current center frequency - float get_gyro_dynamic_notch_center_freq_hz(uint8_t idx) const { return idx 1) { - _imu._gyro_harmonic_notch_filter[i][instance].update(num_gyro_harmonic_notch_center_frequencies(i), gyro_harmonic_notch_center_frequencies_hz(i)); - } else { - _imu._gyro_harmonic_notch_filter[i][instance].update(gyro_harmonic_notch_center_freq_hz(i)); - } - _last_harmonic_notch_center_freq_hz[i] = gyro_harmonic_notch_center_freq_hz(i); - } - } } /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 2e54ded740c..2dcd56958b6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -200,7 +200,7 @@ class AP_InertialSensor_Backend void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < 30000; } + bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); @@ -257,23 +257,6 @@ class AP_InertialSensor_Backend return (uint16_t)_imu._loop_rate; } - // return the harmonic notch filter center in Hz for the sample rate - float gyro_harmonic_notch_center_freq_hz(uint8_t idx) const { return idx Date: Fri, 15 Apr 2022 17:39:29 +1000 Subject: [PATCH 0682/3101] AP_Arming: use HarmonicNotch class --- libraries/AP_Arming/AP_Arming.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 73a048e4e46..d8baeead159 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -394,14 +394,6 @@ bool AP_Arming::ins_checks(bool report) check_failed(ARMING_CHECK_INS, report, "%s", failure_msg); return false; } -#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 - if (ins.gyro_harmonic_notch_enabled(0) && - ins.gyro_harmonic_notch_enabled(1) && - ins.get_gyro_harmonic_notch_tracking_mode(0) != HarmonicNotchDynamicMode::Fixed && - ins.get_gyro_harmonic_notch_tracking_mode(0) != ins.get_gyro_harmonic_notch_tracking_mode(1)) { - check_failed(ARMING_CHECK_INS, report, "conflicting notch filters"); - } -#endif } #if HAL_GYROFFT_ENABLED From 86db91e3b456aa7abebddbdd7b8082311fd830d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:39:30 +1000 Subject: [PATCH 0683/3101] AP_GyroFFT: use HarmonicNotch class --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 28b57e63f9c..91c93bd5e44 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -252,10 +252,10 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) // note that we only allow one harmonic notch filter linked to the FFT code uint8_t harmonics = 0; uint8_t num_notches = 0; - for (uint8_t i=0; iget_gyro_harmonic_notch_tracking_mode(i) == HarmonicNotchDynamicMode::UpdateGyroFFT) { - harmonics = _ins->get_gyro_harmonic_notch_harmonics(i); - num_notches = _ins->get_num_gyro_dynamic_notches(i); + for (auto ¬ch : _ins->harmonic_notches) { + if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + harmonics = notch.params.harmonics(); + num_notches = notch.num_dynamic_notches; break; } } @@ -385,8 +385,8 @@ void AP_GyroFFT::update() _health = 0; } else { uint8_t num_notches = 0; - for (uint8_t i=0; iget_num_gyro_dynamic_notches(i)); + for (auto ¬ch : _ins->harmonic_notches) { + num_notches = MAX(num_notches, notch.num_dynamic_notches); } _health = MIN(_global_state._health, num_notches); } From e6b56ce9baa56149b72053a4c9bf41c5af634e7a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:39:30 +1000 Subject: [PATCH 0684/3101] AP_RPM: use HarmonicNotch class --- libraries/AP_RPM/RPM_HarmonicNotch.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.cpp b/libraries/AP_RPM/RPM_HarmonicNotch.cpp index 677ef01baee..eae7178f35b 100644 --- a/libraries/AP_RPM/RPM_HarmonicNotch.cpp +++ b/libraries/AP_RPM/RPM_HarmonicNotch.cpp @@ -32,10 +32,10 @@ AP_RPM_HarmonicNotch::AP_RPM_HarmonicNotch(AP_RPM &_ap_rpm, uint8_t _instance, A void AP_RPM_HarmonicNotch::update(void) { AP_InertialSensor& ins = AP::ins(); - for (uint8_t i=0; i Date: Fri, 15 Apr 2022 17:40:01 +1000 Subject: [PATCH 0685/3101] Copter: moved harmonic notch update code to AP_Vehicle --- ArduCopter/Copter.h | 1 - ArduCopter/Parameters.cpp | 8 ++--- ArduCopter/system.cpp | 75 --------------------------------------- 3 files changed, 3 insertions(+), 81 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5d1960da143..d4fe2fe6b0a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -879,7 +879,6 @@ class Copter : public AP_Vehicle { // system.cpp void init_ardupilot() override; void startup_INS_ground(); - void update_dynamic_notch(uint8_t idx) override; bool position_ok() const; bool ekf_has_absolute_position() const; bool ekf_has_relative_position() const; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 9bd31301a29..8e148ffe949 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1388,7 +1388,7 @@ void Copter::convert_pid_parameters(void) } #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 - if (!ins.gyro_harmonic_notch_enabled(1)) { + if (!ins.harmonic_notches[1].params.enabled()) { // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch const AP_Param::ConversionInfo notchfilt_conversion_info[] { { Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" }, @@ -1400,10 +1400,8 @@ void Copter::convert_pid_parameters(void) for (uint8_t i=0; iget_throttle_out() / ref)); - - switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) { - case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking - // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle - ins.update_harmonic_notch_freq_hz(idx, throttle_freq); - break; - -#if RPM_ENABLED == ENABLED - case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking - case HarmonicNotchDynamicMode::UpdateRPM2: { - uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1); - float rpm; - if (rpm_sensor.get_rpm(sensor, rpm)) { - // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60))); - } else { - ins.update_harmonic_notch_freq_hz(idx, ref_freq); - } - break; - } -#endif -#if HAL_WITH_ESC_TELEM - case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking - // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { - float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - - for (uint8_t i = 0; i < num_notches; i++) { - notches[i] = MAX(ref_freq, notches[i]); - } - if (num_notches > 0) { - ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches); - } else { // throttle fallback - ins.update_harmonic_notch_freq_hz(idx, throttle_freq); - } - } else { - ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); - } - break; -#endif -#if HAL_GYROFFT_ENABLED - case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking - // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { - float notches[INS_MAX_NOTCHES]; - const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - - ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches); - } else { - ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz()); - } - break; -#endif - case HarmonicNotchDynamicMode::Fixed: // static - default: - ins.update_harmonic_notch_freq_hz(idx, ref_freq); - break; - } -} - // position_ok - returns true if the horizontal absolute position is ok and home position is set bool Copter::position_ok() const { From 15084cb6f3079880e0be95c9f72ff7d7490e8a9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:40:01 +1000 Subject: [PATCH 0686/3101] Plane: moved harmonic notch update code to AP_Vehicle --- ArduPlane/Parameters.cpp | 8 ++-- ArduPlane/Plane.h | 1 - ArduPlane/system.cpp | 80 ---------------------------------------- 3 files changed, 3 insertions(+), 86 deletions(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 484b40d82f6..2f01551955f 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1503,7 +1503,7 @@ void Plane::load_parameters(void) #endif #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1 - if (!ins.gyro_harmonic_notch_enabled(1)) { + if (!ins.harmonic_notches[1].params.enabled()) { // notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch const AP_Param::ConversionInfo notchfilt_conversion_info[] { { Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" }, @@ -1515,10 +1515,8 @@ void Plane::load_parameters(void) for (uint8_t i=0; iget_throttle_out() / ref))); - } -#endif - break; - - case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking - case HarmonicNotchDynamicMode::UpdateRPM2: { - uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1); - float rpm; - if (rpm_sensor.get_rpm(sensor, rpm)) { - // set the harmonic notch filter frequency from the main rotor rpm - ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0))); - } else { - ins.update_harmonic_notch_freq_hz(idx, ref_freq); - } - break; - } -#if HAL_WITH_ESC_TELEM - case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking - // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { - float notches[INS_MAX_NOTCHES]; - const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - - for (uint8_t i = 0; i < num_notches; i++) { - notches[i] = MAX(ref_freq, notches[i]); - } - if (num_notches > 0) { - ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches); -#if HAL_QUADPLANE_ENABLED - } else if (quadplane.available()) { // throttle fallback - ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref))); -#endif - } else { - ins.update_harmonic_notch_freq_hz(idx, ref_freq); - } - } else { - ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); - } - break; -#endif -#if HAL_GYROFFT_ENABLED - case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking - // set the harmonic notch filter frequency scaled on measured frequency - if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) { - float notches[INS_MAX_NOTCHES]; - const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches); - - ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches); - } else { - ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz()); - } - break; -#endif - case HarmonicNotchDynamicMode::Fixed: // static - default: - ins.update_harmonic_notch_freq_hz(idx, ref_freq); - break; - } -} From 301c56d30a7b31a607d4af2a426193a20932d217 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 17:40:24 +1000 Subject: [PATCH 0687/3101] AP_Vehicle: implement common harmonic notch update code --- libraries/AP_Vehicle/AP_Vehicle.cpp | 91 +++++++++++++++++++++++++++-- libraries/AP_Vehicle/AP_Vehicle.h | 2 +- 2 files changed, 87 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 07910f49af8..04d9b8fd405 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include @@ -395,19 +396,98 @@ bool AP_Vehicle::is_crashed() const return AP::arming().last_disarm_method() == AP_Arming::Method::CRASH; } +// update the harmonic notch filter center frequency dynamically +void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) +{ +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI + if (!notch.params.enabled()) { + return; + } + const float ref_freq = notch.params.center_freq_hz(); + const float ref = notch.params.reference(); + if (is_zero(ref)) { + notch.update_freq_hz(ref_freq); + return; + } + + const AP_Motors* motors = AP::motors(); + const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0; + const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors_throttle / ref)); + + switch (notch.params.tracking_mode()) { + case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking + // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle + notch.update_freq_hz(throttle_freq); + break; + + case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking + case HarmonicNotchDynamicMode::UpdateRPM2: { + const auto *rpm_sensor = AP::rpm(); + uint8_t sensor = (notch.params.tracking_mode()==HarmonicNotchDynamicMode::UpdateRPM?0:1); + float rpm; + if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) { + // set the harmonic notch filter frequency from the main rotor rpm + notch.update_freq_hz(MAX(ref_freq, rpm * ref * (1.0/60))); + } else { + notch.update_freq_hz(ref_freq); + } + break; + } +#if HAL_WITH_ESC_TELEM + case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking + // set the harmonic notch filter frequency scaled on measured frequency + if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + float notches[INS_MAX_NOTCHES]; + const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(notch.num_dynamic_notches, notches); + + for (uint8_t i = 0; i < num_notches; i++) { + notches[i] = MAX(ref_freq, notches[i]); + } + if (num_notches > 0) { + notch.update_frequencies_hz(num_notches, notches); + } else { // throttle fallback + notch.update_freq_hz(throttle_freq); + } + } else { + notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); + } + break; +#endif +#if HAL_GYROFFT_ENABLED + case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking + // set the harmonic notch filter frequency scaled on measured frequency + if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { + float notches[INS_MAX_NOTCHES]; + const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches); + + notch.update_frequencies_hz(peaks, notches); + } else { + notch.update_freq_hz(gyro_fft.get_weighted_noise_center_freq_hz()); + } + break; +#endif + case HarmonicNotchDynamicMode::Fixed: // static + default: + notch.update_freq_hz(ref_freq); + break; + } +#endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI +} + + // run notch update at either loop rate or 200Hz void AP_Vehicle::update_dynamic_notch_at_specified_rate() { - for (uint8_t i=0; i 5) { _last_notch_update_ms[i] = now; - update_dynamic_notch(i); + update_dynamic_notch(notch); } } } @@ -534,3 +614,4 @@ AP_Vehicle *vehicle() } }; + diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 2652baf5c25..8074f4babcc 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -249,7 +249,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { #endif // AP_SCRIPTING_ENABLED // update the harmonic notch - virtual void update_dynamic_notch(uint8_t idx) {}; + void update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch); // zeroing the RC outputs can prevent unwanted motor movement: virtual bool should_zero_rc_outputs_on_reboot() const { return false; } From 492e203fd2d024fb9999a812c7abd7eb60e43656 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Apr 2022 18:56:15 +1000 Subject: [PATCH 0688/3101] AP_GyroFFT: allow for 2 FFT based notches --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 91c93bd5e44..24b37cb80c4 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -254,9 +254,8 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { - harmonics = notch.params.harmonics(); - num_notches = notch.num_dynamic_notches; - break; + harmonics |= notch.params.harmonics(); + num_notches = MAX(num_notches, notch.num_dynamic_notches); } } // count the number of active harmonics or dynamic notchs From 184f84f4ee55fced2439410d9ca3aaa2324f758f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Apr 2022 12:08:09 +1000 Subject: [PATCH 0689/3101] AP_InertialSensor: don't update disabled notches --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index affac1b8464..e689e67ee84 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -877,6 +877,9 @@ AP_InertialSensor::init(uint16_t loop_rate) // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the // configured value for (auto ¬ch : harmonic_notches) { + if (!notch.params.enabled()) { + continue; + } notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; @@ -1635,7 +1638,9 @@ void AP_InertialSensor::update(void) const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; const float gyro_rate = _gyro_raw_sample_rates[i]; for (auto ¬ch : harmonic_notches) { - notch.update_params(i, converging, gyro_rate); + if (notch.params.enabled()) { + notch.update_params(i, converging, gyro_rate); + } } } From e0bb71723131a117059150f0024a45f9d421a13f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Apr 2022 12:08:22 +1000 Subject: [PATCH 0690/3101] AP_GyroFFT: skip disabled notches --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 24b37cb80c4..a14f00c5d3e 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -253,7 +253,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) uint8_t harmonics = 0; uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { - if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { harmonics |= notch.params.harmonics(); num_notches = MAX(num_notches, notch.num_dynamic_notches); } @@ -385,7 +385,9 @@ void AP_GyroFFT::update() } else { uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { - num_notches = MAX(num_notches, notch.num_dynamic_notches); + if (notch.params.enabled()) { + num_notches = MAX(num_notches, notch.num_dynamic_notches); + } } _health = MIN(_global_state._health, num_notches); } From dff0e5ecc1c45303237859b3fc587fd3a6eed524 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 17:03:46 +1000 Subject: [PATCH 0691/3101] AP_InertialSensor: fixed the last notch values to be per-instance thanks to Andy for noticing this --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 14 +++++++------- libraries/AP_InertialSensor/AP_InertialSensor.h | 6 +++--- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e689e67ee84..99517dbf686 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1593,23 +1593,23 @@ void AP_InertialSensor::_save_gyro_calibration() void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate) { const float center_freq = calculated_notch_freq_hz[0]; - if (!is_equal(last_bandwidth_hz, params.bandwidth_hz()) || - !is_equal(last_attenuation_dB, params.attenuation_dB()) || + if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) || + !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) || converging) { filter[instance].init(gyro_rate, center_freq, params.bandwidth_hz(), params.attenuation_dB()); - last_center_freq_hz = center_freq; - last_bandwidth_hz = params.bandwidth_hz(); - last_attenuation_dB = params.attenuation_dB(); - } else if (!is_equal(last_center_freq_hz, center_freq)) { + last_center_freq_hz[instance] = center_freq; + last_bandwidth_hz[instance] = params.bandwidth_hz(); + last_attenuation_dB[instance] = params.attenuation_dB(); + } else if (!is_equal(last_center_freq_hz[instance], center_freq)) { if (num_calculated_notch_frequencies > 1) { filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz); } else { filter[instance].update(center_freq); } - last_center_freq_hz = center_freq; + last_center_freq_hz[instance] = center_freq; } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index a3fb8d1259e..1c662fb8cf9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -446,9 +446,9 @@ class AP_InertialSensor : AP_AccelCal_Client private: // support for updating harmonic filter at runtime - float last_center_freq_hz; - float last_bandwidth_hz; - float last_attenuation_dB; + float last_center_freq_hz[INS_MAX_INSTANCES]; + float last_bandwidth_hz[INS_MAX_INSTANCES]; + float last_attenuation_dB[INS_MAX_INSTANCES]; } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; private: From bccca9ed2e3444d40e7ee9d6ee2354ba99056b62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 17:24:45 +1000 Subject: [PATCH 0692/3101] AP_GyroFFT: added defaults for FFT with no notch allow for testing with FFT enabled, and defaulting number of frequencies to look for --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index a14f00c5d3e..71c2512d6b3 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -253,11 +253,15 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz) uint8_t harmonics = 0; uint8_t num_notches = 0; for (auto ¬ch : _ins->harmonic_notches) { - if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { + if (notch.params.enabled()) { harmonics |= notch.params.harmonics(); num_notches = MAX(num_notches, notch.num_dynamic_notches); } } + if (harmonics == 0) { + // this allows use of FFT to find peaks with all notch filters disabled + harmonics = 3; + } // count the number of active harmonics or dynamic notchs _tracked_peaks = constrain_int16(MAX(__builtin_popcount(harmonics), num_notches), 1, FrequencyPeak::MAX_TRACKED_PEAKS); @@ -383,7 +387,7 @@ void AP_GyroFFT::update() if (!_rpy_health.x && !_rpy_health.y) { _health = 0; } else { - uint8_t num_notches = 0; + uint8_t num_notches = 1; for (auto ¬ch : _ins->harmonic_notches) { if (notch.params.enabled()) { num_notches = MAX(num_notches, notch.num_dynamic_notches); From db14ba46dc3fb1a8e0e87996329d0b93fb480ea5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Apr 2022 17:35:57 +1000 Subject: [PATCH 0693/3101] AP_InertialSensor: call notch param update with semaphore held --- libraries/AP_InertialSensor/AP_InertialSensor.cpp | 9 --------- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 10 +++++++++- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 99517dbf686..f3e90daf34c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1634,15 +1634,6 @@ void AP_InertialSensor::update(void) for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } - for (uint8_t i=0; i<_gyro_count; i++) { - const bool converging = AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; - const float gyro_rate = _gyro_raw_sample_rates[i]; - for (auto ¬ch : harmonic_notches) { - if (notch.params.enabled()) { - notch.update_params(i, converging, gyro_rate); - } - } - } if (!_startup_error_counts_set) { for (uint8_t i=0; i Date: Wed, 13 Apr 2022 13:50:36 +0100 Subject: [PATCH 0694/3101] SITL: move motor related constants to motor object --- libraries/SITL/SIM_Frame.cpp | 11 +++++------ libraries/SITL/SIM_Frame.h | 2 -- libraries/SITL/SIM_Motor.cpp | 15 ++++++++------- libraries/SITL/SIM_Motor.h | 9 +++++---- 4 files changed, 18 insertions(+), 19 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 03f88b343e6..d7bac9fe7c0 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -465,9 +465,9 @@ void Frame::init(const char *frame_str, Battery *_battery) float hover_power = model.refCurrent * model.refVoltage; float hover_velocity_out = 2 * hover_power / hover_thrust; float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out)); - velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut); + float velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut); thrust_max = 0.5 * ref_air_density * effective_disc_area * sq(velocity_max); - effective_prop_area = effective_disc_area / num_motors; + float effective_prop_area = effective_disc_area / num_motors; // power_factor is ratio of power consumed per newton of thrust float power_factor = hover_power / hover_thrust; @@ -476,7 +476,7 @@ void Frame::init(const char *frame_str, Battery *_battery) for (uint8_t i=0; iget_voltage()); + ref_air_density, battery->get_voltage()); ::printf("pwm[%u] cmd=%.3f thrust=%.3f hovthst=%.3f\n", pwm, motors[0].pwm_to_command(pwm), -thrust.z*num_motors, hover_thrust); } @@ -538,8 +538,7 @@ void Frame::calculate_forces(const Aircraft &aircraft, float current = 0; for (uint8_t i=0; iget_voltage()); + motors[i].calculate_forces(input, motor_offset, mraccel, mthrust, vel_air_bf, air_density, battery->get_voltage()); current += motors[i].get_current(); rot_accel += mraccel; thrust += mthrust; diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index e0c11e5816d..72c3b9e8953 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -134,9 +134,7 @@ class Frame { // exposed area times coefficient of drag float areaCd; float mass; - float velocity_max; float thrust_max; - float effective_prop_area; Battery *battery; float last_param_voltage; diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index fd025ad5348..1c784764e31 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -28,8 +28,6 @@ void Motor::calculate_forces(const struct sitl_input &input, Vector3f &thrust, const Vector3f &velocity_air_bf, float air_density, - float velocity_max, - float effective_prop_area, float voltage) { // fudge factors @@ -64,7 +62,7 @@ void Motor::calculate_forces(const struct sitl_input &input, float velocity_in = MAX(0, -velocity_air_bf.z); // get thrust for untilted motor - float motor_thrust = calc_thrust(command, air_density, effective_prop_area, velocity_in, velocity_max * voltage_scale); + float motor_thrust = calc_thrust(command, air_density, velocity_in, voltage_scale); // thrust in NED thrust = {0, 0, -motor_thrust}; @@ -153,7 +151,8 @@ float Motor::get_current(void) const // setup PWM ranges for this motor void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, - float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max) + float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, + float _velocity_max) { mot_pwm_min = _pwm_min; mot_pwm_max = _pwm_max; @@ -165,6 +164,8 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, diagonal_size = _diagonal_size; power_factor = _power_factor; voltage_max = _voltage_max; + effective_prop_area = _effective_prop_area; + max_outflow_velocity = _velocity_max; // assume 50% of mass on ring around center moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5); @@ -186,14 +187,14 @@ float Motor::pwm_to_command(float pwm) const /* calculate thrust given a command value */ -float Motor::calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const +float Motor::calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const { - float velocity_out = velocity_max * sqrtf((1-mot_expo)*command + mot_expo*sq(command)); + float velocity_out = voltage_scale * max_outflow_velocity * sqrtf((1-mot_expo)*command + mot_expo*sq(command)); float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in)); #if 0 if (command > 0) { ::printf("air_density=%f effective_prop_area=%f velocity_in=%f velocity_max=%f\n", - air_density, effective_prop_area, velocity_in, velocity_max); + air_density, effective_prop_area, velocity_in, voltage_scale * max_outflow_velocity); ::printf("calc_thrust %.3f %.3f\n", command, ret); } #endif diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index abbaf0c2e61..6dddaa1963f 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -75,8 +75,6 @@ class Motor { Vector3f &body_thrust, // Z is down const Vector3f &velocity_air_bf, float air_density, - float velocity_max, - float effective_prop_area, float voltage); uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value) const; @@ -89,7 +87,8 @@ class Motor { // setup motor key parameters void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, - float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max); + float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, + float _velocity_max); // override slew limit void set_slew_max(float _slew_max) { @@ -101,7 +100,7 @@ class Motor { } // calculate thrust of motor - float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const; + float calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const; private: float mot_pwm_min; @@ -116,6 +115,8 @@ class Motor { float power_factor; float voltage_max; Vector3f moment_of_inertia; + float effective_prop_area; + float max_outflow_velocity; float last_command; uint64_t last_calc_us; From 6272dc33dd96364c34f2205af028313dc1f063f1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Apr 2022 15:03:53 +0100 Subject: [PATCH 0695/3101] SITL: move moments inertia to frame property --- libraries/SITL/SIM_Frame.cpp | 19 +++++++++++++++---- libraries/SITL/SIM_Frame.h | 3 +++ libraries/SITL/SIM_Motor.cpp | 19 ++++--------------- libraries/SITL/SIM_Motor.h | 4 +--- 4 files changed, 23 insertions(+), 22 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index d7bac9fe7c0..fc84513f87e 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -476,9 +476,14 @@ void Frame::init(const char *frame_str, Battery *_battery) for (uint8_t i=0; iget_voltage()); + Vector3f mtorque, mthrust; + motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, air_density, battery->get_voltage()); current += motors[i].get_current(); - rot_accel += mraccel; + torque += mtorque; thrust += mthrust; // simulate motor rpm if (!is_zero(AP::sitl()->vibe_motor)) { @@ -548,6 +554,11 @@ void Frame::calculate_forces(const Aircraft &aircraft, } } + // calculate total rotational acceleration + rot_accel.x = torque.x / model.moment_of_inertia.x; + rot_accel.y = torque.y / model.moment_of_inertia.y; + rot_accel.z = torque.z / model.moment_of_inertia.z; + body_accel = thrust/aircraft.gross_mass(); if (terminal_rotation_rate > 0) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index 72c3b9e8953..2f4ee6db384 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -127,6 +127,9 @@ class Frame { // momentum drag coefficient float mdrag_coef = 0.2; + // if zero value will be estimated from mass + Vector3f moment_of_inertia; + } default_model; struct Model model; diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index 1c784764e31..da41cd03418 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -24,7 +24,7 @@ using namespace SITL; // calculate rotational accel and thrust for a motor void Motor::calculate_forces(const struct sitl_input &input, uint8_t motor_offset, - Vector3f &rot_accel, + Vector3f &torque, Vector3f &thrust, const Vector3f &velocity_air_bf, float air_density, @@ -39,7 +39,7 @@ void Motor::calculate_forces(const struct sitl_input &input, if (voltage_scale < 0.1) { // battery is dead - rot_accel.zero(); + torque.zero(); thrust.zero(); current = 0; return; @@ -96,7 +96,7 @@ void Motor::calculate_forces(const struct sitl_input &input, last_change_usec = now; // calculate torque in newton-meters - Vector3f torque = (arm % thrust) + rotor_torque; + torque = (arm % thrust) + rotor_torque; // possibly rotate the thrust vector and the rotor torque if (!is_zero(roll) || !is_zero(pitch)) { @@ -106,11 +106,6 @@ void Motor::calculate_forces(const struct sitl_input &input, torque = rotation * torque; } - // calculate total rotational acceleration - rot_accel.x = torque.x / moment_of_inertia.x; - rot_accel.y = torque.y / moment_of_inertia.y; - rot_accel.z = torque.z / moment_of_inertia.z; - // calculate current float power = power_factor * fabsf(motor_thrust); current = power / MAX(voltage, 0.1); @@ -151,7 +146,7 @@ float Motor::get_current(void) const // setup PWM ranges for this motor void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, - float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, + float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, float _velocity_max) { mot_pwm_min = _pwm_min; @@ -160,17 +155,11 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, mot_spin_max = _spin_max; mot_expo = _expo; slew_max = _slew_max; - vehicle_mass = _vehicle_mass; diagonal_size = _diagonal_size; power_factor = _power_factor; voltage_max = _voltage_max; effective_prop_area = _effective_prop_area; max_outflow_velocity = _velocity_max; - - // assume 50% of mass on ring around center - moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5); - moment_of_inertia.y = moment_of_inertia.x; - moment_of_inertia.z = vehicle_mass * 0.5 * sq(diagonal_size*0.5); } /* diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index 6dddaa1963f..2cbdf8f5f95 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -87,7 +87,7 @@ class Motor { // setup motor key parameters void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, - float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, + float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area, float _velocity_max); // override slew limit @@ -109,12 +109,10 @@ class Motor { float mot_spin_max; float mot_expo; float slew_max; - float vehicle_mass; float diagonal_size; float current; float power_factor; float voltage_max; - Vector3f moment_of_inertia; float effective_prop_area; float max_outflow_velocity; From 47f327b50030f7726b66b9881e203795850f384a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Apr 2022 15:51:47 +0100 Subject: [PATCH 0696/3101] SITL: allow Vector3f inertia to be set via JSON --- libraries/SITL/SIM_Frame.cpp | 43 +++++++++++++++++++++++++++--------- 1 file changed, 33 insertions(+), 10 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index fc84513f87e..c4cdd13d5cc 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -379,11 +379,17 @@ void Frame::load_frame_params(const char *model_json) exit(1); } + enum JSON_TYPE { + JSON_FLOAT, + JSON_VECTOR3F, + }; + struct { const char *label; - float &v; + void *ptr; + JSON_TYPE t; } vars[] = { -#define FRAME_VAR(s) { #s, model.s } +#define FRAME_VAR(s) { #s, &model.s, JSON_TYPE::JSON_FLOAT } FRAME_VAR(mass), FRAME_VAR(diagonal_size), FRAME_VAR(refSpd), @@ -405,8 +411,8 @@ void Frame::load_frame_params(const char *model_json) FRAME_VAR(slew_max), FRAME_VAR(disc_area), FRAME_VAR(mdrag_coef), + {"moment_inertia", &model.moment_of_inertia, JSON_TYPE::JSON_VECTOR3F}, }; - static_assert(sizeof(model) == sizeof(float)*ARRAY_SIZE(vars), "incorrect model vars"); for (uint8_t i=0; i()) { - AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + if (vars[i].t == JSON_TYPE::JSON_FLOAT) { + if (!v.is()) { + AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + } + *((float *)vars[i].ptr) = v.get(); + + } else if (vars[i].t == JSON_TYPE::JSON_VECTOR3F) { + if (!v.is() || !v.contains(2) || v.contains(3)) { + AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + } + Vector3f &vec = *(Vector3f *)vars[i].ptr; + for (uint8_t j=0; j<3; j++) { + auto array_item = v.get(j); + if (!array_item.is()) { + AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + } + vec[j] = array_item.get(); + } } - vars[i].v = v.get(); } ::printf("Loaded model params from %s\n", model_json); @@ -479,10 +500,12 @@ void Frame::init(const char *frame_str, Battery *_battery) model.diagonal_size, power_factor, model.maxVoltage, effective_prop_area, velocity_max); } - // assume 50% of mass on ring around center - model.moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5); - model.moment_of_inertia.y = model.moment_of_inertia.x; - model.moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5); + if (is_zero(model.moment_of_inertia.x) || is_zero(model.moment_of_inertia.y) || is_zero(model.moment_of_inertia.z)) { + // if no inertia provided, assume 50% of mass on ring around center + model.moment_of_inertia.x = model.mass * 0.25 * sq(model.diagonal_size*0.5); + model.moment_of_inertia.y = model.moment_of_inertia.x; + model.moment_of_inertia.z = model.mass * 0.5 * sq(model.diagonal_size*0.5); + } #if 0 From c4518e5cb71036b5580a28d48aa74f0e919d2f29 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Apr 2022 20:58:16 +0100 Subject: [PATCH 0697/3101] SITL: motor: use postion and thrust vector --- libraries/SITL/SIM_Motor.cpp | 17 ++++++++--------- libraries/SITL/SIM_Motor.h | 24 +++++++++++++++++++++--- 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index da41cd03418..5050be8ab23 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -56,20 +56,16 @@ void Motor::calculate_forces(const struct sitl_input &input, last_command = command; // the yaw torque of the motor - Vector3f rotor_torque(0, 0, yaw_factor * command * yaw_scale * voltage_scale); + Vector3f rotor_torque = thrust_vector * yaw_factor * command * yaw_scale * voltage_scale * -1.0; // calculate velocity into prop, clipping at zero, assumes zero roll/pitch - float velocity_in = MAX(0, -velocity_air_bf.z); + float velocity_in = MAX(0, -velocity_air_bf.projected(thrust_vector).z); // get thrust for untilted motor float motor_thrust = calc_thrust(command, air_density, velocity_in, voltage_scale); // thrust in NED - thrust = {0, 0, -motor_thrust}; - - // define the arm position relative to center of mass - Vector3f arm(cosf(radians(angle)), sinf(radians(angle)), 0); - arm *= diagonal_size; + thrust = thrust_vector * motor_thrust; // work out roll and pitch of motor relative to it pointing straight up float roll = 0, pitch = 0; @@ -96,7 +92,7 @@ void Motor::calculate_forces(const struct sitl_input &input, last_change_usec = now; // calculate torque in newton-meters - torque = (arm % thrust) + rotor_torque; + torque = (position % thrust) + rotor_torque; // possibly rotate the thrust vector and the rotor torque if (!is_zero(roll) || !is_zero(pitch)) { @@ -155,11 +151,14 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, mot_spin_max = _spin_max; mot_expo = _expo; slew_max = _slew_max; - diagonal_size = _diagonal_size; power_factor = _power_factor; voltage_max = _voltage_max; effective_prop_area = _effective_prop_area; max_outflow_velocity = _velocity_max; + + position.x = cosf(radians(angle)) * _diagonal_size; + position.y = sinf(radians(angle)) * _diagonal_size; + position.z = 0; } /* diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index 2cbdf8f5f95..bdf657db1cc 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -49,7 +49,15 @@ class Motor { angle(_angle), // angle in degrees from front yaw_factor(_yaw_factor), // positive is clockwise display_order(_display_order) // order for clockwise display - {} + { + position.x = cosf(radians(angle)); + position.y = sinf(radians(angle)); + position.z = 0; + + thrust_vector.x = 0; + thrust_vector.y = 0; + thrust_vector.z = -1; + } /* alternative constructor for tiltable motors @@ -67,7 +75,15 @@ class Motor { pitch_servo(_pitch_servo), pitch_min(_pitch_min), pitch_max(_pitch_max) - {} + { + position.x = cosf(radians(angle)); + position.y = sinf(radians(angle)); + position.z = 0; + + thrust_vector.x = 0; + thrust_vector.y = 0; + thrust_vector.z = -1; + } void calculate_forces(const struct sitl_input &input, uint8_t motor_offset, @@ -109,7 +125,6 @@ class Motor { float mot_spin_max; float mot_expo; float slew_max; - float diagonal_size; float current; float power_factor; float voltage_max; @@ -118,6 +133,9 @@ class Motor { float last_command; uint64_t last_calc_us; + + Vector3f position; + Vector3f thrust_vector; }; } From a7abfeeb4ade9a7c3c7255694e6b12b7dd170555 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Apr 2022 22:15:57 +0100 Subject: [PATCH 0698/3101] SITL: allow cutom motor postions and thrust vectors to be specified --- libraries/SITL/SIM_Frame.cpp | 64 +++++++++++++++++++++++++----------- libraries/SITL/SIM_Frame.h | 11 +++++++ libraries/SITL/SIM_Motor.cpp | 16 ++++++--- libraries/SITL/SIM_Motor.h | 2 +- 4 files changed, 68 insertions(+), 25 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index c4cdd13d5cc..e84bd7f9622 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -23,10 +23,7 @@ #include #include -#define USE_PICOJSON (CONFIG_HAL_BOARD == HAL_BOARD_SITL) -#if USE_PICOJSON -#include "picojson.h" -#endif + using namespace SITL; @@ -384,11 +381,13 @@ void Frame::load_frame_params(const char *model_json) JSON_VECTOR3F, }; - struct { + struct json_search { const char *label; void *ptr; JSON_TYPE t; - } vars[] = { + }; + + json_search vars[] = { #define FRAME_VAR(s) { #s, &model.s, JSON_TYPE::JSON_FLOAT } FRAME_VAR(mass), FRAME_VAR(diagonal_size), @@ -421,28 +420,52 @@ void Frame::load_frame_params(const char *model_json) continue; } if (vars[i].t == JSON_TYPE::JSON_FLOAT) { - if (!v.is()) { - AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); - } - *((float *)vars[i].ptr) = v.get(); + parse_float(v, vars[i].label, *((float *)vars[i].ptr)); } else if (vars[i].t == JSON_TYPE::JSON_VECTOR3F) { - if (!v.is() || !v.contains(2) || v.contains(3)) { - AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + parse_vector3(v, vars[i].label, *(Vector3f *)vars[i].ptr); + + } + } + + json_search per_motor_vars[] = { + {"position", &model.motor_pos, JSON_TYPE::JSON_VECTOR3F}, + {"vector", &model.motor_thrust_vec, JSON_TYPE::JSON_VECTOR3F}, + }; + char label_name[20]; + for (uint8_t i=0; i()) { + // use default value + continue; } - Vector3f &vec = *(Vector3f *)vars[i].ptr; - for (uint8_t j=0; j<3; j++) { - auto array_item = v.get(j); - if (!array_item.is()) { - AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); - } - vec[j] = array_item.get(); + if (per_motor_vars[i].t == JSON_TYPE::JSON_VECTOR3F) { + parse_vector3(v, label_name, *(((Vector3f *)per_motor_vars[i].ptr) + j)); } } } ::printf("Loaded model params from %s\n", model_json); } + +void Frame::parse_float(picojson::value val, const char* label, float ¶m) { + if (!val.is()) { + AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str()); + } + param = val.get(); +} + +void Frame::parse_vector3(picojson::value val, const char* label, Vector3f ¶m) { + if (!val.is() || !val.contains(2) || val.contains(3)) { + AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str()); + } + for (uint8_t j=0; j<3; j++) { + parse_float(val.get(j), label, param[j]); + } +} + #endif /* @@ -497,7 +520,8 @@ void Frame::init(const char *frame_str, Battery *_battery) for (uint8_t i=0; i Date: Fri, 15 Apr 2022 16:54:56 +0800 Subject: [PATCH 0699/3101] AP_Logger: must set default fd = -1 --- libraries/AP_Logger/AP_Logger.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 5b9d7442407..a0d286db6f3 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -528,7 +528,7 @@ class AP_Logger void reset(); void remove_and_free(file_list *victim); struct file_list *head, *tail; - int fd; + int fd{-1}; uint32_t offset; bool fast; uint8_t counter; From 1340132f6fc242e5d4c678935a0f4091e6db7c2a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 22 Oct 2021 08:55:42 +1100 Subject: [PATCH 0700/3101] autotest: add rangefinder test --- Tools/autotest/common.py | 9 ++++++--- Tools/autotest/rover.py | 25 +++++++++++++++++++++++++ 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 5f0ce974454..2974b276b59 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4321,14 +4321,17 @@ def send_mavlink_arm_command(self): 0, 0) - def set_analog_rangefinder_parameters(self): - self.set_parameters({ + def analog_rangefinder_parameters(self): + return { "RNGFND1_TYPE": 1, "RNGFND1_MIN_CM": 0, "RNGFND1_MAX_CM": 4000, "RNGFND1_SCALING": 12.12, "RNGFND1_PIN": 0, - }) + } + + def set_analog_rangefinder_parameters(self): + self.set_parameters(self.analog_rangefinder_parameters()) def send_debug_trap(self, timeout=6000): self.progress("Sending trap to autopilot") diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 6eca1fc0802..685739b05d3 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -5785,6 +5785,27 @@ def FRAMStorage(self): # make sure we're back at our original value: self.assert_parameter_value("LOG_BITMASK", 1) + def RangeFinder(self): + # the following magic numbers correspond to the post locations in SITL + home_string = "%s,%s,%s,%s" % (51.8752066, 14.6487840, 54.15, 315) + + self.set_parameters({ + "SIM_SONAR_ROT": 6, + **(self.analog_rangefinder_parameters()), + }) + self.customise_SITL_commandline([ + "--home", home_string, + ]) + self.wait_ready_to_arm() + if self.mavproxy is not None: + self.mavproxy.send('script /tmp/post-locations.scr\n') + m = self.assert_receive_message('RANGEFINDER', very_verbose=True) + if m.voltage == 0: + raise NotAchievedException("Did not get non-zero voltage") + want_range = 10 + if abs(m.distance - want_range) > 0.1: + raise NotAchievedException("Expected %fm got %fm" % (want_range, m.distance)) + def test_depthfinder(self): # Setup rangefinders self.customise_SITL_commandline([ @@ -6083,6 +6104,10 @@ def tests(self): "Accelerometer Calibration testing", self.accelcal), + ("RangeFinder", + "Test RangeFinder", + self.RangeFinder), + ("AP_Proximity_MAV", "Test MAV proximity backend", self.ap_proximity_mav), From 348f0ad590aa66b946056fcf365028fdb97895fa Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 18 Apr 2022 15:30:09 +1000 Subject: [PATCH 0701/3101] SITL: add SIM_SONAR_ROT, use it for measuring horizontal distances --- libraries/SITL/SIM_Aircraft.cpp | 11 +- libraries/SITL/SIM_SerialProximitySensor.cpp | 94 ----------------- libraries/SITL/SIM_SerialProximitySensor.h | 10 +- libraries/SITL/SITL.cpp | 104 ++++++++++++++++++- libraries/SITL/SITL.h | 3 + 5 files changed, 118 insertions(+), 104 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 62d23d659ef..63e16e6b4f8 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -483,11 +483,19 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) // is bouncing off: float Aircraft::perpendicular_distance_to_rangefinder_surface() const { - return sitl->height_agl; + switch ((Rotation)sitl->sonar_rot.get()) { + case Rotation::ROTATION_PITCH_270: + return sitl->height_agl; + case ROTATION_NONE ... ROTATION_YAW_315: + return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45); + default: + AP_BoardConfig::config_error("Bad simulated sonar rotation"); + } } float Aircraft::rangefinder_range() const { + float roll = sitl->state.rollDeg; float pitch = sitl->state.pitchDeg; @@ -524,6 +532,7 @@ float Aircraft::rangefinder_range() const // sensor position offset in body frame const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset; + // n.b. the following code is assuming rotation-pitch-270: // adjust altitude for position of the sensor on the vehicle if position offset is non-zero if (!relPosSensorBF.is_zero()) { // get a rotation matrix following DCM conventions (body to earth) diff --git a/libraries/SITL/SIM_SerialProximitySensor.cpp b/libraries/SITL/SIM_SerialProximitySensor.cpp index aa2c0575016..b357551ebdd 100644 --- a/libraries/SITL/SIM_SerialProximitySensor.cpp +++ b/libraries/SITL/SIM_SerialProximitySensor.cpp @@ -43,98 +43,4 @@ void SerialProximitySensor::update(const Location &location) write_to_autopilot((char*)data, packetlen); } -float SerialProximitySensor::measure_distance_at_angle_bf(const Location &location, float angle) const -{ - const SIM *sitl = AP::sitl(); - - Vector2f vehicle_pos_cm; - if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) { - // should probably use SITL variables... - return 0.0f; - } - static uint64_t count = 0; - - if (count == 0) { - unlink("/tmp/rayfile.scr"); - unlink("/tmp/intersectionsfile.scr"); - } - - count++; - - // the 1000 here is so the files don't grow unbounded - const bool write_debug_files = count < 1000; - - FILE *rayfile = nullptr; - if (write_debug_files) { - rayfile = fopen("/tmp/rayfile.scr", "a"); - } - // cast a ray from location out 200m... - Location location2 = location; - location2.offset_bearing(wrap_180(angle + sitl->state.yawDeg), 200); - Vector2f ray_endpos_cm; - if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) { - // should probably use SITL variables... - return 0.0f; - } - if (rayfile != nullptr) { - ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7); - fclose(rayfile); - } - - // setup a grid of posts - FILE *postfile = nullptr; - FILE *intersectionsfile = nullptr; - if (write_debug_files) { - static bool postfile_written; - if (!postfile_written) { - ::fprintf(stderr, "Writing /tmp/post-locations.scr\n"); - postfile_written = true; - postfile = fopen("/tmp/post-locations.scr", "w"); - } - intersectionsfile = fopen("/tmp/intersections.scr", "a"); - } - const float radius_cm = 100.0f; - float min_dist_cm = 1000000.0; - const uint8_t num_post_offset = 10; - for (int8_t x=-num_post_offset; xmeasure_distance_at_angle_bf(location, angle); + } private: uint32_t last_sent_ms; - const Location post_origin { - 518752066, - 146487830, - 0, - Location::AltFrame::ABSOLUTE - }; }; } diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 8007e4e4e1f..7e07c8efb6f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -54,6 +54,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { AP_GROUPINFO("WIND_DIR", 10, SIM, wind_direction, 180), AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0), AP_GROUPINFO("SERVO_SPEED", 16, SIM, servo_speed, 0.14), + AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270), AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, 12.6f), AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0), AP_GROUPINFO("SONAR_GLITCH", 23, SIM, sonar_glitch, 0), @@ -508,7 +509,14 @@ const AP_Param::GroupInfo SIM::var_ins[] = { #endif // HAL_INS_TEMPERATURE_CAL_ENABLE AP_GROUPEND }; - + +const Location post_origin { + 518752066, + 146487830, + 0, + Location::AltFrame::ABSOLUTE +}; + /* report SITL state via MAVLink SIMSTATE*/ void SIM::simstate_send(mavlink_channel_t chan) const { @@ -649,8 +657,100 @@ float SIM::get_rangefinder(uint8_t instance) { return -1; }; -} // namespace SITL +float SIM::measure_distance_at_angle_bf(const Location &location, float angle) const +{ + // should we populate state.rangefinder_m[...] from this? + Vector2f vehicle_pos_cm; + if (!location.get_vector_xy_from_origin_NE(vehicle_pos_cm)) { + // should probably use SITL variables... + return 0.0f; + } + static uint64_t count = 0; + + if (count == 0) { + unlink("/tmp/rayfile.scr"); + unlink("/tmp/intersectionsfile.scr"); + } + count++; + + // the 1000 here is so the files don't grow unbounded + const bool write_debug_files = count < 1000; + + FILE *rayfile = nullptr; + if (write_debug_files) { + rayfile = fopen("/tmp/rayfile.scr", "a"); + } + // cast a ray from location out 200m... + Location location2 = location; + location2.offset_bearing(wrap_180(angle + state.yawDeg), 200); + Vector2f ray_endpos_cm; + if (!location2.get_vector_xy_from_origin_NE(ray_endpos_cm)) { + // should probably use SITL variables... + return 0.0f; + } + if (rayfile != nullptr) { + ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7); + fclose(rayfile); + } + + // setup a grid of posts + FILE *postfile = nullptr; + FILE *intersectionsfile = nullptr; + if (write_debug_files) { + static bool postfile_written; + if (!postfile_written) { + ::fprintf(stderr, "Writing /tmp/post-locations.scr\n"); + postfile_written = true; + postfile = fopen("/tmp/post-locations.scr", "w"); + } + intersectionsfile = fopen("/tmp/intersections.scr", "a"); + } + const float radius_cm = 100.0f; + float min_dist_cm = 1000000.0; + const uint8_t num_post_offset = 10; + for (int8_t x=-num_post_offset; x Date: Fri, 22 Oct 2021 08:42:55 +1100 Subject: [PATCH 0702/3101] Rover: use rangefinder distance() rather than distance_cm --- Rover/GCS_Mavlink.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6ca3f6b11b4..af76d74b8f2 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -149,7 +149,7 @@ int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const void GCS_MAVLINK_Rover::send_rangefinder() const { - float distance_cm; + float distance; float voltage; bool got_one = false; @@ -160,8 +160,8 @@ void GCS_MAVLINK_Rover::send_rangefinder() const continue; } if (!got_one || - s->distance_cm() < distance_cm) { - distance_cm = s->distance_cm(); + s->distance() < distance) { + distance = s->distance(); voltage = s->voltage_mv(); got_one = true; } @@ -173,7 +173,7 @@ void GCS_MAVLINK_Rover::send_rangefinder() const mavlink_msg_rangefinder_send( chan, - distance_cm * 0.01f, + distance, voltage); } From 93b55e367e4be5cffa76991bd0340ee1fe6c3109 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Apr 2022 10:25:16 +1000 Subject: [PATCH 0703/3101] AP_Logger: fixed log listing with gap, and EKF error on log list this fixes two issues: The first issue that if we are missing a log file in the middle of the list then it was not possible to download recent logs, as we get the incorrect value for total number of logs. This happened for me with 107 logs, with log62 missing from the microSD. It would only show 45 available logs, so the most recent logs could not be downloaded. The second issue is that get_num_logs() was very slow if there were a lot of log files in a directory. This would cause EKF errors and ESC resets. Using a opendir/readdir loop is much faster (approx 10x faster in my testing with 107 logs on a MatekH743). --- libraries/AP_Logger/AP_Logger_File.cpp | 77 +++++++++++++++++--------- libraries/AP_Logger/AP_Logger_File.h | 2 + 2 files changed, 52 insertions(+), 27 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 3249cd1373f..6e8cfa17dfa 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -201,6 +201,29 @@ int64_t AP_Logger_File::disk_space() return AP::FS().disk_space(_log_directory); } +/* + convert a dirent to a log number + */ +bool AP_Logger_File::dirent_to_log_num(const dirent *de, uint16_t &log_num) const +{ + uint8_t length = strlen(de->d_name); + if (length < 5) { + return false; + } + if (strncmp(&de->d_name[length-4], ".BIN", 4) != 0) { + // doesn't end in .BIN + return false; + } + + uint16_t thisnum = strtoul(de->d_name, nullptr, 10); + if (thisnum > MAX_LOG_FILES) { + return false; + } + log_num = thisnum; + return true; +} + + // find_oldest_log - find oldest log in _log_directory // returns 0 if no log was found uint16_t AP_Logger_File::find_oldest_log() @@ -230,19 +253,9 @@ uint16_t AP_Logger_File::find_oldest_log() EXPECT_DELAY_MS(3000); for (struct dirent *de=AP::FS().readdir(d); de; de=AP::FS().readdir(d)) { EXPECT_DELAY_MS(3000); - uint8_t length = strlen(de->d_name); - if (length < 5) { - // not long enough for \d+[.]BIN - continue; - } - if (strncmp(&de->d_name[length-4], ".BIN", 4) != 0) { - // doesn't end in .BIN - continue; - } - - uint16_t thisnum = strtoul(de->d_name, nullptr, 10); - if (thisnum > MAX_LOG_FILES) { - // ignore files above our official maximum... + uint16_t thisnum; + if (!dirent_to_log_num(de, thisnum)) { + // not a log filename continue; } if (current_oldest_log == 0) { @@ -661,29 +674,39 @@ void AP_Logger_File::get_log_info(const uint16_t list_entry, uint32_t &size, uin } - /* get the number of logs - note that the log numbers must be consecutive */ uint16_t AP_Logger_File::get_num_logs() { - uint16_t ret = 0; + auto *d = AP::FS().opendir(_log_directory); + if (d == nullptr) { + return 0; + } uint16_t high = find_last_log(); - uint16_t i; - for (i=high; i>0; i--) { - if (! log_exists(i)) { - break; + uint16_t ret = high; + uint16_t smallest = high; + uint16_t smallest_above_last = 0; + + EXPECT_DELAY_MS(2000); + for (struct dirent *de=AP::FS().readdir(d); de; de=AP::FS().readdir(d)) { + EXPECT_DELAY_MS(100); + uint16_t thisnum; + if (!dirent_to_log_num(de, thisnum)) { + // not a log filename + continue; } - ret++; - } - if (i == 0) { - for (i=MAX_LOG_FILES; i>high; i--) { - if (! log_exists(i)) { - break; - } - ret++; + smallest = MIN(smallest, thisnum); + if (thisnum > high && (smallest_above_last != 0 || thisnum < smallest_above_last)) { + smallest_above_last = thisnum; } } + AP::FS().closedir(d); + if (smallest_above_last != 0) { + // we have wrapped, add in the logs with high numbers + ret += (MAX_LOG_FILES - smallest_above_last) + 1; + } + return ret; } diff --git a/libraries/AP_Logger/AP_Logger_File.h b/libraries/AP_Logger/AP_Logger_File.h index 5f76f4fb925..7d1f6496c23 100644 --- a/libraries/AP_Logger/AP_Logger_File.h +++ b/libraries/AP_Logger/AP_Logger_File.h @@ -100,6 +100,8 @@ class AP_Logger_File : public AP_Logger_Backend bool file_exists(const char *filename) const; bool log_exists(const uint16_t lognum) const; + bool dirent_to_log_num(const dirent *de, uint16_t &log_num) const; + // write buffer ByteBuffer _writebuf{0}; const uint16_t _writebuf_chunk = HAL_LOGGER_WRITE_CHUNK_SIZE; From ed1a5c086b6750567e8b18d619c64b831521864b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Apr 2022 21:18:03 +1000 Subject: [PATCH 0704/3101] AP_Logger: removed annoying message on missing logs --- libraries/AP_Logger/AP_Logger_File.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 6e8cfa17dfa..e8818e299e2 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -542,9 +542,6 @@ uint32_t AP_Logger_File::_get_log_size(const uint16_t log_num) struct stat st; EXPECT_DELAY_MS(3000); if (AP::FS().stat(fname, &st) != 0) { - if (_open_error_ms == 0) { - printf("Unable to fetch Log File Size (%s): %s\n", fname, strerror(errno)); - } free(fname); return 0; } From 0d01ac389c3f7569a641b27d6b0200087401f3c9 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Mon, 11 Apr 2022 13:15:17 -0500 Subject: [PATCH 0705/3101] Plane: add Q_OPTION for RTL always on RC failsafe for ship landing --- ArduPlane/events.cpp | 8 ++++++-- ArduPlane/quadplane.cpp | 4 ++-- ArduPlane/quadplane.h | 1 + 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 090c8364f1a..134e2a25014 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -54,7 +54,9 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso case Mode::Number::QAUTOTUNE: #endif case Mode::Number::QACRO: - if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { + if (quadplane.options & QuadPlane::OPTION_FS_RTL) { + set_mode(mode_rtl, reason); + } else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { set_mode(mode_qrtl, reason); } else { set_mode(mode_qland, reason); @@ -147,7 +149,9 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason #if QAUTOTUNE_ENABLED case Mode::Number::QAUTOTUNE: #endif - if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { + if (quadplane.options & QuadPlane::OPTION_FS_RTL) { + set_mode(mode_rtl, reason); + } else if (quadplane.options & QuadPlane::OPTION_FS_QRTL) { set_mode(mode_qrtl, reason); } else { set_mode(mode_qland, reason); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c638730acfe..d370fcdc370 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -256,8 +256,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: OPTIONS // @DisplayName: quadplane options - // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. - // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL + // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Use FW Approach:Use a fixed wing approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. Force RTL mode: forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL). + // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Use FW Approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL, 20: Force RTL mode on VTOL failsafes overriding bit 5(USE QRTL) AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 9bd0fa9e27c..64182b897e2 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -524,6 +524,7 @@ class QuadPlane OPTION_REPOSITION_LANDING=(1<<17), OPTION_ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18), OPTION_TRANS_FAIL_TO_FW=(1<<19), + OPTION_FS_RTL=(1<<20), }; AP_Float takeoff_failure_scalar; From c523aa3460b381992848aa296251486d943f3af0 Mon Sep 17 00:00:00 2001 From: Lee Yong Ler Date: Tue, 19 Apr 2022 15:10:30 +0800 Subject: [PATCH 0706/3101] Tools: Update GIT_Success.txt --- Tools/GIT_Test/GIT_Success.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 872410cc003..9ece1cf40ea 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -163,4 +163,5 @@ xianglunkai Mike Lyons MiloÅ¡ PetraÅ¡inović (mpetrasinovic) Takeshi Yamada -Diego Borrajo \ No newline at end of file +Diego Borrajo +Lee Yong Ler (Retry) From e73f868fc0e32b9c733969ea76ecc6eef98117f7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Apr 2022 19:33:23 +1000 Subject: [PATCH 0707/3101] HAL_ChibiOS: disable fatal exceptions for DMA errors this zeros-watchdog was caused by a SPI DMA error on STM32F405: https://discuss.ardupilot.org/t/crash-with-4-2-0-beta-and-4-3-0-daily-bdshot/83297 we had incorrectly left these internal errors enabled when asserts were not enabled. That led to a osalSysHalt() without these we get an spi_fail internal error, caught by the SPIDevice code --- libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h | 6 ++++++ libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h | 6 +++--- libraries/AP_HAL_ChibiOS/hwdef/common/stm32f3_mcuconf.h | 6 +++--- libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h | 8 ++++---- libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h | 6 +++--- libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h | 6 +++--- libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h | 6 +++--- 7 files changed, 25 insertions(+), 19 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h index da2b77a23c5..50ac6afe44d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h @@ -37,6 +37,12 @@ // include generated config #include "hwdef.h" +#ifdef HAL_CHIBIOS_ENABLE_ASSERTS +#define STM32_DMA_ERROR_HOOK(devp) osalSysHalt("DMA failure") +#else +#define STM32_DMA_ERROR_HOOK(devp) do {} while(0) +#endif + #if defined(STM32F1) #include "stm32f1_mcuconf.h" #elif defined(STM32F3) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h index c9f8b039103..535f09edbd3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f1_mcuconf.h @@ -113,7 +113,7 @@ #define STM32_I2C_I2C2_IRQ_PRIORITY 5 #define STM32_I2C_I2C1_DMA_PRIORITY 3 #define STM32_I2C_I2C2_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * ICU driver system settings. @@ -154,7 +154,7 @@ #define STM32_SPI_SPI2_DMA_PRIORITY 1 #define STM32_SPI_SPI1_IRQ_PRIORITY 10 #define STM32_SPI_SPI2_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -172,7 +172,7 @@ #define STM32_UART_USART1_DMA_PRIORITY 0 #define STM32_UART_USART2_DMA_PRIORITY 0 #define STM32_UART_USART3_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) /* * WDG driver system settings. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f3_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f3_mcuconf.h index aed727582a1..92d28e121e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f3_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f3_mcuconf.h @@ -127,7 +127,7 @@ #define STM32_I2C_I2C2_IRQ_PRIORITY 5 #define STM32_I2C_I2C1_DMA_PRIORITY 3 #define STM32_I2C_I2C2_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * ICU driver system settings. @@ -168,7 +168,7 @@ #define STM32_SPI_SPI2_DMA_PRIORITY 1 #define STM32_SPI_SPI1_IRQ_PRIORITY 10 #define STM32_SPI_SPI2_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -186,7 +186,7 @@ #define STM32_UART_USART1_DMA_PRIORITY 0 #define STM32_UART_USART2_DMA_PRIORITY 0 #define STM32_UART_USART3_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) /* * WDG driver system settings. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h index 1044902f55f..e49257d3602 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h @@ -389,7 +389,7 @@ #define STM32_I2C_I2C1_DMA_PRIORITY 3 #define STM32_I2C_I2C2_DMA_PRIORITY 3 #define STM32_I2C_I2C3_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * I2S driver system settings. @@ -398,7 +398,7 @@ #define STM32_I2S_SPI3_IRQ_PRIORITY 10 #define STM32_I2S_SPI2_DMA_PRIORITY 1 #define STM32_I2S_SPI3_DMA_PRIORITY 1 -#define STM32_I2S_DMA_ERROR_HOOK(i2sp) osalSysHalt("DMA failure") +#define STM32_I2S_DMA_ERROR_HOOK(i2sp) STM32_DMA_ERROR_HOOK(i2sp) /* * ICU driver system settings. @@ -485,7 +485,7 @@ #define STM32_SPI_SPI2_IRQ_PRIORITY 10 #define STM32_SPI_SPI3_IRQ_PRIORITY 10 #define STM32_SPI_SPI4_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -523,7 +523,7 @@ #define STM32_UART_UART4_DMA_PRIORITY 0 #define STM32_UART_UART5_DMA_PRIORITY 0 #define STM32_UART_USART6_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) #define STM32_IRQ_UART1_PRIORITY 12 #define STM32_IRQ_UART2_PRIORITY 12 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h index 7b95516834b..7cd46d1ac22 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h @@ -239,7 +239,7 @@ #define STM32_I2C_I2C2_DMA_PRIORITY 3 #define STM32_I2C_I2C3_DMA_PRIORITY 3 #define STM32_I2C_I2C4_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * SPI driver system settings. @@ -252,7 +252,7 @@ #define STM32_SPI_SPI2_IRQ_PRIORITY 10 #define STM32_SPI_SPI3_IRQ_PRIORITY 10 #define STM32_SPI_SPI4_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -276,7 +276,7 @@ #define STM32_UART_USART3_DMA_PRIORITY 0 #define STM32_UART_UART4_DMA_PRIORITY 0 #define STM32_UART_UART5_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) /* * USB driver system settings. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 7b2a749335d..97ca23092c0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -431,7 +431,7 @@ #define STM32_I2C_I2C2_DMA_PRIORITY 3 #define STM32_I2C_I2C3_DMA_PRIORITY 3 #define STM32_I2C_I2C4_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * ICU driver system settings. @@ -538,7 +538,7 @@ #define STM32_SPI_SPI4_IRQ_PRIORITY 10 #define STM32_SPI_SPI5_IRQ_PRIORITY 10 #define STM32_SPI_SPI6_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -565,7 +565,7 @@ #define STM32_UART_USART6_DMA_PRIORITY 0 #define STM32_UART_UART7_DMA_PRIORITY 0 #define STM32_UART_UART8_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) #define STM32_IRQ_LPUART1_PRIORITY 12 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h index 3909d8a5528..73dea046289 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h @@ -220,7 +220,7 @@ #define STM32_I2C_I2C2_DMA_PRIORITY 3 #define STM32_I2C_I2C3_DMA_PRIORITY 3 #define STM32_I2C_I2C4_DMA_PRIORITY 3 -#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) /* * PWM driver system settings. @@ -266,7 +266,7 @@ #define STM32_SPI_SPI1_IRQ_PRIORITY 10 #define STM32_SPI_SPI2_IRQ_PRIORITY 10 #define STM32_SPI_SPI3_IRQ_PRIORITY 10 -#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) /* * ST driver system settings. @@ -294,7 +294,7 @@ #define STM32_UART_USART3_DMA_PRIORITY 0 #define STM32_UART_UART4_DMA_PRIORITY 0 #define STM32_UART_UART5_DMA_PRIORITY 0 -#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) /* * USB driver system settings. From eaa62fb490c24176a702078587369c0a5e386980 Mon Sep 17 00:00:00 2001 From: yuri-rage Date: Tue, 12 Apr 2022 09:12:04 -0500 Subject: [PATCH 0708/3101] AP_HAL: remove BRD_PWM_COUNT reference --- libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat index a3ba4e1bab2..9ed4d9bf5ac 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat @@ -165,9 +165,9 @@ PD12 USART3_RTS USART3 PD10 FRAM_CS CS SPEED_VERYLOW # Now we start defining some PWM pins. We also map these pins to GPIO -# values, so users can set BRD_PWM_COUNT to choose how many of the PWM -# outputs on the primary MCU are setup as PWM and how many as -# GPIOs. To match HAL_PX4 we number the GPIOs for the PWM outputs +# values, so users can set SERVOx_FUNCTION=-1 to determine which +# PWM outputs on the primary MCU are set up as GPIOs. +# To match HAL_PX4 we number the GPIOs for the PWM outputs # starting at 50. PE14 TIM1_CH4 TIM1 PWM(1) GPIO(50) PE13 TIM1_CH3 TIM1 PWM(2) GPIO(51) From 51fa26c50569994488a00c98fcc7c73be6550f8e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 Apr 2022 09:22:21 +1000 Subject: [PATCH 0709/3101] Tools: added options to board_list.py make it easier to rebuild all bootloaders for AP_Periph with: ./Tools/scripts/board_list.py AP_Periph --per-line | xargs -i ./Tools/scripts/build_bootloaders.py '{}' --- Tools/scripts/board_list.py | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index d6a20ca33fa..5919519b509 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -181,9 +181,21 @@ def find_ap_periph_boards(self): AP_PERIPH_BOARDS = BoardList().find_ap_periph_boards() if __name__ == '__main__': - import sys - if len(sys.argv) < 2: - print("Usage: board_list.py TARGET") - sys.exit(1) + import argparse + parser = argparse.ArgumentParser(description='list boards to build') + + parser.add_argument('target') + parser.add_argument('--per-line', action='store_true', default=False, help='list one per line for use with xargs') + args = parser.parse_args() board_list = BoardList() - print(sorted(board_list.find_autobuild_boards(sys.argv[1]))) + target = args.target + if target == "AP_Periph": + blist = board_list.find_ap_periph_boards() + else: + blist = board_list.find_autobuild_boards(target) + blist = sorted(blist) + if args.per_line: + for b in blist: + print(b) + else: + print(blist) From 05bda89516bfdc870adb22f96cbd7367e506eb76 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 11 Apr 2022 09:23:18 +1000 Subject: [PATCH 0710/3101] Tools: rebuild peripheral bootloaders get longer timeout to make updating firmware remotely using CAN over mavlink possible over slow links. This helps with supporting users remotely --- Tools/bootloaders/ARK_GPS_bl.bin | Bin 16516 -> 16540 bytes Tools/bootloaders/ARK_GPS_bl.hex | 2051 +++++------ Tools/bootloaders/BirdCANdy_bl.bin | Bin 15996 -> 16524 bytes Tools/bootloaders/BirdCANdy_bl.elf | Bin 342304 -> 343776 bytes Tools/bootloaders/BirdCANdy_bl.hex | 1867 +++++----- Tools/bootloaders/CUAV_GPS_bl.bin | Bin 16596 -> 16628 bytes Tools/bootloaders/CUAV_GPS_bl.elf | Bin 393328 -> 344004 bytes Tools/bootloaders/CUAV_GPS_bl.hex | 2064 +++++------ Tools/bootloaders/FreeflyRTK_bl.bin | Bin 24244 -> 24100 bytes Tools/bootloaders/FreeflyRTK_bl.elf | Bin 495200 -> 415472 bytes Tools/bootloaders/FreeflyRTK_bl.hex | 3021 ++++++++------- Tools/bootloaders/HitecMosaic_bl.bin | Bin 17548 -> 15632 bytes Tools/bootloaders/HitecMosaic_bl.elf | Bin 376284 -> 329120 bytes Tools/bootloaders/HitecMosaic_bl.hex | 2074 +++++------ Tools/bootloaders/HolybroG4_GPS_bl.bin | Bin 20988 -> 21572 bytes Tools/bootloaders/HolybroG4_GPS_bl.elf | Bin 334076 -> 334820 bytes Tools/bootloaders/HolybroG4_GPS_bl.hex | 2029 +++++----- Tools/bootloaders/HolybroGPS_bl.bin | Bin 21532 -> 22132 bytes Tools/bootloaders/HolybroGPS_bl.elf | Bin 364476 -> 328288 bytes Tools/bootloaders/HolybroGPS_bl.hex | 2730 +++++++------- Tools/bootloaders/MatekL431-Airspeed_bl.bin | Bin 21284 -> 21324 bytes Tools/bootloaders/MatekL431-Airspeed_bl.hex | 556 +-- Tools/bootloaders/MatekL431-Periph_bl.bin | Bin 21284 -> 21316 bytes Tools/bootloaders/MatekL431-Periph_bl.hex | 556 +-- Tools/bootloaders/Pixracer-periph_bl.bin | Bin 29948 -> 28800 bytes Tools/bootloaders/Pixracer-periph_bl.hex | 3672 +++++++++---------- Tools/bootloaders/Sierra-L431_bl.bin | Bin 20676 -> 20940 bytes Tools/bootloaders/ZubaxGNSS_bl.bin | Bin 16816 -> 16848 bytes Tools/bootloaders/ZubaxGNSS_bl.elf | Bin 365940 -> 263180 bytes Tools/bootloaders/ZubaxGNSS_bl.hex | 2096 +++++------ Tools/bootloaders/f103-ADSB_bl.bin | Bin 16948 -> 15364 bytes Tools/bootloaders/f103-ADSB_bl.elf | Bin 366704 -> 320468 bytes Tools/bootloaders/f103-ADSB_bl.hex | 2021 +++++----- Tools/bootloaders/f103-Airspeed_bl.bin | Bin 16940 -> 15364 bytes Tools/bootloaders/f103-Airspeed_bl.elf | Bin 366704 -> 320464 bytes Tools/bootloaders/f103-Airspeed_bl.hex | 2020 +++++----- Tools/bootloaders/f103-GPS_bl.bin | Bin 16940 -> 15364 bytes Tools/bootloaders/f103-GPS_bl.hex | 2020 +++++----- Tools/bootloaders/f103-QiotekPeriph_bl.bin | Bin 18908 -> 15364 bytes Tools/bootloaders/f103-QiotekPeriph_bl.hex | 2143 +++++------ Tools/bootloaders/f103-RangeFinder_bl.bin | Bin 16948 -> 15372 bytes Tools/bootloaders/f103-RangeFinder_bl.elf | Bin 366712 -> 320476 bytes Tools/bootloaders/f103-RangeFinder_bl.hex | 2021 +++++----- Tools/bootloaders/f303-GPS_bl.bin | Bin 17476 -> 15640 bytes Tools/bootloaders/f303-GPS_bl.hex | 2071 +++++------ Tools/bootloaders/f303-HWESC_bl.bin | Bin 17476 -> 15640 bytes Tools/bootloaders/f303-HWESC_bl.hex | 2071 +++++------ Tools/bootloaders/f303-M10025_bl.bin | Bin 17476 -> 15640 bytes Tools/bootloaders/f303-M10025_bl.hex | 2071 +++++------ Tools/bootloaders/f303-M10070_bl.bin | Bin 17476 -> 15640 bytes Tools/bootloaders/f303-M10070_bl.elf | Bin 376040 -> 329120 bytes Tools/bootloaders/f303-M10070_bl.hex | 2071 +++++------ Tools/bootloaders/f303-MatekGPS_bl.bin | Bin 17476 -> 15640 bytes Tools/bootloaders/f303-MatekGPS_bl.elf | Bin 376044 -> 329120 bytes Tools/bootloaders/f303-MatekGPS_bl.hex | 2071 +++++------ Tools/bootloaders/f303-Universal_bl.bin | Bin 17484 -> 15648 bytes Tools/bootloaders/f303-Universal_bl.elf | Bin 376044 -> 329124 bytes Tools/bootloaders/f303-Universal_bl.hex | 2071 +++++------ Tools/bootloaders/f405-MatekAirspeed_bl.bin | Bin 23376 -> 23876 bytes Tools/bootloaders/f405-MatekAirspeed_bl.hex | 2788 +++++++------- Tools/bootloaders/f405-MatekGPS_bl.bin | Bin 23360 -> 23860 bytes Tools/bootloaders/f405-MatekGPS_bl.hex | 2786 +++++++------- Tools/bootloaders/mRo-M10095_bl.bin | Bin 20384 -> 21316 bytes Tools/bootloaders/mRo-M10095_bl.elf | Bin 331104 -> 333940 bytes Tools/bootloaders/mRo-M10095_bl.hex | 2439 ++++++------ 65 files changed, 26057 insertions(+), 27323 deletions(-) mode change 100644 => 100755 Tools/bootloaders/Pixracer-periph_bl.bin mode change 100644 => 100755 Tools/bootloaders/f103-QiotekPeriph_bl.bin mode change 100644 => 100755 Tools/bootloaders/f405-MatekAirspeed_bl.bin diff --git a/Tools/bootloaders/ARK_GPS_bl.bin b/Tools/bootloaders/ARK_GPS_bl.bin index a2a0c61f4c8989317b5e2b6f91dc20f8fbbbf562..e5e610d13b3b321a9c1d8c701b9692e48d535969 100755 GIT binary patch literal 16540 zcmdseYgkiP*6==;B!>_nT(l5S4~aTJP$SqG6sHYII3UsDrJdJeJ3RqyPpH*Y?F>_U z31FvG+o=|(uR`y1dUM(VEFBq3gJNfFZD$g#Q_$&@j-5uJttSM{Apy?2c7mO@GtcwA z-=FWtr_av5oVC|pd+oK?T6^scQX%%AG-A4EAZ96~ebf0S$orZ9+xN@w z79%DAY1VSYtblY6r0>Igzkc^T#C!s2@qENQ1nK$z&}YW7|1%zpcUEo;F7C(wJNWO@ z^8ZVHU3Wz{%4Iw#C(BbQ%8MbHkhr7!^#G%}Yl!|c5|z>?5U1bY~#4_e- zn)?-pjH`URE&Dm=KxepNbZrL_CWFTVEWw6}fF?{%lnhge`^}ZZS>@T9cOuf2IpIe` zOl0N-XtydOs=z^udLV0rqLcyI8;Ynx zFGrzi8U^G+x2Udv8K7158*OBfd01)wJQK~I)$x%T8QvX2*2P@UMe*T1mbd}50Ew5} zXg;(OCN9=~INW#zEv)eEv5;pPENFDxy-Vi+Itca|4lNk;0_+W+_{nbh?|5D)&n!OS zettwlz)Z?u4pYQ@R?`s)1yzyMWb-glk{F@HCJO^|AqXlU7m=r%0P88Ym5YOU0Hmmj zXp?Qjge@_Grd}$=kkNT8CQrXqibjvR8ScfAMA|$|nV<6iF~F)0oAqJU#L8hx!-iF3 zL|7TjvW{a;u0T-kh>T0D22&CjNsm&%L=u4lGOQepk5a(|X2N=vV~1lr_+h-pI;^z9 z$XA}R&|z{6bAqB^^rA(lpRp`}(Q9n2nUBEQhpwn1GYdgdN|cer;GSvjf}|A9JiW(~ zHb7qIu)U|_h7nifU&!Tf3cr%O@I^W&ePJ(s^>p_L$!r|aBoak!evTXVv)q2ane+L* z+)h7P?64L1Otk`E+?kSPz_^>}~MaH*kx)r*LOnZyvI?o#TYJnvA5qr&w^l!mX z;?02TtMRmzX{&x~q0ZeX(V0zU<9;NTNXaXcztcvECEW-i(Cf$m2_Xg%kVC!B3}tAW z3n>TEfu)EYBoOoM1pB{o!B^Vd$0UunPk5T^`BJ+ZcH85~&=^c^%Hf_XLo6x&a&pQ~ zit&@^(XzqLtv6PrCW!f{HnY8|+#114D zNJxBg0*UucA@KoO!F>R2_sZtX=Rj_dXpn;bS2#5HFMgV<6ba^(-_2Ey-MiyYQ1|+A z=|ex7`w4Jv{v^~~B;-)@5J=SytZJXXEUq2Vqrqw+d7M)liq+g)NqOr%7>8CdE-1xGdVI((ELg?m}Y2REk}<8R>N!Gq&h%&)9PF z=BkZ1Z>hR{!ZKHoH$^R(&*t3hIHgPjihRnc6aK5Aq-oNfweU$JPt46)Gal zaTTe;icu#z*qNz85uz}aSK8AB+vsXXLnqP@5fY{I?oL-33-DQxv|T7$tEYj#>!&C? zb&BO3e7f))kH^s7E6;&-v+7qJZAap{sY8N!^ecUzuGvyBp#0q{yphY%qwFG%5YI&v zfkcGFg;Qug&D|Duk3*dab_USFQX-H;s0Nr3CB^75?1>2R!+@VL5c&^?$O6R@jh$$I zvD0Kg%^wcYI}~kahL9n5r>dL~|1@6Q4szA_yC8DcT`7FF`TY+ca}^@?en>V*uC4?w zjhhWP%g0xDlMA&zB0tfW62$;o$w08G!JK@pSJ6-8$9Wa`NnYsFW6F>C;-fHM`eXUU z>HPEQd=SZi%X;&_q z7j0h@Ar8<1hSKVDvI#LKQpA$&ur7EnRm$yiUe~-~9=#{U%_VW{D=0gcN8-;W%Yg3s7=6(S73gx`d~T2$B|hX}cH>;-G#F6E(g&c1>YmxgEP zO&fDL6ZcDLIV*>nW$huFBREE%13bPMX7taeCj?3;T>C7X5U{|C(g7`j>jUBBLoC$N zqqWoFltX5y6-8^O!fA&vWL>m&0(!2A_H;+qc88;Y3J5N$5RIwH6gwc zK2sVOAmW0)48U+RXn8_>F-%`>+5wt1?n!Bh$zG}+IDBU|XSc;1h>=2n!%q5m1@ui8E{0Wf1FhR_H!H@SCp5u(` z5%V`$3Fd)dcENo2LV5ubgXB;zZ+MSxJm4;0UuZ5u-9Nn~JP-$*bwLgb|{{i;A3&~y;bIgmya&7#ET zC6qJZOIpbE@3+r(4FK-8%QLs*F?{|=mgFKh)C_x&!x*lY#_P;L`7F85e_?lyalTjE zPZ(!=6~?1JY{_@6c*y>~K#mPor&>?(xCMD8{U+mYeKij}xQZyO-Fd#V*)qgYzJEMm z+}UE;gmb4@n=J-j(`I#+~uq7alNfNAqC5amBM3 zeJ)qd{b`7}X_6)J&>^kEJ!o4pPQC?B!X!Pp~+=0lBYo7xH(bK(cWQ0cVj9!j;2M#$!qVb6`*DS+Iz41L~0`f*NT;gdTKV z1mnEq52jlJhy;EWV63>ma)e@lXKo}O47*F?&On<9z9FIzWlr-G$si}l0Q?kR4a=d7 zDW*dXX(x&O>cQsmIOzJ~l-d(#bgy+yE zg*|VJ&P93D_Tjs9d=_^MY+1|ews*0P!nDprF+Ty_j?Ak|o@-wZc{+SsFeI1RSCWRB zyODuxuCSkxQ19E9?|m@kOu4QNh#4eMvW6$xIZA~ULwb)vlMykri= zjrBh*#hb2QVJ@$l{j{Vu5yhFbb(mb&BQY$oEY)fUshmFv7P+~|zH|j@URu5sER%aJ z5`R2dWY4He?pOla=T@*;FNYPdYo3aLme}4N367GHR7!Dqrb`=c6c^glc~W#mn2sca zp`OM%`QbZfAHM9~>N$OE2fTAjgFU0i)|3p0xs}5RTjs3UV!cAA=A_!H5o1}I) zQ?_4rJje_r!YL9%zNh^%~s_->Op!!e=viHDAxQw5(vFry3dF59{0X3 zF{>YgnLQ@O^G*I#?knIc){&TZ*ISa1x$b^K3+1LIh&{+4W;v9xkCLrH4CXzS53DEm zz5MYIx+7_{Ja=FSJ@W9X=Jrn|TjwuV=D?0RJed$Msa1Pjhhd*(uZEUijjLKScESpi zjvz%oCYjjv!a*VNHR^b#kkC>S%p#wbOs0ej>##kSn zRJ9shfA$;dRL14~M4`czK8^jh@lU0b9l}av$l%sSdp$ZqaIT|TJ@X0@SC7{?8^_!= zH4Y^HeLS8^d6ndnhbZtK=4u|jNTH{tCE8;_3O%A+=}A1;BNg>NCgmf}b1aypSt+8D z4}xlf*_XpHuOcx;{=koE;U;^dxeN9M$|?7KDy;<1{PpqkpqCF=%x7Tz$5ne_{llu3 z$0XJqvlis#ncKix1PHJcDPSp10raO*T1Tsdw8`Cs0U%AQIn_QnbReuccr2i$KkroL z=XjKJ_1qd;LLeKo%*PUQh-yv;*(R3cgU9)KCjt77-e1fWfLDRU+zDjZ19*{m`}n+y z`72X}KHXuE|HWst;8*>0oaEx8aF-@9w00uruwmz?(*Aa2O!H6%68b8jZ}~XL+x9ga zNUjJ}qV*ulVmRHPGLg+GFluk`FUUN4Q|gC9)N`oW!r7xZMJvm~QH+laz9FT1Zd@}p;5;OFmxzUv{8V*e2oK>QO3U3QyDal;XW z6n34UuxB>4>KZNNfYwHc6CS``hT(C)qAt}cn#&Pb_!5#q;-J)XvB*q{%#k9%jp39l z_DOS1g&kneYU`oI=;hEU4*^(^=N3F}4(TDWKB#VS;5Iy8IF}fj;f(jeEhlay3djA} z@1;1B^XgK@iABY?1wkw^?!fR}M zZJ&2OB+cc|`{YoojB-BqF@wfhYNUZ`wEV>@hxS3+DIa2cE=r9A8}Z}$B)y4P94A9v zt?ieBarJ^;06BP4z7$k6()N}+&(*ek^S`wgPRC$Nz6 zp2lGf)i9h$&Jblqz(*`k6J`3MZ_wXw^;s$KhLNZFhr_Rqd?s2z7nZ#1oY8&E0b0D? zE*M0ZH*?saEb?5({gyLwRl++XrytadyiM&zdFkg~)VyiA#`m-( z$(O+Y9dz;6{+BH(A1S(R$Aj)|meP3djUAW#%;~TEAAp94=RfqnX(7CCTK?Iu6Q1Hw z-amaAd|xak0dY)1RMC~gXBoXsA*vK;#D^-!9$c~CLin(Kp+%Jn+SUJ6dL38f`3d*O z0HaLK{1wPeBKH}?jGRp@xg}Lf?Cmz^hLxf0usR@jJ~~VgWnpz_sToQd3cfC}U28k* zcMlURV<;KTCW%fnLb>ivh!qwd@vM%n!5Bz1LPcE~?y>aWSdg!%Ck^CU4t*|%@Hz?= z9jOAv_calZ5QVLs2Q7W>A`exsX1uBLW4dvOVzz@cn8w0(-@=wHjBf664czT-|# z!7*=SanuKIT5@rG>{S?CFZ1g%&ZCas0NZ2 zQW~UmNGv3XfA_BP6#?7i|^yD;Q3k`D)>tz?8zHtnk(wA-g6@p%!0Sss%rjq4cv- zVKw&M)X|Ck%*Cw`FA? zjs${u)mkiPP9F`RByV)|88X{_LPG5xKcV;gd!h7Gj^%0a*-A^#4q*CdndxTv-$lQn z`S{ygQU2ig8@Z>k{lg)Y+9NGrt>X59TqB55FX%{YMMfk(=2k5z=vM_apwm8^!n1@q z(kw@{_k2n2!6&OU@B-w}t&@od^Eiky09@&$kk#8Mhmyhb4^FW=4uU--iqgVvkrL-n zHRAOYwGFj`Z(8Eui@0o{FKddJ`;i~Pc?NTi`C7ZIQgo|1&c>xf)Kd<8BrztE1u8-R9?aMP@pZz~+sWh*-Zw0HXXMc7DLM4!6pYQH){@zzIES4Utlki=4^#}3YM{bWFEP*Uk+kTPM6)+@ML^F-lf;rYEOR@XAgTuxsgL>| zf;j0(Kj^$uBqZX8Z*~63zf~mYUjQGat{GfnG=@cUbD;Of6Ux>azUwlYqiTJbU*f5a ztGps_GgQ;S#sBU^1~{3)DS_gD&`{IR)*$sLd7{w7u|FIITvT0Vx4jb?PkIO(6F{gA z+oMqBC(`o0J(6j4!ZS}xMdlvK>~e!Nw{qPe1%wYRTH~9OU)g^Ee1-OI)uOWgYekAG z^DX06a4@z@4uvLJkw6PbGhI4NZ!~Lc!{)j0WqbCZd04@!7MY>-$^e>&+ujYZ=r;kD z%nK_5@nJXJIP#YUO;gpQS5T)eK?DQ{QP$TtRz0Eel%5BM--m# zoMUYAAVZ-GBn6~ot4N`7%rSJukNww&+MnP)k;>gkPd_c0*{3BN%x0L3(AI<~HO;G>&RCj<#Qu>VFqTlFJMCr$T zxj0Qfo=Xtcs@I7+Xp^g~oRJB|ywZ}d;}ZmYyArMok3NaJR~Xy^xV-N1)>AKZPt zpxXp@6;E1z4!^TcH#+xN62L2Xa=NtMHWD(Vbk%z;N?9NFOMkI*V=9i?+#<^5W`-l zEIyCuW{i<)Hf2+-sU3d+D)N1R6`7-Dg%>@^99qjcb)ZuxiaODvBi`FpYOnebHS!Tr zZAq4x<)o-0Q%eXaG5N~}wE5=IE;kFmzjxy~A@M0UV^}iy2M*0Y?x$M^jX9n{gR(i^ zcF3Y=P8+Doqz<4bpD118S<9X7bn2Kxv`O`Z9GW$uYME(!ZG=?M0FMn*O}8pr6bD;% zbTieODNxNX`4P~L=eLZ@<=*h)(NevH;Z5M%W-pVU0%up&R6W}Od*!T3c@})10y0dG z8LaqRfxkThE4aq51<0OnAbsC~d>z{Hw^spjxu4BX>Sqqt%*Js~#X; zFt4>(hY6}?#7Y%djaHBX1K=^d2N5Fq3%pM!;Fm$;UoiR40hbRCu=!v2_eE#@x|cLG zMoUM%wBZ52sf)-<1FwddYtDbDJ;!67i{=q|5S70vy+5<=G0@XBu!b~VLq)Y^#`GFC zn-x$(`zTHi-Uj)K9NG|3w3~XEyw3-;c~pO96P%SzpZVFm$2>TepDB1a?dhUB3C>7{lfmAU3=jMkpA0JJSBhl(%&4BhzK(m; zSD!BSS{2P^@q_w!I5{ElWc|?qnM8Ssx(b0S?)*~%6Y*1vh&&Q#b70| zH7$>*xIt`9QS6jnjXPhL zu6vFsKHvGewA`d@eN3{N_QM(LG0EKCC=w92utB+mZxbGqEN;y617%SPD54Pi9j#=f z0kP%&$o=mH%_*HDXXcp2b8gIm!?4EvZq-5K&dT9~DJXY|_q3|yBg$wV^}4UpWZe12 zEDFXh;FeCWpErzh63($umqSOw-yex$Q~igUiW~EGQp=9)KHa$(psod|BMs-Q$3Ysa zgMUhm-1C;dwtL|$Jg0x*eG%}UIiI(aSoRP9`0iuixe~>S+;g2b_vg$)230_(X@L{C zRw70;5-~_DCJL3g=R3!I)Is$=`e6LNW(&X&nteqcy6_i5!oDS*n;_PK42jK=-I#;m zVNyEVzxW5-WUulMK@7y)j=6|=xRcM@d1N=SEO!?8G~>m4jx;n{C|{cp<1GtvEr*s) z!JNQr-SsUl7wN%JJLh~4+aPVinkyh@fB%rez*(P$v(P01IhV`eD=v<8h(af$;^ zgM}bQP~tm*%%qn77R!NwEI3`pK~l65g^K2{!1BhSTF^*pbO-zJ3}VprB?^9?o;?a10VYfGt!?wNIa=9cC5)>;^O?X~7okx=wO32K%lX4^2tIzZDw z3yE^qdJ)?vWswT5Pol{_Ndqa3q(*va!jzSBLNc#CA=%bm@qv#d5}GIL(wai5a?crw z-lhrDqRpeSLceXlS6QGDnXg;ZsF|puMT0CMn%hIp$v6uTY*%9W!rD^OTIC#U&z1^i$E6 zMYzSCT_V!N8E1S4Q+@z$c=2t#vK+^jzlPW=uJv4G%!GJNH}c?d_ChM^N?F-f`-$+H zD#csn)Qcopa(<7@6rjFS{+|go7#b*nC}Fe z+*W(gX@C~0JdVCf*|SGt^VN1GuEaMNe;H4(w^heyr?N=w7?00RgZy>KXF&cc$@T*lE!_9U`LwXgmg!ZF=CI0bwS->Bx7Ah&r&X%6 z)o}BdA`Ccmyt1Ooq3t2E;mTf|Jpp$*QU2pK-|#Sjs9atDrxyv4R41i1*0u_%;^1v* z5Ff*Fw#M26LT}K`UoG{1Q$qNhF@2_{a_7Y`%*$QSSbI=779VPJRa>_U;T>V)PyIMQ zx&3@J_iq2#Pvok-WNwnTzPqvZobZH-@TEJcK^)6#tnC-H;9;jnBYTata4LdUZP!wO z91tE+W%@FmPpN1h@F8~w;0XKbYkgAhVxM%P#3!93v<{#20kPV_vW$aqx>eWs%AICW zi&6$LTsy$M32;ZEaBoN9PVy%Ow~C6wk*M6!7c?V?9TA8y50@lsf5D0-L|BvWene^a z>=Tk7InQiJSm}YNIZ*?#>C|78d-m}ggA|o|cja)J0g`RKC%Hb=b));YezM|>U=@3* zGr(_TLuT4uC1|ZJ!g^lakv>aX2CbeqK_+k+n0vdCkL|zMmrtc$cbd3}TI$1lvPhYFHd~oI-7637K?txUE6*_?cPHe^D-Uij zHjhmAzgIcqYz@H2@ceZs70#q_zQWDL9X}Aws_SVjd%Z>uy((XthX1RyQGZ5=@6bB& zUJy&*RN*nePEQk=Bn`xI-Rx(e3(X>nUWn#qRD<&|H&Fgd`9M0irXh~$~9isKUL(0Nl zIP+NXia;ERHPG-LCb@|;P_7bQ4i!w%27@ccXKfE&mpL5v6K8TUZUIilP^T~^cypt^ zj%~`^!9w&u7cBfwr+^oLiy2%BXJI5Ri=do~-WV=?`is3^`aVAGckpfd7QS8I!P^z3tB95J$-`Jfo;)K zVZ<^$_>E}7kpcf%L(f8%Fxn>zBM_w911M5Z{t zyCiz|UJCsK(<@z<8H;(~b2pqJ_Ojf`GNN@4VE9Tm+lDdy%^#a{pCj3ZS{tHVAYlHk zG6`(Lzhv!Y;boW!o=1(dDy!Nt%k|MT7cK}j|C&ow=B~~JMQ8Fbj3U8Z2XGViwzdoN z^WJax+2F{kqnt5rHb%L5d7S06)7<>u&i~TXzvln$`2XGfNx>>4TM3@(NXI%5rJ3ZA z6>eC+}r5OZ`=*M)UpuUT@|ND3q_6#SpBV8aNGPRCRcMt$S8L}P4G?}{WS@EkLZl^h| z*XUGN5O7nQ2{M51322DS;Ckh_(YfBK0xrm*0a^0nei@4}KQyaK97boo&;s+JIjTQ4 z_dT9;=>Min81fwiXV}5Q%bOzeWGMb9^5DKS+yQ1f_X@4>dT2jb4Q%akyS9s2X%JY5 zqUre?c|vb+=yu4Vy)wbEPV75PhKB!u;g~Kt^og9_fxS0Ix-83*1LqB~3o*v`WkK zE$~~Eud1+0S^38WtE9~bD6K6Ajmk=1ssaPVjDkmx*yZCqC;u70H@PIN>X_%Ps3LgevuEc zO3X(J_^JZ!wS2iw?>p;;BVneLGGTN8uKgcR3MG)8&c>2 z&tfd!subuGrODNmbwe*iSrpNCTU#Q9+&U?3Q42&hP1WxX5p(l+b$-4FWvESm98%oW zbI}FdUvYGCtyOm+y~vZrH4ANUC(L9Zab;w?vqji%(SR)fmN)XX{0zPYeD##fmgp`B zI$S-O!i%~uqq11kmASIUc>tm$SN8m7APM422D~uL;L=yDc;NQ*DN7{Gtf&&Kl=gN#nl6!^L1~clsps zncv~;u*7*&ZJ9dkPpOUhp5Amd-29*VL)1@*@)heaEZcZ(VylM`el>=D%JbC3N0|R; zIca9P;_R7q+q>b7fUL6XoG8!Zfs+DH2AT@0c-U2NBd+6_p8g`xMF4Lg?kHut;-OD7 z^!ckC2Yuk=VbM8sf(2gF=wf}MIn=+?SCu1n&Ft5#)T}Vn?d;a9O5_*=*v0CbBzh+& z(MF;e-vckRUGWj%MXixo1h=*9gnt6Y{u?A0>@~u^4%*?q5y}kAQI&&FLc{=Q0@z!B zmX+{*t|{GiTo>O>uw(ud@B&01U3>-3JGhT32TN*BzZ{wmcRW9XTc!u(l!~C?41WQg z@3fAg?N*VbQXQ#I>zKPR)uD*`Y1IJt9LyDPYmrzyo&$1!AG9UHEoV%B9R;0)-x^>Z z;c-$Rs)W~D0lq*QXNNFhA-t1!^cR38hZ8k*i+6D)=%GnDbe$YDM0kjM$%{GjD3L?u zr}xi=+o9YWG`72R(31YVrO>GuAyj!j1N1C}U3DFd?VTjx#$T+bm($mN?mwjZ+*j;0 zjG4C=JBppwVTLMpz&rgr7e?O-;&--2;Tg+CFTenfV!)9FINq29&iZk$LPue=SD_=m zso0Gr1+ILju5Vf_k;R_Qqn5Y5h0&gcj?ci;I}VZsPcU)30L#z{kchgRj!ck1;Sp-k35=+_!Vn}G-08~H-t)v z#cOyvgW^g82*1qTR|uThFi9L_ITCJ)fxqy#01TuUF59FwifipOuEdIjf5U|_QTWwv z6xX$Y3%^H3@CtAubYe1!bsDF(i?k{2r#tt84%7D7vQQ_MHT<^_gPmg_KdW<)ImvG7 z%Gx@YR(+JU)iwsPqC29wE$a?7*ddUa?FOE-G1FK~Q7o_y8>|w$I*JAJ%mwe>0GFzZ z&=apB(6Gm4c&puht3*feJ8n5NDnDmYcw=9Q{rC)qUtQz9tg@Gm-UPdg)K~FlxVviM zS6=zR0(Co_`tfPpgv@C$9}0G8W(Q@jxbm7FpJ{L#{zicv)pb)9r==Y^xLq4jgP9B$M!Nzc*-x^~7+v?bGQtG~EVoOBxj`D(=K_r-~Y(uo3=ZC1!&F z&m}a-M*Yj`w>0*IiUA>eY|s%$$7$5obNm@;f{j~YiH-xt}*mdf4Hx z*7!{d6;>cE8oX-j673s=)G<_(<_*A2FO0rRSPqnLzu<_opTE;^`z{z?4qc3B!H%^- zyhCA+__0L)`KL%#gnHKV?P?{(F?G3FAPb`v}lrMf&wMItQsg2YijyaS7{Rw{?9@tZjX* ztIRG~`8`4-e@f`%T5hP~p0a?y186qIkAF24(9*L2wf-B5mIx_lVQ5S5AI zd0oc47m5E3ajRHdkTmbbI=2gn*TDPKO_7)<_3=9huN8_#q6iYl8}Ul&ZcnJl+|ms3 z3xa9}`|GwY*0)rHB`P>qjr1*!s+x0r75Hma4hGKLS+EBd9*K^_(t`kPVY-r?5H&Cl zbiQY8RV6!AXRmrM<U5{hf$g5oSp_~2%C2(W?5J|; zYO6)CQPsLyc&QW3t7_L*SJmDO2}jbPElNM+x9hgV!@GGLhtTrrThW-=)qWX0I2*@6 zP3d=J+_6qDfVEf$_J1qHGNZJ^zAm;euh4B=UQsz_E=dMk807`zdsP!>TOe9Uku7|I zy+ue7HDtsGf3+iy6B&F9h8%MjVC%V$nmAjv75A&UBgbB~^$F;i3l*w*jKd~XtPvptlARI;V*++ zWzcimXLwIxeMZ9$g}KuiW|A?XxoCeAXnT4vd1K6H!27u5cv1;d!E~*0royUHAQkHz zh7JXc9N)28z>y*BmuucZ%L#Kj)-~UPC8uGV0oMBs?3JGX1WRAV3upD!(TEeqJNO@A znX7mK*82_Y`~MMEdlfIln^)IfrT5KmV6U-jyQ-X7LYkQ#tJ?Sk#nfi_w)sF=8`KmP%~10~4WBmTP<_;|i19`D zt#R(NR5_{v9@g(J=zZ6LG6U3LRH%8OHb~QFlPXfTz_%?2%ASWBj0!a`)T*JzRHSZ& zZ(E@s)L>Mod7%~?mHTFH%50{Kfe6g4Q_5@=I~BSaat897AdiE54diObS3!OaFQ@l#A7oh;w!QAve)m&Z#n_15ngZ_QQkO=ymi|h z3mw}w-s5s^-nwI9$y)nuWvkZ#nKx`@9WEEU-Fb%^SsWYL+keh(-n8Y89RfS=o>)uu zR_J$|b0ajc+l8(7Y~ILLZ~Epdz;E2Ny=I%!wPWkH?{~$Rt!u5UscxRBsBT#-`)*B* z%C>HWVV&FV`nhAW2c%VyZiZ9}X)&Z(knY0C;oO2@ zof|iA!ue{@={%h4uzyBC{bm!KXYoJt_Y%Ti$#2qgXm1`=&};B-H9~V`c-xMC4ry;3 zhEz;{`wf)gAt|nX_aE+uTi|a8|Lx!Z>u>653aD>U^4u}`K|JF%-?XWsrH#{dGrYH- L$|KNe8lnFIG{Da1 literal 16516 zcmdsedwf$>*62QuBqwRwrVlVp3r%whwoS!AQOmUS|iXkTFMrm}@2?X8#1lD8}<%D6`Q1(^P-M&+9*Z_sep} z9K?JDDRC}hW*1~?*Gqq zzcnE%V11r5&OTw)+i$Vea9+sk%C=jntZlJ&uQemqegg8kvc;VhohYC2p}Z_#r6^B_ zWI^JCo%=(K_TF>!Uy-PkzLe;`FVB%I-C9?6%{j5`2`-VNIc5(sFA8k2UB%to+a75c zI}}363I4x^j9Ma+bio+Wj*^k2@ce#-7DbYVEBmjin4Bcdj%`?aHA;XqOK$P1GRF)D$Zv?7u;qykPFBeXPIJ0(I7 zm-kc5ThYN#Wxu|ZjHWm+Ev5Z*DG}Ae_?X*jpn(X<7LT|E=4Tyz%tOw)(J^4ij%>aF zG-FC)6r)%VOfhh@cok55Z!cMlA+7yH$y@jS{Rka%`9_8Tm(sR;x+qk~(j+XX!9~>Z)+;~lkzYXSb zUj(h(_Fz4CC`fSYf>>%wgAB(64 z>mVW+-a3fTCy;KwIP<=Ei*Km%Y2VPQr}3M^gCe}yJPv#LO{Iv?S3l@9H(#7P-bZQl z45e?zy(sa%5lUPH>A8_g@z<8@d6^a4ZF7Ak5}gnUX3fj^NzE|NP?70uvi-`p2;SNt zNkL@1YM%ZlFqC*L;Cg=~eSZ3a*KO3PB@&&vxojke#1biGe#+0&S#6m)d zK?LMb=dDnNwp>VgkhaZ2#y$cuKTV(uDz7=O+j2zG`i}{_xvuYZTVMsAMr`9y%H}+7 zZy7R@;-i$mvvjtfD8kQtXPzjNO}65| zosrEV5*J0LakfF-S|swLOdHCb${}$y;&cg#&7(+sdJKuXWCi!a9v2-VC)Yl>L z*|9XIVFfZ7mSwCo+?}!V))iIDZe3Y*_iZazznxf-r4k={iu3&O`&J?;fL^)W=5N#p{K=JRVQ?pqwpsWY2%M6^Ws-7U7s- z#BAwR?q|8Ze4YvA++@I4F|x3eoR$bGz|Of6Zx`plFs{v=*y zshFkM^PA!h8muD48DVZHhY)k3rr)s~RgmYSOcY2)9!Hvt&^{qr4ecsuS3$c1+9{I> z+SSn)f6}guwNHWeOOZT|4l$I@ln1qokz!+U7v6VrYxW!3{no+XrFytzuohtV#HTNn z$5sG|TP`8dH1=C3Nb%}c(D!=Ydsc-|?k0E=Y+~pZWL~>stsK_Ki`Y$4jzzgo&n=j@ zNitd~F)1>^l(%d;m&a|A<{IVjGqP?U><^AHS%AwO5ytd_J~326;o2LK#E^|*Oms*` z;QD_@QuZ04rjOPB5J}xJXtG3S#L~Z~yzf3jdNXbySdhWz6fqgG z0+uz>hE}ZGuuNQ*fkN0T-~S3Rgi~LKZAUTG#f}-yto55(XSPjuCYcQNDA&j-Kz659 z=!8WAxsN)=u!#c{X`WYDPKmQe6r81Vs|2A~WO9s>mtV=A_gwGG)N-92oTnCs&XTGNhw++34z$kK9Q;+hX3{~N&XBngg`o=pVaJfi|!dTC^x74u0x z9QO%W@qsQm*}Vq!?I zDdL*j+p>0+Vr_}%yW-2m4&-G_D2Cy<{6ny13o)m;{X_Fj7gxP`&&}7ZU+}Inq@^&7 znak(!I304!76nK=G3R{yZqU0Q66#Y)*IhTxF%0pNyny>Y2)4rX0q97?9^fC9zOZ~C z^;nA5Cv<-ENYNTJ1$d~OGl?R@gT;Lb|+v z5ZGZFMHDx5fR0*{6Y~r514d`{!-l%-a`s_r&4;GR^O9hm&qw+|gX0x?EkaBI+t6z1 zqH^@GBNs=XJWHV(q0u%uzzVx;fQiZowD3tzliN_2;`~}L@ZW)-^?p>r32jLgN%PZI z*hRD^ZP|;07Cc5Oz`BV!kC?ycMN_)Lwl(c{ypQ!2rg0)d#-(>7s~+muZ&(QBg(Z7i z#SUkMbAGpk`Zl$aY|VX#EneGs`Q7J+~k+GMeGcSeMdv6>vKX>}Nv+YbsY1YbvaxKvxZtpr=xtSWoGBQdC5l zwq%yA$NC+yZ9&XGtS@h}B_r#)hY1~&Ju{Ipz#wKWl<&|Y2Ky|S?^s5#zWn_wUz|ht zBoE%6-+K-{{`7or>ltZjJERw}6f|7Y?pJNwc>uHscveWc}9A^6`9k06iY&X1@vDs z0h?tO90*Us|YS3 z^M>KGZ=_xEOR#Lsg;WkQHy_ev-!=5XD@XhSlZ<6?0VIS->o0XwT?Z*UUwOW$Nh>sR10V>PcXupf695CxYTvF*b2U_uNZJ+IOV+U zQm&<_4cm97wjRoI_+uY<^5A1F!Qhy7Q^&ffv}72Ia-Dz5R}Y1|p

2OT zfZNQSMw6UV&P}bB!(YqLf)0lmMVBqlcO!RaWK(26DuJCr4|u@mIE>$0UK!A+njCTI zVv?shUUkUr8>QU6p9kddvogx-4lsRFYn20y%2wM)emT4q+TIT!BaT`Sv1fdK$!{Se zj%T^50wwr)qr8`x)%IE3m(S^!+<*bqtnr zRxQ5_dl@G_uM^^!j}XuLEFH`Ojbpnl!Ji@#bJV#@t;F20-#UaSv$e5*AqodAUBzwi{wP4Gq*25KWu`sVFf-Yd70i|3Zl#_!Odvpuv(ROIi=gt?Xy+s zhS!qWLx?IxW&h{e0*6vmDbYXxRSs{M_pFTwU>++(RT8voJ}-TMtL#5X7yEw8{UyYx z7G-J&K4+e`(Rbwek~tdUXK50n%ZEr;ZbUPDC&a4eTW^kN#_0Ye+GVBS-Hnc4IZgyg z4hiLLl#s@s*qrh>QPkYN%XY$3>`SY!E350++=`>5M$o8u^^S}g;Y2Hk{|PaOrESY` z{PYb2M}5?8!>8*se8!_M2J$$jwGN-}N)*DTP69_S>ybJClvjJemLGT-=8?z`1&@Pf z)5hL81y(*Om;>Fst*ZHLX$?Vdc(k;e7uai2Ih=sx1QEc5>M zbpIQ+!Jh9Sek+F^W3YF?Hc-Azn?ygdtIyKmTfdNG{k$~Y+Rdp{oO zlVEfUp8Z6}-9u_B+VB*}js$U=30xAF3v!SRz0Qv+n`?MgGh1D&2B}}uggtGP=QD7x z@W#65+Uby1L2*qsMF7qV{M8LL4J{2)*EF6ex{fnmKNzIeWp+B-k@>ifz%c=Y)?qsn zwkPiClJd4C>;~V=3cip>1wNUMo7i~}_+kxY7yhbe&bkv&Qzc*DGYd|I*9UCebPgz} zlf(Bz#9f_l6bYmrw*Jma&+Dg`S+$OSYc8fRvCrPGW^qqDv|s3z!|WLDD~E3#qhL>1 zi2^Q%D~9^=G4nd2=yZFwc{A*Jb6s46t`aFUf~5pqjP0ecJ?Rga&Y2$AOME>C6eCsV z8}DZ474__bvjG0iGu^Kx9ioTcE#>JP{l{lI`U!^vw4-X8qaQ8H>cw-D!)GA&Fbnbt zKnL-A^c<$69O_q(fu>JZ2L2N@ct&T(slk*D0ZoV9sExMNl^t^%grlybaMHP~GvYSf zFX{K5k?K4OZX%b>E#Y9r)Y?ug6Q99Xt9HXZ0~EEZl<{pVWxMyepSR^eY%3XTa>u28 z?o45yYd~Q6J)L9r&E`%^SaUr*`tpEE{VVW59v^`*ED&{GtekTi(O!&`@>+7J=Crm>pv)cs zyvQ0WEBxp=W?uz&pFs}aK02)(UGuh|S84p3ebm5bM71qNV&;;ficBjZpu`j|>D56p z7Czx)SejKeVO_h&^VYyInD1^6Q&`@!8$9mn$!vri45*YRu@6APS1Z3@9~$Qfeo9uF!{lsgHm%X5TFw4PKyj^tX=J z1~ls5fM8?diHgRrk-R z+7Ipc-K#*=?4YqQxrb@0nQV$sL#I^=Onngr)Ti5-9ZWM=UQ1rkT4?frd5+mvJ$WH_ zD_FzQ5gdIb&_dA7iGCADfOQ)Sb0b!pYoLU%_G?f{e**F8Ifj5<>~V-lD6xbqDR?qU z6eJ378ea1zQ@G(`-J80?gTUMVp5q|*mJVydrq=8pRKWNM6UrqD$Q{&fZ^G$&}x;uH9x8ef4T_1BqJ{ z&s-nK5H}=$IFxKA0tq6OV0QeMQqkm)j|t_n^@2g~rDd)o7g`{~Z<;7MUie^XS&`LU%($+4YPDUzBugC7p5oSy{e zxcAr+{Wl5d+L6Fz_^SaP<$dRGanIpjaeaACNyy<1(Sr>m4>Mx6^2pFe;y%OU?a7h) zu`giGSO@;uM}&+?$>1H{(oYc|4$*ET-cb+V$qA3aH9!&gEckXg%Gm|>dzfsaZAjm$c~p z$haP4Edi|yzT&)LhqzlR z%Aey;1+U`v066=DXFZrgOtE?W8buh>;rah-jHaKA@mb{OW6({} zE!DA6=%!2!#_&6lw>$e z6e;shgXe|U&})M=AXFg(#3+h=bkXkw_4b**+cFTV@kY1cRRK?eGB_@;34%COA;h07 zdoQocdcI)&!7aq>{7DcckIdP6u%QvW@D?GyUPey&njGfFV7}m!{I6GT&eol_bYF@Q z8JoWQ0!~g?^7MWs`wxh6Z0`Kfpa{_n8X&ZP4rL}EYk#4~Yx_g@utN`i^Aq505*9I- zRrDwq46fn4AOULhz;~-e4O$D&M06iKlh8_dCZpxwMIRPGcH;69UqQHQ01~@l_>XV= zoXh|C4*AAyU&+FoTR~PH_f*b5BU!(ivZ7Q(_fJ_iWo0bCJ9p*A+Rr4kTGgMVDiaCS zdr*e@EsFjmMJf2DrTs(>w56f~H{8y6Ps&P7=H8R&)b}JUr1VsJ;D~~lo|XT;WL^Ef zMyrh#3F;`6ejAtu-%w2=T5VWnSQ*Rj&RmJlAAeXk zY4cSgk*xQ9CeaPrh$58bE9qw#7^~qbkxZTp?b?QTi?(5d$_DlH)ya0CVKqJQzT!>+ zQ}73PI)MJHTk_#eR;soLpGiA|8*AgV|CI7S22sIaps^O2j!9NX4oEblbQ1wjEu<{d zo*ah-B&V< zrk98$(L=>ruEi~$oDz{HPPh}=ROLJ2TQ2?vk8%{{NF2Hp|03UYmVr16Ts495HbW}z zNS)tO`=#(lRjR+rZ4ybc%GKgdYa<{6{Vxm}z|cL8VL+)o*;1>^>E+qlgf-18Hh$w22F!X12pN^_Dnnd%Gzrq;mM)TXte z80O_*(_3nHxmjpY)6^Bp)*p=XSJ#m|AIIbZBT3Gd>V%v$BN7jcB;=$+{wCxTAm0c1 zM95zmLGZQ3z$L+b5}g&|G>695Xc@AmICt7~aAr^kbRcWCaaoJyAZsnPgvr3S)K&@I zD%Htq_~Mr;^tueZvZBhR>mqU(z-}7-0pjnM*Zj7}2t?%@dd{3Bz(OUbH`X=_X=2}< z=@9S2akIwSoxXnmE zE&&`UWghq;??LR9CJ^lSrTjKwC*Lic;!8l%GT`feV=Z;z@S1-HaejQwSFzl`=F8xY z&6j&S7JUVN36Zb$QzyuLoj*?DDd8Cv5zxD-J{+TKtnCqW;JNE#5xd6PLGUYPbvD-a z3XiKY1DWn-6%8kQB7cIP%r6hr*9N4+(*x3bL_j)TlI{vfpOh?d8FRHRt@|a_Re||# zhp3}d`!HN9z`X@<2V-!5j=_DxeQk`yW5#%Lv+gX@GMP9x$kkM)3;qn zdHghUTjG2lM8}C5h&!kKuH3hs=Z7dN?Y_!>9Sh02)R$79R=3cj>mn;o2zK!>bpm*e z9LP#Ls|20BNm$Bj+VqojWzgz-9b_HHX>PpvbLsY5eiY2YDaG^c+LQ5pS2>K}-1d#U zYS^>J%*S_M{0&5nJ?Y}L)bs$}pT)|wlR3(i@m^-wX^IHSbDUy~a`-*D^5TXybF2g7 z{U1~&o~%(OjKdDDD7v=^;z30#*1sFa|GBixbV5jI)4B1k6ibkJ=@1J$J6&XwwGe~# z7{5}L`>di7y%@`_s0Qc59-#Co`6cCYL2>f;5}#`2GOS3fi4+PiwZ9e=cwg{p(W<(M z)){Zo%HiM1Kk^@d{Nnk43rW|Zn6Ie7dk~3NM`-7J((N-K_8NOX1@2^G{-F?WM`AW< zRZ|ySb~WKaAGSlAwNXN8=z_~iuwLt7$upe-e#_y4Lny?Ma4)2;0cL)bM>*HDQr5A}ef|n8W$Fbp7yw<>? znX|dC1dC%Vym|C%VYcH%SBr34W(%LkNnP2!d>d>OFts$qx+ic`W4NvE>j8T* zr|ibK-J>;ty9RKl0`8Xwl+0N8&QTlJnb9YVg{uI!%~5>?H=aH2b#n|ij?baoV=l58 z@m78#e}u1hmAFV)7hM;zA> z>9H2NZ)`w+rsWn8`y%Egd(j<@0R0Vv~l>Ui69T&Bk1YT|7(Fx(wb##TiRcX!>; zk?f>E2iW;Wi1-j7Bh=an9NLxu_w+ogQRsF8V|jZF?J&n$=nHS$2e*yj^fyh$U(V^9 z5L4W7%#F1`W+$D)IOOmPaz30@@I8@N$55W~lyMT^a$zq9z7N6|C=za-Wb(K1O`wa? zZ$L(fGp=pGU$lf>!ggDcCi+s9-_<&KW$F*n>%rx>9V`w4hxTY zVt2{m(=uWQgWJw(k%1?L#rz=H(O=;1`6X_JGsDfS)e9sOqnp2dZ#3cHk5cvP@UxoJ80o~pu@kRH{Q-K9i~ZhZaW`>l*}XyD&M zSZOQ)cu&GNNVto-)Rk7Tf!YUCW3>5Q`0{$!g27%-k^0&J1n!SDq&o_Pt(I{+$ZzsiF6ht*|=SxBMc8?7Q z6;3LY4!(Gk3-HW1wZQ+nGJBd zPcf+TcluCBL*~_DSRLFH`t+URF3dx0A@vK;)e5-fv;rbG??(!1TZDSUR@=m$G>5r* zM{K8>1UMIZ7W2#9LH+>PH8>kY9>Z(vK0JQH>I>Go+ii^hHiO1o=sT>}SmM6du6MBA zjL#A4Gc4zLjpD0_5WY8r{mj$U=x2Bx&|J8$+o5)5u3OUy-;QLJ)8Ix0J|CPMcrvtk zMimdc3%+<8c&5u=40;LRS>m2jrXvCRc%jedvKsopNyKJw83Y^LX)(n6#B!+rtgkAs zt|P4{p(0^^+Pa#~gqy)ax*QU#`;EjdB#Ibwz9SqU*tEJOk`dK)!3@Bs8-{T|gR}(p z9N}Dy-<07p1M^em;ZqUO3)%qom|s@H`&^6Oanz8|NkCgFcmra9o&(<)e7H{>KBq?x zKPb!L206K6h&{m{L#NyA!)T3NB&jr4n%h3?DM|wg769hIfhT}dhs3lIIov2??SxNf zM?n+8P6K{myw{wQ>=3J>CUDlkM*Xyk9k#hW*MRnh(=>I1zYz4ye0hldhKD;8vUQNS zn!1MQZB(WCr_4|0tOjV&4`e1d2s0W`DOq=J4~{LcK!H=nU&EwE*3AxE*RU?uJ;%WA;Mf#4+GpeQ5|O zEYMPO$p~ICtT@17oqbNm_lOZY8a{gokEQD%9ZDxXtGj($>?^G!3$z)L#6|k;rSe|sKuVArsVJyJUJMEHjA;tnc zz%h|JxQhn(R2_tgcnyIjJS-DCsn$CrI+_L&2j6bxy`T-r*9a4gzW_rVk;zt&h}eEp zIZFqB4Lg7|Rq@t~^6Dmj{>4vhP*IJFhMLe6lXOHHe_8oPSlDgGnD24jB2tLAB- zM35R@XFByjqh!J}#dkoch(g4<4)tBOOG(b#gtTE)lkN}wpqmY&ZxrSN+)q%}vff$9t84Y6V1^kQM6~Qz;0Cw&{NH9vwUNaSHr+VI28+}3zCqJ)E zaiXs57Fc^b{2pZ}2+z#npvQ_B{F{->3HL?fqX%JZJ2EY;F}O(6Dd1?lj!RfKdhAPU z;%!T7>&l#ho!=@n@*fMw_yo7z<#bckTiy2R1n|KyJkycw%r3_|PbNX!9=mln%*B zPAx%>Gp)Q(Bv3D(~B5ADsW6qjl|Rv?F0j(bobm$CgEr{g@(F&S(| z^*G%6hTuc4BOJpH8V*EG(r*Pv$rcT!!#CF?y1ncTC!-%MeOLk1GRy50eRP*-kG{ zu}7sREMO07j!HW$nb_xr`?HzOoIqwpihzHc1wXJHq*4w1#ELY55|b&)90?>in_wgo zC~Xo*ppq$w1gchm)wNYEt6CY$;kP_>Wzcgq#L(ipjfR~H`i;)8l8goAq8*z-JJWqB z%i{JO@9C1G$t6q$)3Ml{0xL_w{1&^|HU*5F(6&&(ksj7ym+?Zpcwy~jdRwo+Uggwv z+zuy)bSpi)ZZ*W8mGsH93NJwW0h))Jt|DzY^j*HQEC*`Z3NO_BQ2SazpHx?*t$=qc zc9yk3O;O>6njdO$&){LK!^Y=o_{lgA*{WRC01xYTFZ8~5XBi7M7!_)MsP)nG$>fT( zmGExm&axMv2BSjF54CEjF%@a6;N2?d2Q?TKYJRB2N3~ssa_M*qeB-UOR^14km_$dms|T6febB{8q4U zh^6NlxT=N6^-Ph+Y~x4Zjw6-FJjT8sC%y5I!j0*60 z))&7GEfcmYZ(e0|)zul-xbM**n`@cz?gxx3?pk@zTERHw{&-9ED%a{|ce?p z>i!kWjMaBtc?#xLOSzG;ZL1JW)?n;>n6^e`khq+dgN z8PdHj_eu;qAN2cDI5S`$i-7ue3+Q|Jb1z$Q7tSHV`SXM@6nQ1>McWFXg8m493lYkd zqfmJO(l#}QRE)oS8Ok2{M_l{)AMS@+;P-w1_V@q#U5v3n1@#k3zCR*wz%#~wi{cqP VRk6~t@wyf6#E%sasGCOUe*(nkcTNBR diff --git a/Tools/bootloaders/ARK_GPS_bl.hex b/Tools/bootloaders/ARK_GPS_bl.hex index 922a5cde93c..0ce07ba00bf 100644 --- a/Tools/bootloaders/ARK_GPS_bl.hex +++ b/Tools/bootloaders/ARK_GPS_bl.hex @@ -1,1024 +1,1024 @@ :020000040800F2 -:1000000000070020210500082D160008AD15000886 -:1000100005160008AD150008D915000823050008CD -:100020002305000823050008230500088137000880 -:100030002305000823050008230500082305000800 -:1000400023050008230500082305000823050008F0 -:100050002305000823050008B53B0008E13B000824 -:100060000D3C0008393C0008653C000823050008E9 -:1000700023050008230500082305000823050008C0 -:10008000230500082305000823050008A125000812 -:100090000D26000861260008B5260008913C0008DE -:1000A0002305000823050008230500082305000890 -:1000B000013A00082305000823050008230500086D -:1000C0002305000823050008230500082305000870 -:1000D00023050008230500084D2900082305000812 -:1000E000F93C000823050008230500082305000843 -:1000F0002305000823050008230500082305000840 -:10010000230500082305000823050008230500082F -:10011000230500082305000823050008230500081F -:10012000230500082305000823050008230500080F -:1001300023050008230500082305000823050008FF -:1001400023050008230500082305000823050008EF -:1001500023050008230500082305000823050008DF -:1001600023050008230500082305000823050008CF -:1001700023050008230500082305000823050008BF -:1001800023050008230500082305000823050008AF -:10019000230500082305000823050008230500089F -:1001A000230500082305000823050008230500088F -:1001B000230500082305000823050008230500087F -:1001C000230500082305000823050008230500086F -:1001D000230500082305000823050008230500085F -:1001E000D0400B1CD1409C46203AD3401843524289 -:1001F00063469340184370479140031C90409C46CF -:10020000203A9340194352426346D3401943704702 -:1002100053B94AB9002908BF00281CBF4FF0FF316D -:100220004FF0FF3000F07AB9ADF1080C6DE904CE63 -:1002300000F006F8DDF804E0DDE9022304B07047C1 -:100240002DE9F0478C460D460446089E002B51D1FF -:100250008A4217466DD9B2FA82FEBEF1000F0BD06A -:10026000CEF1200C01FA0EF520FA0CFC02FA0EF782 -:100270004CEA050C00FA0EF44FEA174A250CBCFBB9 -:10028000FAF81FFA87F90AFB18CC45EA0C4508FB77 -:1002900009F3AB420AD9ED1908F1FF3280F023814E -:1002A000AB4240F22081A8F102083D44ED1AA4B20D -:1002B000B5FBFAF00AFB105544EA054400FB09F9C6 -:1002C000A14509D9E41900F1FF3380F00A81A14565 -:1002D00040F2078102383C44A4EB090440EA08409C -:1002E0000021002E61D024FA0EF4002334607360E4 -:1002F000BDE8F0878B4207D9002E54D0002186E854 -:1003000021000846BDE8F087B3FA83F1002940F0E8 -:100310008E80AB4202D3824200F2FA80841A65EBEF -:1003200003050120AC46002E3FD086E81010BDE842 -:10033000F08712B90127B7FBF2F7B7FA87FEBEF1D3 -:10034000000F34D1EB1B3A0C1FFA87FC0121B3FBE1 -:10035000F2F8250C02FB183345EA03450CFB08F3C1 -:10036000AB4207D9ED1908F1FF3002D2AB4200F2DF -:10037000D1808046ED1AA3B2B5FBF2F002FB105516 -:1003800043EA05440CFB00FCA44507D9E41900F13D -:10039000FF3302D2A44500F2B8801846A4EB0C0447 -:1003A00040EA08409DE731463046BDE8F087CEF18F -:1003B000200405FA0EF307FA0EF720FA04F83A0CB7 -:1003C00025FA04F448EA0308B4FBF2F14FEA1845B1 -:1003D00002FB11441FFA87FC45EA044501FB0CF3BC -:1003E000AB4200FA0EF409D9ED1901F1FF3080F0AB -:1003F0008A80AB4240F2878002393D44EB1A1FFAF3 -:1004000088F5B3FBF2F002FB103345EA034500FB2D -:100410000CF3AB4207D9ED1900F1FF386FD2AB42B4 -:100420006DD902383D44EB1A40EA01418FE7C1F132 -:10043000200722FA07F88B4005FA01F448EA030383 -:1004400020FA07FE4FEA134CFD404EEA040EB5FBBE -:10045000FCF94FEA1E440CFB19551FFA83F844EAD5 -:10046000054509FB08F4AC4202FA01F200FA01FA70 -:1004700008D9ED1809F1FF3043D2AC4241D9A9F1B6 -:1004800002091D442D1B1FFA8EFEB5FBFCF00CFB70 -:1004900010554EEA054400FB08F8A04507D9E418BA -:1004A00000F1FF3529D2A04527D902381C4440EA83 -:1004B0000940A4EB0804A0FB02894C45C6464D4602 -:1004C00015D312D056B1BAEB0E0364EB050404FA4F -:1004D00007F7CB401F43CC40376074600021BDE874 -:1004E000F0871846F8E69046E0E6C245EAD2B8EB57 -:1004F000020E69EB03050138E4E72846D7E74046DA -:1005000091E78146BEE7014678E702383C4445E77B -:10051000084608E7A8F102083D442BE7704700BFF2 -:1005200002E000F000F8FEE772B63A4880F308886F -:10053000394880F3098839484EF60851CEF2000157 -:10054000086040F20000CCF200004EF63471CEF2AA -:1005500000010860BFF34F8FBFF36F8F40F20000C0 -:10056000C0F2F0004EF68851CEF200010860BFF3F1 -:100570004F8FBFF36F8F4FF00000E1EE100A4EF681 -:100580003C71CEF200010860062080F31488BFF3AE -:100590006F8F03F071F903F051F903F097F94FF001 -:1005A00055301F491B4A91423CBF41F8040BFAE702 -:1005B0001C49194A91423CBF41F8040BFAE71A4919 -:1005C0001A4A1B4B9A423EBF51F8040B42F8040BE7 -:1005D000F8E700201749184A91423CBF41F8040B44 -:1005E000FAE703F02FF903F0C5F9144C144DAC42AF -:1005F00003DA54F8041B8847F9E700F041F8114C7E -:10060000114DAC4203DA54F8041B8847F9E703F0B4 -:1006100017B9000000070020002300200000000898 -:100620000001002000070020604000080023002097 -:10063000242300202823002090390020E001000816 -:10064000E0010008E0010008E00100082DE9F04F9A -:100650002DED108AC1F80CD0C3689D46BDEC108A00 -:10066000BDE8F08F002383F311882846A0470020BF -:1006700002F086FDFEE702F0EDFC00DFFEE7000081 -:100680002DE9F04103F018F8804603F065F80546BF -:1006900000283DD12B4B98453AD0013398453AD0AC -:1006A000294B28F0FF029A4238D15FFA88F000F017 -:1006B00051FA2E4642F2107400F060FC08B100249A -:1006C000264600F04DFA074660B36CBB374635B19D -:1006D0001E4B984503D003F037F80024274600202E -:1006E00002F0F4FF1A4B1B691B0420D40FB100F079 -:1006F00031F800F09BFC00F08FFE00F099FF0546FA -:10070000BCB900F043FD4FF47A7002F03FFDF7E70B -:100710002E460024D0E704460126CDE706464FF4D6 -:100720007A74C9E70446D2E74FF47A74CFE700241D -:10073000DFE700F07DFF401BA042E2D900F00AF89D -:10074000DBE700BF010007B0000008B0263A09B09F -:100750000004024010B51E4B1E4953F8042F01320D -:1007600000D110BD8B42F8D11B4C1C4B22689A4221 -:100770002CD91B4B9B6803F1006303F580339A422D -:1007800024D202F0B5FF02F0C7FF002000F05EFEA9 -:10079000144B0220187000F095FE134B1A6C0022C7 -:1007A0001A64196E1A66196E596C5A64596E5A6633 -:1007B0005B6E72B60D4A0E4B13601B6822682021D7 -:1007C00081F311889D4683F30888104710BD00BF50 -:1007D000FCFF00081C00010804000108FFFF0008DE -:1007E00000230020282300200038024008ED00E00C -:1007F0000000010809490B6849F269009AB21B0C14 -:1008000000FB0233064A0B60136844F2506198B251 -:100810001B0C01FB0030106080B2704720230020C9 -:100820001C23002010B500211022044600F066FEB3 -:10083000034B03CB206061601868A06010BD00BF4F -:10084000107AFF1FF0B51F4CBBB000F0F1FEA3689B -:10085000C31AF92B30D906AD2346A0602822002107 -:10086000284601F0A3FB04F10E0000F03FFE002338 -:10087000C6B2591D5F1CDBB2B3424FEAC10107DAB1 -:100880000E3323440822284601F090FB3B46F0E754 -:10089000012303930823207B02930B4B0193C1F3A5 -:1008A000CF013023059100930146049503A3D3E9BA -:1008B0000023064801F0B4F93BB0F0BD78F6339F51 -:1008C00093CACD8D68330020753300204433002057 -:1008D00070B50D4614461E4601F04AF950B9022E75 -:1008E0000ED1012C0CD112A3D3E90023C5E90023BA -:1008F000012070BD052C14D003D8012C09D0002094 -:1009000070BD282C09D0302CF9D10BA3D3E90023DA -:10091000ECE70BA3D3E90023E8E70BA3D3E900231B -:10092000E4E70BA3D3E90023E0E700BFAFF30080C7 -:10093000401DA12026812A0B78F6339F93CACD8DC6 -:100940009E6AC421818A46EE26417272DF25D7B79E -:10095000F017304A39059E5638B504460D46034611 -:1009600020222846002101F021FB22792346032A78 -:1009700028BF032203F8042F28460222202101F079 -:1009800015FB62792346072A28BF072203F8052FA3 -:1009900028460322222101F009FBA2792346072AD7 -:1009A00028BF072203F8062F28460322252101F03D -:1009B000FDFA284604F108031022282101F0F6FA76 -:1009C000382038BD2DE9F04FFFB01FAE0CAD40F21E -:1009D000791280460F463046002100F08FFD4822F4 -:1009E0000021284600F08AFD00F022FE574B4FF40C -:1009F0007A72B0FBF2F0186093E80700002485E8F3 -:100A0000070001230DF152002B746C74FFF70AFFED -:100A1000512385F8213006AB18464D4985F8204012 -:100A200003F00EFA152228643146284685F83C204A -:100A3000FFF792FF10AB044601460822304601F052 -:100A4000B5FA0822A1180DF14103304601F0AEFAC3 -:100A50000DF14203082204F11001304601F0A6FA1C -:100A600011AB202204F11801304601F09FFA12ABBD -:100A7000402204F13801304601F098FA14AB082204 -:100A800004F17801304601F091FA0DF1510308228A -:100A900004F18001304601F089FA04F1880A0DF171 -:100AA000520904F5847B4B465146082230460AF130 -:100AB000080A01F07BFAD34509F10109F3D119AB1A -:100AC00008225946304601F071FA04F588744FF057 -:100AD000000995F834304B4510D84FF0000995F8CF -:100AE0003C304B4515D92B6C21464B4408223046EF -:100AF00001F05CFA083409F10109F0E7AB6B21461B -:100B00004B440822304601F051FA083409F101093A -:100B1000DFE7E31DC3F3CF03F97E059300230496BB -:100B20000393BB7E0293193701230093019705A31A -:100B3000D3E90023404601F073F87FB0BDE8F08FA1 -:100B4000AFF300809E6AC421818A46EE2C230020E8 -:100B5000EC3E0008014B1870704700BF382300209E -:100B6000F0B5324B1C7B85B034B1314B0E221A816B -:100B70000024204605B0F0BD2E4A1068516802AB33 -:100B800003C308230DEB03022B492C4803F03AF969 -:100B9000054630B9264B2A480A221A8100F002FD88 -:100BA000E6E70169B1F5E02F06D9214B25480B2274 -:100BB0001A8100F0F7FCDCE7438B512B08D01C4A6C -:100BC00021480C2111815122194600F0EBFCD0E79D -:100BD0001E4A024402F11003994204D2144B1C48ED -:100BE00010221A81E5E710398E1A2046134900F0C9 -:100BF00023FD3246074605F11801204600F01CFD92 -:100C0000AB689F4202D1EB6898420AD0084B0D2294 -:100C10001A8100903B46EA68A9680E4800F0C2FCC1 -:100C2000A6E70D4800F0BEFC0124A2E768330020CF -:100C30002C230020503E0008DCFF060000000108C5 -:100C4000583E0008643E0008763E00080800FFF7A2 -:100C5000943E0008B13E0008DA3E00082DE9F04F4E -:100C6000ADB006AF80460C4600F082FF0646002875 -:100C70005BD1237E022B19D1E38A012B16D100F020 -:100C8000D7FC0546FFF7B6FD4FF4C873B54AB0FB75 -:100C9000F3F105F5167501FB1300E37E15FA80F0FC -:100CA0001060914633B9B04B00221A709C37BD4694 -:100CB000BDE8F08FA38AF5B2013BAB4206F1010615 -:100CC0000CD93B1D691CC9001D4400950023082256 -:100CD00001F0F801204601F03BF8EBE707F11400C2 -:100CE000FFF7A0FD2A4607F11401381D03F07AF83A -:100CF00003460028D7D10F2D08D89B4B1D70D9F87B -:100D00000030A3F51673C9F80030CFE707F1980259 -:100D1000014602F8950D20460092072201F018F8CE -:100D2000F978404600F01CFFC0E7E38A052B00F08D -:100D3000068106D8012BB9D121464046FFF742FE75 -:100D4000B4E7282B3DD0302BB0D1637E874D0133E3 -:100D50006A7BDBB29342E94640F0EF80E27E2B7B78 -:100D60009A4240F0EA8007F19803002623F8846D48 -:100D70001022009331460123204600F0E9FFB4F829 -:100D80001480A8F102081FFA88F808F1030323F081 -:100D900003030A3323F00703ADEB030D0DF1180A2B -:100DA00033469BB2B11C98454FEAC10106F10106DA -:100DB0006CDD5344009308220023204600F0C8FF56 -:100DC000EEE7A38A013B9BB2C92B3FF66FAF674E9C -:100DD000357B4DBB06F10C03009308222B462946B8 -:100DE000204600F0B5FFA38A05F10109013BEDB2F1 -:100DF0009D424FEAC90109DA0E35354400950023BA -:100E00000822204600F0A4FF4D46ECE70023002214 -:100E1000C6E900230023B36086F8D730C6F8D8307F -:100E2000337B0BB9E37E3373002507F114063B1DBA -:100E30000822294630467D60BD60FD6001F0B6F8AD -:100E40003B7A05F10109AB424FEAC90107D9FB68BA -:100E500008222B44304601F0A9F84D46F0E7C1F3D3 -:100E6000CF010023E07E059104960393A37E0293B5 -:100E70001934282301460093019438A3D3E90023B1 -:100E8000404600F0CDFEFFF7DDFC0FE795F8D700F8 -:100E900000F0E6FAD5F8D83006461BB995F8D70029 -:100EA00000F0EEFAD5F8D83043449E4204D295F8CB -:100EB000D700013000F0E4FA4FEA980B0024A0B20A -:100EC000584504F1010408DA2B6880000AEB0001A0 -:100ED0000122184400F01AFBF1E7D5E90023D5F808 -:100EE000D84095F8D70012EB080243F100034444C0 -:100EF000C5E90023C5F8D84000F0B2FA844209D30E -:100F000095F8D730013385F8D730D5F8D8309E1B07 -:100F1000C5F8D860B8F1FF0F08DC00232B7300F090 -:100F2000C1FAFFF71DFE08B1FFF714FC2B68104A49 -:100F30009B0A013313810023AB60CD46B6E6BFF3B5 -:100F40004F8F0C490C4BCA6802F4E0621343CB602C -:100F5000BFF34F8F00BFFDE7AFF3008026417272F1 -:100F6000DF25D7B7403300203D3300206833002011 -:100F70002C23002000ED00E00400FA0537B54FF007 -:100F80000054214B22689A4229D1204B627D1A706D -:100F90001F48237D0373C9221E490E3000F09CFABE -:100FA000E0220021204600F0A9FA0125284603B0DE -:100FB00030BD0321184800F053FB8DE8030063683F -:100FC00083421ED1236899424FF0000319D1A360D8 -:100FD0000E4B22691A70114BE2681A60E6E702F0C4 -:100FE000C5FB054668B10E4A0E4C136C43F0007306 -:100FF0001364A2680C4B9A4203D12369013B7E2BF8 -:10100000D7D90025D2E700BF9AAD44C538230020C8 -:101010006833002016000020586600401823002086 -:1010200000380240506600405041A0B0F0B54B4A35 -:101030004B4CD2F800E085B002236371BEF57A7F95 -:101040004FF000030293ADF80C304AD3454B464DA8 -:10105000B5FBFEF19E4594BF102309235A1CB1FB3A -:10106000F2F702FB171222B1022B3ADD013BDBB291 -:10107000F4E77A1EB2F5806F33D2C3EBC306F21CDD -:10108000C2F3C702991A02F1010CC9B24FF47A7087 -:1010900000FB0CF08C44B0FBFCF080B2B0F5617F3B -:1010A00006D9721E082692FBF6F2D2B29B1AD9B26A -:1010B000531E0F2B50D84B1E072B8CBF0023012330 -:1010C000501800FB0770B5FBF0F5AE4509D143B1F0 -:1010D0000123ADF808708DF80C308DF80A208DF8DA -:1010E0000B109DF80B30214921485A1E9DF80A30FB -:1010F000013B1B0443EA0253BDF80820013A1343A5 -:101100004B6001F0E1FC002301931A4B00931A4954 -:101110001A4B1B484FF4805200F0FAFC194B197817 -:1011200011B1174800F01CFD00F082FA0546FFF7E8 -:1011300061FB4FF4C87305F51675B0FBF3F202FBC3 -:101140001300114B15FA80F0186002F005FB08B18E -:101150000F23238105B0F0BD0023B1E71823002041 -:101160002C2300203F420F0040787D0110230020F7 -:1011700080340020D10800083C2300205D0C0008CA -:101180004433002038230020403300202DE9F04F65 -:1011900096A7D7E900670FF25C29D9E900898F4C3F -:1011A00093B0DFF858B2DFF858A2204600F082FD75 -:1011B0000CAD079098B310220021284600F09EF94C -:1011C000079B197B4FF0000261F3030219468DF86B -:1011D000302051F8040F49680EAA03C21B680D9A0B -:1011E00063F31C029DF830300D9243F020038DF81C -:1011F000303000232A461946584601F075FC079006 -:1012000030B9204600F05AFD079B8AF80030CCE741 -:101210009AF80030072B40DC01338AF8003018229E -:101220000021284600F06AF9DFF8D0B1DFF8D4A138 -:10123000002319462A46584601F07EFC014680BB31 -:10124000102208A800F05AF9DAF8143083F4806309 -:10125000CAF8143000F0EEF90B4612A9024611E963 -:1012600003000DF1240E8EE803009DF83410C1F345 -:10127000030089064CBF0E99BDF838108DF82C007C -:1012800046BFC1F31C0141F00041C1F30A010891BE -:10129000204608A900F0D2FECAE7204600F00EFD65 -:1012A000BDE7204600F064FC824600284AD1DFF802 -:1012B00058B100F0BDF9DBF80030984242D300F09D -:1012C000B7F90790FFF796FA079A8DF820A04FF428 -:1012D000C87302F51672B0FBF3F101FB130012FAAA -:1012E00080F0CBF80000DFF824B19BF8001011B9B2 -:1012F00001238DF8203028460791FFF793FA0799CC -:10130000C1F1100A5FFA8AFABAF1060F28BF4FF04E -:10131000060A524629440DF1210000F0DDF80AF1D9 -:101320000103049308AB0393182302932C4B0193FE -:101330000123009332463B46204600F01BFC00236D -:101340008BF8003000F074F9264ADFF8C4A0136867 -:10135000C31AB3F57A7F32D3106000F06BF90246FE -:101360000B46204600F0B4FC204600F001FC30B3F0 -:101370009AF80C30DFF89CB0002B14BF0323022333 -:101380008BF8053000F054F94FF47A732946B0FB1E -:10139000F3F0CBF800005846FFF7DEFA18230730C9 -:1013A0000293114B0193C0F3CF0040F25513049008 -:1013B0000093039542464B46204600F0DBFB9AF82B -:1013C0000C300BB1FFF73EFA9AF80C30002B7FF48B -:1013D000E8AE13B0BDE8F08FAFF3008044330020D7 -:1013E0003C330020483400204C340020401DA12014 -:1013F00026812A0BF1C6A7C1D068080F80340020CF -:101400004D34002000000240403300203D330020D6 -:10141000683300202C23002070B502F01BF8094C23 -:10142000094E2080002530682388834208D902F0C5 -:101430000BF8336805440133B5F5803F3360F2D3D0 -:1014400070BD00BF7C3400205034002002F0B2B8E0 -:1014500000F10060920000F5803002F041B8000019 -:10146000054B1A68054B1B889B1A834202D910440E -:1014700001F0EABF00207047503400207C34002087 -:1014800038B5074D04462868204401F0E5FF28B927 -:1014900028682044BDE8384001F0F6BF38BD00BFE1 -:1014A00050340020064991F8243033B1002308223B -:1014B000086A81F82430FFF7CBBF0120704700BFD6 -:1014C00054340020022802BF024B4FF480629A611C -:1014D000704700BF0000024010B50023934203D0C4 -:1014E000CC5CC4540133F9E710BD0000024603464A -:1014F000981A13F9011B0029FAD1704702440346D8 -:10150000934202D003F8011BFAE770472DE9F04738 -:10151000234C94F8243005468846174683BB2E4654 -:10152000DFF87C90C7B394F824302BB92022FF2138 -:1015300048462662FFF7E2FF94F82400C0F1080550 -:10154000BD4228BF3D465FFA85FAAD0041462A46B6 -:1015500004EB8000FFF7C0FF94F82430A7EB0A07E4 -:101560009A445FFA8AFABAF1080F2E44A844FFB2EF -:1015700084F824A0D6D1FFF795FF0028D2D108E047 -:10158000266A06EB8306B042CAD0FFF78BFF00281D -:10159000C5D10020BDE8F0870120BDE8F08700BF7D -:1015A000543400200FB4002004B0704700B59BB045 -:1015B000EFF3098168226846FFF78EFFEFF305839A -:1015C000044B9A6BDA6A9A6A9A6A9A6A9A6A9A6A6F -:1015D0009B6AFEE700ED00E000B59BB0EFF30981E8 -:1015E00068226846FFF778FFEFF30583044B9A6B98 -:1015F0009A6A9A6A9A6A9A6A9A6A9B6AFEE700BF2E -:1016000000ED00E000B59BB0EFF309816822684669 -:10161000FFF762FFEFF30583034B5A6B9A6A9A6AEE -:101620009A6A9A6A9B6AFEE700ED00E0FEE7000016 -:1016300001F0FEBF01F0E8BF30B5084D0A44914209 -:101640000BD011F8013B09245840013CF7D040F37E -:1016500000032B4083EA5000F7E730BD2083B8ED4C -:101660002DE9F7431AA7D7E9006700EB81014FF096 -:10167000FF324FF0FF330DF1040C884223D050F8B5 -:1016800004EBCDF804E04FF0000E1EF80C800024AF -:101690004FEA086562406B404FF00908B8F1010855 -:1016A0000BD0002A73F1000904DA92185B417240F2 -:1016B0007B40F3E792185B41F0E70EF1010EBEF1BB -:1016C000040FE2D1D9E7D043D94303B0BDE8F0839A -:1016D0009336EAA9EBE1F0422DE9F047089E01F0CC -:1016E00007054FEAD60C2A4406F0070600EBD100A6 -:1016F0004FF47F49954201D1BDE8F08705F0070E10 -:1017000006F0070AD64577464FEAD50438BF574654 -:10171000C7F10807511B0CEBD6088F42045D28BFA8 -:101720000F4604FA0EF413F8081029FA07FE24FAFB -:101730000AF45FFA8EFE4C404EFA0AFE04EA0E04EA -:10174000614003F808103D443E44D3E70246006878 -:1017500048B103681360D388118901339BB2994261 -:10176000D38038BF1381704770B588B00D460446EA -:10177000202200216846FFF7C1FE20460495FFF7AE -:10178000E5FF054658B16B46044608AE1A4603CA43 -:10179000B24220606160134604F10804F6D1284685 -:1017A00008B070BD2DE9F041056885B90160BDE85C -:1017B000F081002B29DA930CB34229D1A54201D143 -:1017C0000D60F3E7C8F800100C60BDE8F0814B68CD -:1017D00023F06047BE0C4FEAD37EC3F3807C16EA49 -:1017E000230638BF3E46A8462C466368BEEBD37F2F -:1017F00023F06042DDD1974203D1C3F380739C454F -:101800000AD1974205E01C46EFE7B24206D01346E4 -:101810009E422CBF00230123002BCFD12368A0467A -:10182000002BF0D12160BDE8F08100002DE9F041EE -:1018300006460F4600254FF6FF7441F221082A465E -:1018400030463946FEF7CCFCC0B284EA00240823B7 -:1018500014F4004F4FEA4404A4B203F1FF3318BF5D -:1018600084EA080413F0FF03F2D10835402DE6D1D5 -:101870002046BDE8F081000010B54B6833B9CA8A34 -:1018800063F30902CA820020002110BDC4681A68EF -:101890001C60C360438A013B43824A60EDE700005D -:1018A00010B50A4441F22104914200D110BD11F853 -:1018B000013B80EA0320082310F4004F4FEA400068 -:1018C00080B203F1FF3318BF604013F0FF03F3D180 -:1018D000EAE700002DE9F04F85B081468DE80A0067 -:1018E000BDF83C409346002A00F0838024B10E9B53 -:1018F000002B7ED0072C2AD809F10C00FFF726FF19 -:10190000054628B96FF00205284605B0BDE8F08FFE -:1019100014220021FFF7F2FD22460E9905F108007E -:10192000FFF7DAFD631C2B749BF800302C4403F0A6 -:101930001F0363F03F032372009B43F000436B607F -:10194000294609F11C00FFF72DFF0125DCE7019B6B -:101950001B0A4FF00008029300F10C034FF0800ABD -:101960004646454603930398FFF7F0FE07460028D6 -:10197000C8D014220021FFF7C1FDC6BB9DF804307A -:101980003B729DF808307B7202220E9B711E194437 -:10199000B4420AD9B8180132D2B211F801EF80F876 -:1019A00008E00136072AB6B2F2D19BF8001001F028 -:1019B0001F01B44208BF4FF0400AB81841EA48116D -:1019C0004AEA01030372009B013243F000437B604B -:1019D0003A74394609F11C00FFF7E4FE0135B442C0 -:1019E0002DB288F001084FF0000ABCD18CE700222C -:1019F000CBE76FF0010587E72DE9F0471E46CB8A5C -:101A00009146C3F30902062A80460F4619D01346B1 -:101A10000021B0B28DB25A1992B2052A09D9A84252 -:101A200010D8FA8A034463F30902FA820120BDE860 -:101A3000F087A842F3D93A4419F80140947601316D -:101A4000E8E70025FB8A7C68C3F30900821F1C239A -:101A5000B2FBF3FA03FB1A2A1FFA8AF27CB30121C4 -:101A60002368002B39D1B31F03441C20B3FBF0F3D0 -:101A700001339BB299420CD2BAF1000F09D1404612 -:101A8000FFF764FE08B1C0F800A0206008B3044668 -:101A90000022B6B24FF0000AB54230D2691E494466 -:101AA0001346E01801339BB211F801EF80F804E00F -:101AB00001351C2BADB214D0AE42F2D8ECE7404653 -:101AC000FFF744FE044608B1002303607C60002C4D -:101AD000DED16FF00200BDE8F087013189B21C460B -:101AE000BEE7AE42D8D94046FFF730FE08B1C0F895 -:101AF00000A020600028ECD004460022CCE7FB8A3E -:101B0000C3F30902164466F30903FB828EE7000063 -:101B1000F8B5044615460E46242200211F46FFF75D -:101B2000EDFC069B6360079B23626A094FF6FF7317 -:101B30009A424FF0000028BF1A46A760207097B263 -:101B400004F10C0503469A4205D100232B6027823D -:101B50006382A382F8BD2E60013335462036F2E75A -:101B600003781BB94BB2002BC8BF0170704700004F -:101B7000007870472DE9F7430D9EBDF828900B9D26 -:101B80009DF83040BDF8388007461946104616B912 -:101B9000B8F1000F40D11F2C3ED83B78D3B9B8F133 -:101BA000070F36D839F0030336D1424631464FF697 -:101BB000FF70FFF775FE4FEA092920F0010009F4D4 -:101BC0004079400449EA0464400C44EA40244FF65A -:101BD000FF730DE043EA0923B8F1070F43EA0464F9 -:101BE000F5D9FFF723FE42463146FFF759FE03467B -:101BF0008DE840012A4621463846FFF76BFE2B78D8 -:101C00000133DBB21F2B88BF00232B7003B0BDE86C -:101C1000F0836FF00300F9E76FF00100F6E70000D2 -:101C20002DE9F7430E9F0B9D9DF83480BDF83C6075 -:101C300081469DF8300007B9AEBB1F2833D899F80C -:101C400000E0BEF1000F31D00C0244F080049DF89A -:101C5000281044EAC83444EA014444EA0E04072E3A -:101C600044EA00641CD919461046FFF7DFFD3246EE -:101C70003946FFF715FE0346019600972A4621468E -:101C80004846FFF727FEB8F1010F06D12B78013344 -:101C9000DBB21F2B88BF00232B7003B0BDE8F0839D -:101CA0004FF6FF73E8E76FF00100F6E76FF003000F -:101CB000F3E70000C06900B104307047C3691A68D7 -:101CC000C261C2681A60C360438A013B43827047A5 -:101CD0002DE9F047D0F818A0DFF8708005461646C9 -:101CE0001F4654464FF000090CB9BDE8F087D4E90F -:101CF0000223B21A67EB0303994508BF904523D22C -:101D0000AB699C42214628460FD1FFF7B5FDAB6970 -:101D10001B68AB61EB6823606B8AEC60AC69013BCC -:101D20006B822346A2461C46DEE7FFF7A5FD23682B -:101D3000CAF80030EB6823606B8AEC60013B6B8271 -:101D4000DAF800305446EDE72368EBE780841E00A4 -:101D50002DE9F04F8BB00E4614460793DDF8508006 -:101D60008346002800F01081B8F1000F00F00C81CC -:101D7000531E3F2B00F20881012A03D1079B002B41 -:101D800040F00281BBF81450ED000023AE420893EE -:101D9000099304D3002630460BB0BDE8F08F331909 -:101DA0009D42DBF80C303ABFAD1BEDB2254623B99E -:101DB000DBF81030002B00F093802F2E65D8C6F191 -:101DC0003007BD424FF000032CBFFFB22F460093F7 -:101DD000314608AB3A46DBF80800FFF77DFCA5EB7F -:101DE00007093E445FFA89F9BBF8143003F1005348 -:101DF000063BDB000493DBF80C3003933021039B9C -:101E000013B1B9F1000F43D1DBF8100040B1B9F1C3 -:101E1000000F05D0009708AB4A46711AFFF75CFC2B -:101E20002EB2002DB6D060070AD00AAB03EBD40166 -:101E3000624211F8083C02F00702134101F8083C25 -:101E4000082C54D9102C54D9202C8CBF08230423DF -:101E5000079A002A00F09E80B4EBC30F00F09D802B -:101E6000082C48D89DF82030621E23FA02F2D107D0 -:101E700006D54FF0FF3202FA04F423438DF82030E8 -:101E80009DF8203088F8003085E7A9460027ABE7A9 -:101E9000049BE02B28BFE02306930B44B342059339 -:101EA00015D9A3EB060AD145039800972CBF5FFA1A -:101EB0008AFACA46711A08AB52460430FFF70CFC86 -:101EC0005744A9EB0A095644FFB25FFA89F9049B0B -:101ED000069A05999B1A0493039B1B6803938EE74C -:101EE00000932A4608AB3146DBF8080096E7012349 -:101EF000AEE70223ACE7102C12D8BDF82030621EEA -:101F000023FA02F2D20706D54FF0FF3202FA04F4A8 -:101F10002343ADF82030BDF82030A8F800303AE770 -:101F2000202C0FD8089A631E22FA03F3DB0705D58D -:101F30004FF0FF3303FA04F414430894089BC8F8E5 -:101F4000003028E7402C22D0DDE908AB621E504665 -:101F50005946FEF745F9002100F0010050EA01035F -:101F60000DD0224601200021FEF746F9404261EBE8 -:101F7000410140EA0A0041EA0B01CDE90801DDE92F -:101F80000823C8E9002306E76FF0010603E76FF0B6 -:101F9000080600E7012C3FF473AF082C7FF670AF02 -:101FA000102CB8D9202CEAD8C8E7000030B5402A58 -:101FB00085B01FD8002A08BF01220024012A0294FC -:101FC000039419D11B788DF8083053070AD004AB5D -:101FD00003EBD205544215F8083C04F00704A34073 -:101FE00005F8083C00910346002102A8FFF774FBA6 -:101FF00005B030BD4022E0E7082AE3D9102A03D813 -:102000001B88ADF80830E0E7202A02D81B6802934D -:10201000DBE7D3E90045CDE90245D6E710B5CB684B -:102020001BB98B600B618B8210BDC4681A681C6081 -:10203000C360438A013B4382CA60F0E72DE9F04F59 -:10204000D1F800908FB0C9F3C01604460D46CDE913 -:102050000223002E50D0C9F3C03AC9F30626B9F1C5 -:10206000000F80F2CB8119F0C04F40F0C7812B7B6D -:10207000002B00F0C381BAF1020F03D02278B242E4 -:1020800040F0BF8109F07F02BAF1020F059236D10C -:1020900019F07F0FC9F30F2B01D10BF0030B2B4469 -:1020A0007606059A93F8038046EA0A4646EA82468F -:1020B0005FEAD81346EA0B06049300F0A1800022E1 -:1020C00000230EA961E90823059B00936768534626 -:1020D0005A462046B847002800F08F80A7698FB97C -:1020E000314604F10C00FFF73FFB0746D0B96FF013 -:1020F00002000FB0BDE8F08F4FF0020AAFE7C9F35E -:10210000074BCCE73B699E420DD03F68002FF9D1C9 -:10211000314604F10C00FFF727FB07460028E6D004 -:10212000A3693B60A761DDE90601FFF77FFBB88289 -:10213000F97D08F01F06C1F38401711A89B20FFA04 -:1021400081FED7E90223BEF1000FBCBF01F1200ED2 -:102150000FFA8EFE52EA0301C9F3046900F05D81B3 -:10216000DDE90201801A61EB030102460B46B64825 -:102170000021994208BF9042C0F04F81049B002B80 -:1021800048D0BEF1010F00F3488118F0400F41D054 -:10219000DDE902230021C7E9022306A82022FFF778 -:1021A000ADF9DDE90223CDE906232B1D08932B7B36 -:1021B000ADF82EB0013BDBB2ADF82C309DF81430F9 -:1021C0008DF833308DF830A0A3688DF831608DF82C -:1021D000329006A920469847FB7DC3F38402013262 -:1021E00062F38603FB75FB8A6FF30903FB821B0A0C -:1021F0006FF3C713FB7500207BE76FF00B0078E7E8 -:10220000A76917B96FF00C0073E73B699E428FD046 -:102210003F68F6E7FB7DC8F34012B2EBD31F40F0F6 -:10222000F380C3F38403B34240F0F18004992B7B25 -:102230004FEA981200293BD012F001085CD1032B21 -:1022400040F2E880DDE90223C7E902232B7BAE1DC3 -:10225000033BDBB23246394604F10C00FFF7CCFBFE -:10226000002814DA39462046FFF706FBFB7DC3F34E -:102270008402013262F38603FB75FB8A68F309036B -:10228000FB821B0A68F3C713FB75032031E7AB8899 -:102290003B832A7B033AD2B2B88A3146FFF700FB70 -:1022A000FB7DB882DA43C2F3C01262F3C713A1E721 -:1022B00012BB2E1D013BDBB23246394604F10C0045 -:1022C000FFF79AFB002814DA39462046FFF7D4FAC4 -:1022D000FB7DC3F38402013262F38603FB75049A2B -:1022E000FB8A62F30903FB82049A1B0A62F3C71399 -:1022F000CAE72A7B013ACEE7F98AC1F30901013B1B -:102300000529DAB25AD8281D00239A420AD907EBC8 -:10231000010E013110F801CB8EF81AC001330629E5 -:10232000DBB2F2D1DDE902019342CDE9060107F10A -:102330001A01089138BF04337968099134BF5B19D9 -:1023400000230A93FB8AADF82EB0C3F309031A44A5 -:102350009DF814308DF833300023ADF82C208DF823 -:1023600030A08DF831608DF832907B602A7BB88A7E -:10237000013A291DFFF794FA3B8BB882834203D1BF -:10238000A36806A920469847204606A9FFF746FEFF -:10239000FB7DB88AC3F38402013262F38603FB75C6 -:1023A000FB8A6FF30903FB821B0A6FF3C713FB75EC -:1023B0003B8B984214BF112000209AE6D7F804E026 -:1023C000BEF1000F18D00623DEF8000088B9C91A44 -:1023D00005F1040800EB010CBCF11B0FC3B2A1D83E -:1023E00093429FD2F44418F8013B8CF8043001303A -:1023F000F0E71C338646E7E7734693E76FF0090082 -:1024000077E66FF00A0074E66FF00D0071E66FF08A -:102410000E006EE66FF00F006BE6FB7D68F386033F -:102420006FF3C713FB7539462046FFF725FA049B67 -:10243000002B7FF4AAAEFB7DC3F38402013262F36A -:102440008603FB75DEE600BF80841E002DE9F041A7 -:10245000504EB04240F089804F4CDFF840E1256C8F -:1024600045F000752564256E45F000752566246EDF -:10247000846AD4F8007207EA0E0747F00107C4F82F -:102480000072D4F8005205EA0E0545F0010545EA50 -:102490000121C4F80012002A69D00021C4F81C12DE -:1024A0000F46C4F80412C4F80C12C4F8141204EB5A -:1024B000C10501311C29C5F84072C5F84472F6D136 -:1024C0004FF0000E4FF0010C964514D1826AD2F8FD -:1024D0000032B04223F00103C2F8003257D12E4B34 -:1024E0001A6C22F000721A641A6E22F000721A66D8 -:1024F0001B6EBDE8F0819F781D8817F0010F18BF93 -:10250000D4F804820CFA05F11CBF41EA0808C4F8AB -:10251000048217F0020F1EBFD4F80C8241EA0808AB -:10252000C4F80C827F0742BFD4F814720F43C4F87A -:10253000147204EBC5055F68C5F840729F68C5F862 -:102540004472D4F81C522943C4F81C120C330EF107 -:10255000010EB9E7846A0021C4F81C12C4F8041201 -:10256000C4F80C12C4F81412AAE7002AF2D1836A44 -:102570000022C3F84022C3F84422C3F80422C3F85F -:1025800014220122C3F80C22C3F81C229EE7BDE8E6 -:10259000F08100BF80340020003802400000FFFFBF -:1025A000184A916A08B58B688B6013F0010105D059 -:1025B00013F00C0F14BF4FF480310121D80506D55C -:1025C00013F4406F14BF41F4003141F00201D8030D -:1025D00006D513F4402F14BF41F4802141F00401CB -:1025E000D3690BB107489847202383F31188002152 -:1025F000054800F0F3FD002383F31188BDE808408F -:1026000001F0F2B8803400208834002038B5124C34 -:10261000A36ADD68AA0712D05A6922F002025A6141 -:10262000A36913B1012120469847202383F3118821 -:1026300000210A4800F0D2FD002383F31188EB0645 -:1026400006D5A36A1021D960236A0BB102489847C6 -:10265000BDE8384001F0C8B8803400209034002034 -:1026600038B5124CA36A1D69AA0712D05A6922F024 -:1026700010025A61A36913B1022120469847202312 -:1026800083F3118800210A4800F0A8FD002383F39A -:102690001188EB0606D5A36A10211961236A0BB1D4 -:1026A00002489847BDE8384001F09EB880340020C9 -:1026B0009034002038B50F4CA36A5D685D602A072E -:1026C0000AD5042222701A6822F002021A60636A94 -:1026D00013B10021204698476B0706D5A36A996974 -:1026E000236A13B1090403489847BDE8384001F054 -:1026F0007BB800BF8034002010B50F4C204600F09E -:10270000D1F90E4BA3620B21132000F0B3F90B217A -:10271000142000F0AFF90B21152000F0ABF90B21CC -:10272000162000F0A7F9002320461A460E21BDE826 -:102730001040FFF78BBE00BF8034002000640040D3 -:10274000114B984210B5044609D1104B1A6C42F057 -:1027500000721A641A6E42F000721A661B6EA36A47 -:1027600001221A60A36A5A68D20707D562685168C5 -:102770001268D9611A60064A5A6110BD0121082009 -:1027800000F022FCEEE700BF803400200038024059 -:102790005B87010003291AD8DFE801F0020A0F1451 -:1027A000836A9B6813F0E05F14BF0120002070472C -:1027B000836A9868C0F380607047836A9868C0F342 -:1027C000C0607047836A9868C0F30070704700204B -:1027D0007047000010B503291FD8DFE801F0021F81 -:1027E0002327816A8B68C3F30163183301EB03135A -:1027F000107881061ED55468C0F30011490041EAE3 -:10280000C40141F0040100F00F0058609068986026 -:10281000D268DA6041F00101196010BD836A03F5E6 -:10282000C073E5E7836A03F5C873E1E7836A03F5DC -:10283000D073DDE79488C0F30011490041EA4451A8 -:10284000E1E7000001290CD003D3022910D00020B9 -:102850007047836ADA68920701D1186903E00120A2 -:102860007047836AD86810F0030018BF01207047D2 -:10287000836AF2E710B539B9836AD96889071BD131 -:102880001B699B0704D110BD012915D0022948D12D -:10289000816AD1F8C031D1F8C401D1F8C8411461BE -:1028A000D1F8CC41546120240C610C69A40717D1E4 -:1028B0004C6944F0100412E0816AD1F8B031D1F8CB -:1028C000B401D1F8B8411461D1F8BC41546120245D -:1028D000CC60CC68A40703D14C6944F002044C617D -:1028E00011795C0864F304119C0864F345111171BB -:1028F00089064BBF91681189DB085B0D4CBF63F300 -:102900001C0163F30A01137948BF916060F303036C -:1029100013714FEA10234FEA104058BF1181137012 -:10292000508010BD024AD36843F0C003D3607047A3 -:1029300000440040044B5A6810439A6858600AB13A -:10294000181D1047704700BFAC3400202DE9F0413E -:102950003D4ED6F85C52EF682B68DA059CB20CD578 -:10296000202383F311884FF40070FFF7E3FF6FF427 -:1029700080732B60002383F31188202383F3118855 -:10298000DFF8C48014F02F0331D183F311883806A7 -:1029900013D5210611D5202383F311882B4800F08D -:1029A00005FA002844DA0820FFF7C4FF4FF67F73CA -:1029B0003B40EB60002383F311887A060DD5630654 -:1029C0000BD5202383F31188F26C336D9A4201D129 -:1029D000336C7BBB002383F31188D6F86422D36861 -:1029E0000BB110699847BDE8F04100F0FDBE230728 -:1029F00012D014F0080F0CBF00208020E10748BF60 -:102A000040F02000A20748BF40F04000630748BFE5 -:102A100040F48070FFF78EFFA4066B6805D596F82A -:102A200060124046194000F063FA2C68A4B2A9E78E -:102A30006860BFE727F040073F0410203F0CFFF716 -:102A400079FFEF60C6E700BFAC340020E43400201B -:102A500010B5054C054A0021204600F027FA044B2A -:102A6000C4F85C3210BD00BFAC340020252900083A -:102A70000044004000F1604300F01F02400903F5EC -:102A8000614309018000C9B200F1604083F800137E -:102A900000F5614001239340C0F880310360704726 -:102AA000FFF72ABE00F108020123037082600023B1 -:102AB000C26000F110024360026142618361C36140 -:102AC000036243627047000010B52023044683F37D -:102AD0001188022303704160FFF732FE0423237044 -:102AE000002383F3118810BD2DE9F0411F460446F1 -:102AF0000D461646202383F3118800F10808237839 -:102B0000052B0ED029462046FFF744FE48B120464B -:102B100032462946FFF75EFE002080F31188BDE8AB -:102B2000F0813946404600F03DFB0028E7D0002305 -:102B300083F31188BDE8F0812DE9F0411F4604467A -:102B40000D461646202383F3118800F110082378E0 -:102B5000052B0ED029462046FFF774FE48B12046CB -:102B600032462946FFF786FE002080F31188BDE833 -:102B7000F0813946404600F015FB0028E7D00023DD -:102B800083F31188BDE8F081F8B5154682680669BF -:102B9000AA420B46816938BF8568761AB542044659 -:102BA00007D218462A46FEF797FCA3692B44A36177 -:102BB0000DE011D932461846FEF78EFCAF1B3A469F -:102BC000E1683044FEF788FCE2683A44A261A368F9 -:102BD0005B1BA3602846F8BD18462A46FEF77CFC1E -:102BE000E368E4E783682DE9F04104469342154623 -:102BF000266938BF85684069361AB5420F4606D245 -:102C00002A46FEF769FC63692B4463610DE012D923 -:102C10003246A5EB0608FEF75FFC4246B919E068AC -:102C2000FEF75AFCE26842446261A3685B1BA36042 -:102C30002846BDE8F0812A46FEF74EFCE368E4E74B -:102C400010B50A440024C361029B006040608460A8 -:102C5000C160816141610261036210BD08B5826992 -:102C60004369934201D1826882B9826801328260ED -:102C70005A1C42611970426903699A4201D3C368C0 -:102C80004361002100F09EFA002008BD4FF0FF30A4 -:102C900008BD000070B5202304460E4683F311885A -:102CA000A568A5B1A368A269013BA360531CA361F9 -:102CB00015782269934224BFE368A361E3690BB1ED -:102CC00020469847002383F31188284670BD31467B -:102CD000204600F067FA0028E2DA85F3118870BD1B -:102CE0002DE9F74F05460F4690469A46D0F81C90BE -:102CF000202686F311884FF0000B144664B122465B -:102D000039462846FFF740FF034668B95146284632 -:102D100000F048FA0028F1D0002383F31188A8EBD3 -:102D2000040003B0BDE8F08FB9F1000F03D00190AB -:102D30002846C847019B8BF31188E41A1F4486F389 -:102D40001188DBE7C16081614161C3611144009B6F -:102D5000006040608260016103627047F8B504461C -:102D60000E461746202383F31188A568A5B1A368F2 -:102D7000013BA36063695A1C62611E70236962692A -:102D80009A4224BFE3686361E3690BB12046984728 -:102D9000002080F31188F8BD3946204600F002FA81 -:102DA0000028E2DA85F31188F8BD000083694269E2 -:102DB0009A4210B501D182687AB982680132826084 -:102DC0005A1C82611C7803699A4201D3C3688361EB -:102DD000002100F0F7F9204610BD4FF0FF3010BD84 -:102DE0002DE9F74F05460F4690469A46D0F81C90BD -:102DF000202686F311884FF0000B144664B122465A -:102E000039462846FFF7EEFE034668B95146284684 -:102E100000F0C8F90028F1D0002383F31188A8EB53 -:102E2000040003B0BDE8F08FB9F1000F03D00190AA -:102E30002846C847019B8BF31188E41A1F4486F388 -:102E40001188DBE7026843681143016003B118474A -:102E5000704700001430FFF743BF00004FF0FF330E -:102E60001430FFF73DBF00003830FFF7B9BF000056 -:102E70004FF0FF333830FFF7B3BF00001430FFF7D7 -:102E800009BF00004FF0FF311430FFF703BF00000F -:102E90003830FFF763BF00004FF0FF323830FFF7E4 -:102EA0005DBF000000207047FFF7D2BD37B50F4B64 -:102EB0000360002343608360C360012304460374FE -:102EC000154600900B464FF4807200F15C011430FF -:102ED000FFF7B6FE00942B464FF4807204F5AE71F6 -:102EE00004F13800FFF72EFF03B030BD043F0008A7 -:102EF00038B5C36904460D461BB904210844FFF7E1 -:102F0000A1FF294604F11400FFF7A8FE002806DA05 -:102F1000201D4FF48061BDE83840FFF793BF38BDF6 -:102F2000024B00221B605B609A60704714370020E0 -:102F3000002303748268054B1B6899689142FBD299 -:102F40005A68036042601060586070471437002070 -:102F500008B5202383F31188037C032B05D0042BB1 -:102F60000DD02BB983F3118808BD436900221A6084 -:102F70004FF0FF334361FFF7DBFF0023F2E790E8F8 -:102F80000C001A6002685360F2E70000002303742B -:102F90008268054B1B6899689142FBD85A680360A8 -:102FA000426010605860704714370020054B196963 -:102FB0000874186802681A6053601861012303746A -:102FC000FDF744BB1437002030B54B1C87B00546D5 -:102FD0000A4C10D023690A4A01A800F01BF92846C0 -:102FE000FFF7E4FF049B13B101A800F051F9236936 -:102FF000586907B030BDFFF7D9FFF8E71437002054 -:10300000512F000838B50C4D41612B6981689A68D1 -:103010009142044603D8BDE83840FFF789BF1846FF -:10302000FFF7B4FF01232C61014623742046BDE85D -:103030003840FDF70BBB00BF14370020044B1A6863 -:103040001B6990689B68984294BF002001207047DC -:103050001437002010B5084C236820691A682260D4 -:103060005460012223611A74FFF790FF0146206922 -:10307000BDE81040FDF7EABA1437002008B5FFF7A5 -:10308000DDFF18B1BDE80840FFF7E4BF08BD000050 -:10309000FEE7000010B50C4CFFF742FF00F0ACF863 -:1030A00080220A49204600F031F8012344F8180C28 -:1030B000037400F075FB002383F3118862B60448A3 -:1030C000BDE8104000F044B83C3700202C3F000819 -:1030D000343F000800F018B9EFF3118020B9EFF386 -:1030E0000583202282F311887047000010B558B97B -:1030F000EFF30584C4F3080414B180F3118810BD04 -:10310000FFF7BCFF84F3118810BD0000826002222B -:1031100002740022427470478368A3F17C0243F872 -:103120000C2C026943F83C2C426943F8382C074ABE -:1031300043F81C2CC26843F8102C022203F8082C18 -:10314000002203F8072CA3F1180070476506000859 -:1031500010B5202383F31188FFF7DEFF002104461A -:10316000FFF750FF002383F31188204610BD0000B5 -:10317000024B1B6958610F20FFF718BF143700205E -:10318000202383F31188FFF7F3BF000008B5014641 -:10319000202383F311880820FFF716FF002383F311 -:1031A000118808BD49B1064B42681B6918605A6016 -:1031B000136043600420FFF707BF4FF0FF307047F4 -:1031C000143700200368984206D01A6802605060E5 -:1031D00059611846FFF7ACBE7047000038B5044689 -:1031E0000D462068844200D138BD036823605C60CE -:1031F0004561FFF79DFEF4E7054B03F114025A61A8 -:103200009A614FF0FF32DA6100221A62704700BF04 -:1032100014370020F8B50361C2600E46054600F081 -:1032200041FB1A4B19461F4651F8142F8A420DD103 -:103230001862AE606A602A600A2E2CBF80190A30BC -:103240009D615D61BDE8F84000F01ABB1B6A9268A1 -:10325000C41A341928BF3446A24202D9181900F002 -:103260001DFB7B699A6894420CD8AC6098685A68D8 -:103270002B60041B6A6015605D609C604FF0FF333B -:10328000FB61F8BDA41A1B68ECE700BF14370020EF -:1032900038B51C4B01685A6990421D460DD04468F0 -:1032A0002160026800215460C16091688068014417 -:1032B00091604FF0FF32DA6138BD1A4642F8141FB0 -:1032C0004A605B6900219342C16003D1BDE8384088 -:1032D00000F0DEBA9A6881680A449A602C6A00F0AD -:1032E000E1FA6A699268001B82420AD9111A092917 -:1032F00098BF00F10A02286ABDE83840104400F087 -:10330000CDBA38BD143700202DE9F041184D002703 -:1033100005F114066C6900F0C5FA2A6AA368811ADF -:103320008B4216D813442B6294E80C001A60226872 -:10333000D4F80C8053606B69E760B34201D100F0B0 -:10334000A7FA87F311882069C047202383F31188E7 -:10335000E0E76A69B24208D05B1A0A2B2CBFC0189A -:103360000A30BDE8F04100F099BABDE8F08100BF35 -:103370001437002000207047FEE70000704700006F -:103380004FF0FF3070470000BFF34F8F024AD36801 -:10339000DB03FCD4704700BF003C024008B5094B7A -:1033A0001B7873B9FFF7F0FF074B1A69002ABFBFFC -:1033B000064A5A6002F188325A601A6822F4806222 -:1033C0001A6008BDD0380020003C02402301674548 -:1033D00008B50B4B1B7893B9FFF7D6FF094B1A6959 -:1033E00042F000421A611A6842F480521A601A6868 -:1033F00022F480521A601A6842F480621A6008BD92 -:10340000D0380020003C0240072870B513D80B4A82 -:103410000B4C137863B90B4E4FF0006144F8231046 -:1034200056F823500133082B2944F7D10123137098 -:1034300054F8200070BD002070BD00BFF43800209B -:10344000D4380020543F0008014B53F82000704747 -:10345000543F000808207047072810B5044601D9DA -:10346000002010BDFFF7D0FF064B53F8243018445E -:10347000C21A0BB9012010BD12680132F0D1043B11 -:10348000F6E700BF543F0008072810B5044621D8CE -:10349000FFF77AFFFFF782FF0F4AF323D360C300E1 -:1034A000DBB243F4007343F002031361136943F486 -:1034B00080331361FFF768FFFFF7A6FF074B53F850 -:1034C000241000F013F9FFF783FF2046BDE81040F9 -:1034D000FFF7C2BF002010BD003C0240543F00086F -:1034E0002DE9F84312F00103154640D184182E4A05 -:1034F00094423CD82D4B1B6813F0010337D02C4C61 -:10350000FFF74CFFF323E360FFF73EFF40F2012794 -:10351000032D01D9830713D0254C0F46401A40F2E2 -:103520000118EB1B0B44012B00EB0706236924D881 -:1035300023F001032361FFF74BFF0120BDE8F8836F -:10354000236923F44073236123693B43236151F8CA -:10355000046B0660BFF34F8FFFF716FF03689E42B0 -:1035600008D0236923F001032361FFF731FF002016 -:10357000BDE8F883043D0430CAE723F440732361B7 -:10358000236943EA08032361B94637F8023B3380D5 -:10359000BFF34F8FFFF7F8FE3688B9F80030B6B2A8 -:1035A000B342BED0DDE700BF00000808003802408B -:1035B000003C0240084908B50B7828B153B9FFF721 -:1035C000EDFE01230B7008BD23B1BDE80840087073 -:1035D000FFF7FEBE08BD00BFD038002038B500F0B0 -:1035E00061F9074B1A68824207D9064AD2E90045B9 -:1035F000003445F10105C2E90045186038BD00BF3F -:10360000F83800200039002070B5FFF765FD064648 -:10361000FFF7E4FF054BD3E90045241845F1000509 -:103620003046FFF763FD2046294670BD0039002073 -:1036300008B5FFF7E9FF4FF47A720023FCF7E8FDC5 -:1036400008BD00BF10B50244064B0439D2B2904207 -:1036500000D110BD441C00B253F8200041F8040F03 -:10366000E0B2F4E750280040104B30B51C6F240442 -:1036700007D41C6F44F400741C671C6F44F40044AE -:103680001C670B4C236843F4807323600244094B8E -:103690000439D2B2904200D130BD441C00B251F87E -:1036A000045F43F82050E0B2F4E700BF0038024066 -:1036B000007000405028004007B5012201A90020F9 -:1036C000FFF7C0FF019803B05DF804FB13B5044693 -:1036D000FFF7F2FFA04206D002A9012241F8044DF3 -:1036E0000020FFF7C1FF02B010BD000070470000CE -:1036F000074B45F255521A6002225A6040F6FF729B -:103700009A604CF6CC421A60024B01221A70704744 -:10371000003000400C390020034B1B781BB1034BD9 -:103720004AF6AA221A6070470C3900200030004087 -:10373000034B1A6812B9034A12681A60704700BF37 -:103740000839002074380240024B4FF080721A6032 -:10375000704700BF7438024008B5FFF7E9FF024B1D -:103760001868C0F3407008BD0839002008B5FFF79D -:10377000DFFF024B1868C0F3007008BD0839002055 -:10378000EFF3098305494A6822F001024A60683371 -:1037900083F30988002383F31188704730EF00E03A -:1037A000202080F3118862B60B4B0C4AD96821F4B3 -:1037B000E0610904090C0A430949DA60CA6842F069 -:1037C0008072CA6007490A6842F001020A6010224A -:1037D000DA7783F82200704700ED00E00003FA0575 -:1037E000F0ED00E0001000E010B5202383F3118815 -:1037F0000E4B5B6813F4006314D0F1EE103AEFF354 -:103800000984683C4FF08073E361094BDB682366F1 -:1038100084F30988FFF712FC10B1064BA36110BDB9 -:10382000054BFBE783F3118810BD00BF00ED00E0FE -:1038300030EF00E0770600087A06000870470000C5 -:10384000FEE70000084A094B09498B4204D3094AA4 -:103850000021934205D3704752F8040F43F8040B3C -:10386000F3E743F8041BF4E7804000089039002098 -:1038700090390020903900207047000000F086B891 -:103880004FF08043002258631A610222DA607047C9 -:103890004FF080430022DA60704700004FF0804311 -:1038A000586370474FF08043586A70474B684360D5 -:1038B0008B688360CB68C3600B6943614B690362AB -:1038C0008B6943620B6803607047000008B51F4BAB -:1038D0001F481A6942F0FF021A611A6922F0FF02BA -:1038E0001A611A691A6B42F0FF021A631A6D42F0EC -:1038F000FF021A65174A1B6D1146FFF7D7FF1648DE -:1039000002F11C01FFF7D2FF144802F13801FFF762 -:10391000CDFF134802F15401FFF7C8FF114802F12F -:103920007001FFF7C3FF104802F18C01FFF7BEFFE3 -:103930000E4802F1A801FFF7B9FF0D4802F1C401DA -:10394000FFF7B4FFBDE8084000F0A4B8003802401B -:1039500000000240743F00080004024000080240DA -:10396000000C024000100240001402400018024007 -:10397000001C024008B500F019FAFFF78BFBBDE808 -:103980000840FFF7D5BE000070470000104B1A6CCE -:1039900042F001021A641A6E42F001021A660D4AE0 -:1039A0001B6E936843F0010393604FF08043312214 -:1039B0009A624FF0FF32DA6200229A615A63DA604B -:1039C0005A6001225A6108211A601C20FFF752B880 -:1039D00000380240002004E04FF0804208B513692F -:1039E000D1680B40D9B2C9439B07116107D5202389 -:1039F00083F31188FFF76EFB002383F3118808BD62 -:103A000008B5FFF7E9FFBDE80840FFF7EDBE00008D -:103A10001E4B1A6962F0FF021A611A69D2B21A616A -:103A20004FF0FF301A695A69586100215A6959618B -:103A30005A691A6A62F080521A621A6A02F0805257 -:103A40001A621A6A5A6A58625A6A59625A6A1A6C2F -:103A500042F080521A641A6E42F080521A661A6E50 -:103A60000B4A106840F480701060186F00F44070CA -:103A7000B0F5007F1EBF4FF4803018671967536898 -:103A800023F40073536000F071B900BF00380240A6 -:103A9000007000403C4B3D4A1A643D4A4FF440419F -:103AA00011601A6842F001021A601A689207FCD588 -:103AB0009A6822F003029A60334B19469A6812F012 -:103AC0000C02FBD1186800F0F90018609A601A68BF -:103AD00042F480321A600B689803FCD54B6F43F0B8 -:103AE00001034B67284B5A6F9207FCD5294A5A604D -:103AF0001A6842F080721A60254A53685804FCD54F -:103B0000214B1A4619688901FCD52349C3F8841052 -:103B1000196841F08061196013681B01FCD51F4BC7 -:103B200093600323C2F88C304FF00063C2F89430E6 -:103B30001B4B1A681B4B9A421B4B21D11B4A116825 -:103B40001B4A91421CD140F203121A60164A1368B4 -:103B500003F00F03032BFAD10B4B9A6842F00202D9 -:103B60009A609A6802F00C02082AFAD15A6C42F460 -:103B700080425A645A6E42F480425A665B6E7047C5 -:103B800040F20372E1E700BF003802400004001079 -:103B90000070004004194002083000240094883866 -:103BA000002004E011640020003C024000ED00E031 -:103BB00041C20F41084A08B5536911680B4003F030 -:103BC0000103536123B1054A13680BB1506898474C -:103BD000BDE80840FFF708BE003C01401039002056 -:103BE000084A08B5536911680B4003F0020353619A -:103BF00023B1054A93680BB1D0689847BDE80840E7 -:103C0000FFF7F2BD003C014010390020084A08B51A -:103C1000536911680B4003F00403536123B1054A53 -:103C200013690BB150699847BDE80840FFF7DCBD48 -:103C3000003C014010390020084A08B5536911685A -:103C40000B4003F00803536123B1054A93690BB19C -:103C5000D0699847BDE80840FFF7C6BD003C014069 -:103C600010390020084A08B5536911680B4003F069 -:103C70001003536123B1054A136A0BB1506A984788 -:103C8000BDE80840FFF7B0BD003C014010390020FE -:103C9000174B10B55C691A68144004F478725A61C5 -:103CA000A30604D5134A936A0BB1D06A98476006FD -:103CB00004D5104A136B0BB1506B9847210604D5FD -:103CC0000C4A936B0BB1D06B9847E20504D5094AB7 -:103CD000136C0BB1506C9847A30504D5054A936C3F -:103CE0000BB1D06C9847BDE81040FFF77DBD00BF19 -:103CF000003C0140103900201A4B10B55C691A686D -:103D0000144004F47C425A61620504D5164A136DCE -:103D10000BB1506D9847230504D5134A936D0BB131 -:103D2000D06D9847E00404D50F4A136E0BB1506E66 -:103D30009847A10404D50C4A936E0BB1D06E9847F6 -:103D4000620404D5084A136F0BB1506F98472304DF -:103D500004D5054A936F0BB1D06F9847BDE810406A -:103D6000FFF742BD003C014010390020062108B594 -:103D70000846FEF77FFE06210720FEF77BFE0621A0 -:103D80000820FEF777FE06210920FEF773FE0621C4 -:103D90000A20FEF76FFE06211720FEF76BFE0621B4 -:103DA0002820BDE80840FEF765BE000008B5FFF713 -:103DB0002FFE00F00BF8FEF773FEFFF775F8FFF724 -:103DC000E3FDBDE80840FFF759BD000000230449AA -:103DD0001A465A50C8180833802B4260F9D17047F0 -:103DE0001039002010B501390244904201D1002061 -:103DF00010BD10F8013B11F8014FA342F5D0181B7C -:103E000010BD00002DE9F8430746884691461E463E -:103E10008BB10D46A8EB0500B54207EB000402D2BA -:103E20000020BDE8F883324649462046FFF7DAFF16 -:103E300018B1013DEEE7BDE8F8832046BDE8F88300 -:103E4000034611F8012B03F8012B002AF9D1704722 -:103E500040A2E4F1646891064E6F20617070207397 -:103E600069670A00426164206677206C656E67743A -:103E7000682025750A0042616420626F6172645F88 -:103E800069642025752073686F756C6420626520F5 -:103E900025750A0042616420667720646573637249 -:103EA0006970746F72206C656E6774682025750A7E -:103EB00000426164206170702043524320307825B5 -:103EC0003038783A307825303878203078253038D6 -:103ED000783A3078253038780A00476F6F6420666A -:103EE00069726D776172650A000000006F72672E5B -:103EF0006172647570696C6F742E41524B5F4750EC -:103F00005300000000000000712E00085D2E000824 -:103F1000992E0008852E0008912E00087D2E00089D -:103F2000692E0008552E0008A52E00086D61696EE7 -:103F3000000000004C3F000858370020D038002017 -:103F400001000000913000080000000069646C6509 -:103F500000000000004000000040000000400000A1 -:103F6000004000000000010000000200000002000C -:103F700000000200A000902A00000000AAAAAAAA3D -:103F800050000024FFFB00000077000000900900B3 -:103F90000100000500000000AAAAAAA501000080F7 -:103FA000FFEF000000000000000000000000000023 -:103FB00000000000AAAAAAAA00000000FFFF00005B +:1000000000070020E5040008B915000839150008AC +:10001000911500083915000865150008E704000867 +:10002000E7040008E7040008E7040008993700081F +:10003000E7040008E7040008E7040008E7040008F4 +:10004000E7040008E7040008E7040008E7040008E4 +:10005000E7040008E7040008C53B0008F13B00087E +:100060001D3C0008493C0008753C0008E7040008F6 +:10007000E7040008E7040008E7040008E7040008B4 +:10008000E7040008E7040008E704000871250008F9 +:10009000DD2500083126000885260008A13C00085F +:1000A000E7040008E7040008E7040008E704000884 +:1000B000153A0008E7040008E7040008E704000810 +:1000C000E7040008E7040008E7040008E704000864 +:1000D000E7040008E70400081D290008E7040008F9 +:1000E000093D0008E7040008E7040008E7040008E9 +:1000F000E7040008E7040008E7040008E704000834 +:10010000E7040008E7040008E7040008E704000823 +:10011000E7040008E7040008E7040008E704000813 +:10012000E7040008E7040008E7040008E704000803 +:10013000E7040008E7040008E7040008E7040008F3 +:10014000E7040008E7040008E7040008E7040008E3 +:10015000E7040008E7040008E7040008E7040008D3 +:10016000E7040008E7040008E7040008E7040008C3 +:10017000E7040008E7040008E7040008E7040008B3 +:10018000E7040008E7040008E7040008E7040008A3 +:10019000E7040008E7040008E7040008E704000893 +:1001A000E7040008E7040008E7040008E704000883 +:1001B000E7040008E7040008E7040008E704000873 +:1001C000E7040008E7040008E7040008E704000863 +:1001D000E7040008E7040008E7040008E704000853 +:1001E00053B94AB9002908BF00281CBF4FF0FF319E +:1001F0004FF0FF3000F074B9ADF1080C6DE904CE9A +:1002000000F006F8DDF804E0DDE9022304B07047F1 +:100210002DE9F047089D04468E46002B4DD18A42B9 +:10022000944669D9B2FA82F252B101FA02F3C2F1EC +:10023000200120FA01F10CFA02FC41EA030E94407D +:100240004FEA1C48210CBEFBF8F61FFA8CF708FB9E +:1002500016E341EA034306FB07F199420AD91CEB76 +:10026000030306F1FF3080F01F81994240F21C81A8 +:10027000023E63445B1AA4B2B3FBF8F008FB1033F0 +:1002800044EA034400FB07F7A7420AD91CEB040425 +:1002900000F1FF3380F00A81A74240F207816444F5 +:1002A000023840EA0640E41B00261DB1D44000237A +:1002B000C5E900433146BDE8F0878B4209D9002DDE +:1002C00000F0EF800026C5E9000130463146BDE868 +:1002D000F087B3FA83F6002E4AD18B4202D38242D2 +:1002E00000F2F980841A61EB030301209E46002D81 +:1002F000E0D0C5E9004EDDE702B9FFDEB2FA82F2D6 +:10030000002A40F09280A1EB0C014FEA1C471FFA33 +:100310008CFE0126200CB1FBF7F307FB131140EA1A +:1003200001410EFB03F0884208D91CEB010103F1E7 +:10033000FF3802D2884200F2CB804346091AA4B2A9 +:10034000B1FBF7F007FB101144EA01440EFB00FE7D +:10035000A64508D91CEB040400F1FF3102D2A645E2 +:1003600000F2BB800846A4EB0E0440EA03409CE781 +:10037000C6F12007B34022FA07FC4CEA030C20FA2E +:1003800007F401FA06F31C43F9404FEA1C4900FA4E +:1003900006F3B1FBF9F8200C1FFA8CFE09FB1811CB +:1003A00040EA014108FB0EF0884202FA06F20BD93E +:1003B0001CEB010108F1FF3A80F08880884240F28E +:1003C0008580A8F102086144091AA4B2B1FBF9F0D2 +:1003D00009FB101144EA014100FB0EFE8E4508D9CD +:1003E0001CEB010100F1FF346CD28E456AD9023852 +:1003F000614440EA0840A0FB0294A1EB0E01A14237 +:10040000C846A64656D353D05DB1B3EB080261EBA4 +:100410000E0101FA07F722FA06F3F1401F43C5E97E +:10042000007100263146BDE8F087C2F12003D840B4 +:100430000CFA02FC21FA03F3914001434FEA1C47F6 +:100440001FFA8CFEB3FBF7F007FB10360B0C43EAE8 +:10045000064300FB0EF69E4204FA02F408D91CEB98 +:10046000030300F1FF382FD29E422DD90238634496 +:100470009B1B89B2B3FBF7F607FB163341EA034136 +:1004800006FB0EF38B4208D91CEB010106F1FF3885 +:1004900016D28B4214D9023E6144C91A46EA00467C +:1004A00038E72E46284605E70646E3E61846F8E60E +:1004B0004B45A9D2B9EB020864EB0C0E0138A3E757 +:1004C0004646EAE7204694E74046D1E7D0467BE738 +:1004D000023B614432E7304609E76444023842E7B0 +:1004E000704700BF02E000F000F8FEE772B63A483D +:1004F00080F30888394880F3098839484EF6085156 +:10050000CEF20001086040F20000CCF200004EF68E +:100510003471CEF200010860BFF34F8FBFF36F8FCD +:1005200040F20000C0F2F0004EF68851CEF2000119 +:100530000860BFF34F8FBFF36F8F4FF00000E1EE05 +:10054000100A4EF63C71CEF200010860062080F3DE +:100550001488BFF36F8F03F09FF903F07BF903F06A +:10056000C5F94FF055301F491B4A91423CBF41F835 +:10057000040BFAE71C49194A91423CBF41F8040BAD +:10058000FAE71A491A4A1B4B9A423EBF51F8040B2C +:1005900042F8040BF8E700201749184A91423CBF83 +:1005A00041F8040BFAE703F059F903F0EDF9144CA4 +:1005B000144DAC4203DA54F8041B8847F9E700F005 +:1005C00041F8114C114DAC4203DA54F8041B884732 +:1005D000F9E703F041B900000007002000230020E4 +:1005E00000000008000100200007002078400008FB +:1005F00000230020242300202823002098390020F5 +:10060000E0010008E0010008E0010008E001000846 +:100610002DE9F04F2DED108AC1F80CD0C3689D462E +:10062000BDEC108ABDE8F08F002383F311882846C3 +:10063000A047002002F086FDFEE702F00BFD00DF80 +:10064000FEE700002DE9F04103F044F8074603F00F +:100650008FF8054600283DD12B4B9F423AD00133FD +:100660009F423AD0294B27F0FF029A4239D1F8B283 +:1006700000F052FAA84642F2107400F055FC08B19E +:100680000024A04600F04EFA064670B37CBB4646F6 +:1006900035B11F4B9F4203D003F062F80024264679 +:1006A000002003F021F81B4B1B691B0422D40EB160 +:1006B00000F032F800F096FC00F07CFE00F07EFFC7 +:1006C0000546CCB100F07AFF401BA04214D900F0DF +:1006D00023F8F3E7A8460024CFE704464FF00108CB +:1006E000CBE780464FF47A74C7E70446D0E74FF46F +:1006F0007A74CDE70024DDE700F026FD4FF47A7030 +:1007000002F026FDDDE700BF010007B0000008B0E1 +:10071000263A09B0000402401E4B1F4A10B51C4681 +:100720001968013134D004339342F9D162681B4B0C +:100730009A422DD91A4B9B6803F1006303F580336D +:100740009A4225D202F0E0FF02F0F2FF002000F012 +:100750004BFE144B0220187000F082FE124B1A6CF4 +:1007600000221A64196E1A66196E596C5A64596E11 +:100770005A665B6E72B64FF0E0232021C3F8084D35 +:10078000D4E9003281F311889D4683F3088810472D +:1007900010BD00BF0000010820000108FFFF000895 +:1007A000002300202823002000380240094A136853 +:1007B00049F2690099B21B0C00FB01331360064B30 +:1007C000186844F2506182B2000C01FB020018600C +:1007D00080B27047202300201C23002010B5002188 +:1007E0001022044600F058FE034B03CB20606160EA +:1007F0001868A06010BD00BF107AFF1F2DE9F043FC +:10080000224DBBB000F0DAFEAB6840F2ED22C31A15 +:10081000934232D906AFA8602B46282200213846E1 +:1008200001F0DAFB05F10E0000F02EFE0026044672 +:100830005FFA80F905F10E08F3B2F100994501F174 +:10084000280107D908EB06030822384601F0C4FB4B +:100850000136F1E708230122CDE9023205340C4BC1 +:100860000193A4B230230093CDE9047405A3D3E926 +:100870000023297B074801F0C7F93BB0BDE8F083AE +:10088000AFF3008078F6339F93CACD8D703300208C +:100890007D3300204433002070B50D4614461E46BB +:1008A00001F048F950B9022E10D1012C0ED112A33B +:1008B000D3E90023C5E90023012007E0282C10D04C +:1008C00005D8012C09D0052C0FD0002070BD302C8C +:1008D000FBD10BA3D3E90023ECE70BA3D3E900235F +:1008E000E8E70BA3D3E90023E4E70BA3D3E9002354 +:1008F000E0E700BFAFF30080401DA12026812A0B56 +:1009000078F6339F93CACD8D9E6AC421818A46EEC4 +:1009100026417272DF25D7B7F017304A39059E5647 +:1009200013B504462346084620220021019001F019 +:1009300053FB22790198032A234628BF032203F898 +:10094000042F2021022201F047FB62790198072A37 +:10095000234628BF072203F8052F2221032201F096 +:100960003BFBA2790198072A234628BF072203F8F8 +:10097000062F2521032201F02FFB019804F1080323 +:100980001022282101F028FB382002B010BD000001 +:100990002DE9F04FFFB01FAD0CAE40F2751280464E +:1009A0000F4620A80021296000F076FD4822002192 +:1009B000304600F071FD00F001FE554B4FF47A72A5 +:1009C000B0FBF2F0186093E80700012386E8070007 +:1009D0000DF152003382FFF701FF4FF4A24333843D +:1009E00006AB18464B4903F039FA15223064294604 +:1009F000304686F83C20FFF793FF10AB04460146D3 +:100A00000822284601F0E8FA0822A1180DF1410356 +:100A1000284601F0E1FA0DF14203082204F1100129 +:100A2000284601F0D9FA11AB202204F1180128461A +:100A300001F0D2FA12AB402204F13801284601F04D +:100A4000CBFA14AB082204F17801284601F0C4FA6D +:100A50000DF15103082204F18001284601F0BCFA8F +:100A600004F1880A0DF1520904F5847B4B46514686 +:100A7000082228460AF1080A01F0AEFAD34509F126 +:100A80000109F3D119AB08225946284601F0A4FA0E +:100A900004F588744FF0000996F834304B450AD9B4 +:100AA000B36B21464B440822284601F095FA0834DE +:100AB00009F10109F0E74FF0000996F83C304B4589 +:100AC00004EBC90108D9336C08224B44284601F0D5 +:100AD00083FA09F10109F0E700230393BB7E029337 +:100AE000073107F119030193C1F3CF010123CDE9C8 +:100AF00004510093F97E04A3D3E90023404601F09A +:100B000083F87FB0BDE8F08F9E6AC421818A46EEEB +:100B10002C2300206C3E0008014B1870704700BF6A +:100B200038230020F0B5324B1C7B85B034B1314BFB +:100B30000E221A810024204605B0F0BD2E4A10680E +:100B4000516802AB03C308232C492D480DEB030267 +:100B500003F062F9054630B9264B2A480A221A8169 +:100B600000F0E6FCE6E70169B1F5E02F06D9214B7C +:100B700025480B221A8100F0DBFCDCE7438B512B6C +:100B800008D01C4A21480C2111815122194600F03D +:100B9000CFFCD0E71E4A024402F11003994204D26E +:100BA000144B1C4810221A81E5E710398E1A204692 +:100BB000134900F007FD3246074605F118012046AB +:100BC00000F000FDAB689F4202D1EB6898420AD06A +:100BD000084B0D221A810090D5E902123B460E48BF +:100BE00000F0A6FCA6E70D4800F0A2FC0124A2E755 +:100BF000703300202C230020153F0008DCFF060086 +:100C000000000108843E0008903E0008A23E000853 +:100C10000800FFF7C03E0008DD3E0008063F000860 +:100C20002DE9F04FADB006AF80460C4600F082FFD4 +:100C3000054600285AD1237E022B1BD1E38A012BC3 +:100C400018D100F0BBFC0646FFF7B0FD03464FF499 +:100C5000C870DFF8D092B3FBF0F206F5167602FB0F +:100C6000103316FA83F3C9F80030E37E33B9A84B8A +:100C700000221A709C37BD46BDE8F08FA38AEEB201 +:100C8000013BB34205F101050BD93B1D1E44E900B0 +:100C900000960023082201F0F801204601F060F8D8 +:100CA000ECE707F11400FFF799FD324607F1140154 +:100CB000381D03F0A1F80028D9D10F2E08D8944B85 +:100CC0001E70D9F80030A3F51673C9F80030D1E7CB +:100CD000FB1CF8700146009307220346204601F0F2 +:100CE0003FF8F978404600F01DFFC3E7E38A282B60 +:100CF00026D010D8012B1ED0052BBBD1BFF34F8FB0 +:100D00008449854BCA6802F4E0621343CB60BFF3A9 +:100D10004F8F00BFFDE7302BACD1637E7F4D013399 +:100D20006A7BDBB29342E94603D1E27E2B7B9A4297 +:100D300065D0CD469EE721464046FFF729FE99E75C +:100D4000A38A013B9BB2C92B94D8744D2E7B26BB42 +:100D500005F10C030093082233463146204600F08B +:100D6000FFFF731CF2B2D9001E46A38A013B9A42D0 +:100D700005DA0E322A44009200230822EEE700230F +:100D80000022C5E900230023AB6085F8D730C5F801 +:100D9000D8302B7B0BB9E37E2B73002507F11409A8 +:100DA0003B1D082229464846C7E90155FD6001F070 +:100DB00013F93B7A05F1010AAB424FEACA0608D99A +:100DC000FB6808222B443146484601F005F9554698 +:100DD000EFE7C6F3CF06E17ECDE904960023039347 +:100DE000A37E0293193428230093019446A3D3E9E8 +:100DF0000023404600F008FFFFF700FD3AE74FF000 +:100E0000000807F11403A7F814801022009341464C +:100E10000123204600F0A4FFA68A023EB6B2F31CCE +:100E20009B109B000733DB08A9EBC3039D460DF124 +:100E3000180A1FFA88F34FEAC801B34201F1100102 +:100E40000AD20AEB0803009308220023204600F090 +:100E500087FF08F10108ECE795F8D70000F0CEFA1B +:100E6000D5F8D83004461BB995F8D70000F0D6FA6B +:100E7000D5F8D83033449C4204D295F8D7000130DD +:100E800000F0CCFA4FEA960B4FF000081FFA88F1F9 +:100E90008B45D5E9003209D90AEB880103EB8800BC +:100EA000012200F001FB08F10108EFE7F31842F11D +:100EB0000002C5E90032D5F8D83095F8D70006EB26 +:100EC0000308C5F8D88000F099FA804509D395F851 +:100ED000D730D5F8D8000133001B85F8D730C5F8D6 +:100EE000D800FF2E08D800232B7300F0A9FAFFF7D3 +:100EF00019FE08B1FFF710FC2B68094A9B0A013361 +:100F000013810023AB6014E726417272DF25D7B747 +:100F10003D33002000ED00E00400FA0570330020AE +:100F20002C2300204033002030B54FF00054244BD8 +:100F300022689A4285B007D002F024FC0446A8B982 +:100F40000024204605B030BD1E4B627D1A701E483D +:100F5000237D03731D49C9220E3000F08BFA204611 +:100F6000E022002100F098FA0124EAE7184A194D1E +:100F7000136C43F000731364AA6D174B9A42DFD1D0 +:100F80002B6E013B7E2BDBD8144A07CA01AB83E8EA +:100F900007001846032100F02BFB6B6D83424FF0D6 +:100FA0000003CDD12A6D8A4201BFAB65054B2A6E85 +:100FB0001A7003BF0A4BEA6D1A601C46C1E700BFF6 +:100FC0009AAD44C53823002070330020160000205D +:100FD00000380240006600405041A0B05866004012 +:100FE000182300202DE9FF474B4C02236371002397 +:100FF00002934A4BD3F800C0BCF57A7F12D3484B1A +:10100000484FB7FBFCF69C458CBF0A231123581EA2 +:10101000B6FBF3F503FB1562C1B2002A3ED00228ED +:101020000346F4D89DF80B303F4940485A1E9DF8BE +:101030000A30013B1B0443EA0253BDF80820013A81 +:1010400013434B6001F026FD00230193384B3949CF +:1010500000933948394B4FF4805200F03DFD384B36 +:10106000197811B1344800F05DFD00F0A7FA05468B +:10107000FFF79CFB4FF4C873B0FBF3F202FB1303C2 +:1010800005F5167010FA83F02E4B186002F070FB15 +:1010900008B10F23238104B0BDE8F0876B1EB3F5C0 +:1010A000806FBFD2C1EBC10E0EF103034FEAE3091B +:1010B000C3F3C703A1EB030809F1010A4FF47A70E7 +:1010C0005FFA88F609FB00005AFA88F8B0FBF8F0DE +:1010D000B0F5617F08D90EF1FF33C3F3C703C91A16 +:1010E000CEB2591E0F2914D8721E072A8CBF0022B7 +:1010F0000122991901FB0551B7FBF1F7BC4591D1CC +:10110000002A8FD0ADF808508DF80A308DF80B60AA +:1011100088E71346EDE700BF2C23002018230020AA +:101120003F420F0040787D011023002088340020CA +:10113000990800083C23002044330020210C0008BB +:1011400038230020403300202DE9F04F91A7D7E944 +:1011500000670FF24829D9E90089874D93B0DFF87D +:1011600044B2864C284600F0B3FD0DF1300A0790DA +:1011700070B310220021504600F08EF9079B197BB6 +:101180004FF0000261F303028DF83020586899682F +:101190000EAA03C21B680D9A63F31C029DF830303F +:1011A0000D9243F020038DF830300023524619464B +:1011B000584601F07FFC079028B9284600F08CFDC6 +:1011C000079B2370CEE72378072B3CD8013323708D +:1011D00018220021504600F05FF9DFF8C8B1684CD2 +:1011E000002319465246584601F08CFC014670BB5C +:1011F000102208A800F050F9636983F480636361EA +:1012000000F0DEF90B4612A9024611E903000DF1C8 +:10121000240C8CE803009DF83410C1F30300890608 +:101220004CBF0E99BDF838C08DF82C0046BFC1F3F5 +:101230001C0C4CF00041CCF30A010891284608A987 +:1012400000F012FFCCE7284600F046FDC0E7284634 +:1012500000F070FC0446002848D1DFF84CB100F0E3 +:10126000ADF9DBF80030984240D300F0A7F90790C1 +:10127000FFF79CFA079A8DF8204003464FF4C87098 +:1012800002F51672B3FBF0F101FB103312FA83F38F +:10129000CBF80030DFF814B19BF8001011B901232E +:1012A0008DF8203050460791FFF798FA0799C1F161 +:1012B0001004E4B2062C28BF0624224651440DF146 +:1012C000210000F0D7F808AB0393182302930134F0 +:1012D0002C4B0193E4B20123009304943B46324625 +:1012E000284600F029FC00238BF8003000F066F956 +:1012F000254A264C1368C31AB3F57A7F31D31060A0 +:1013000000F05EF902460B46284600F0EFFC284646 +:1013100000F010FC28B3237BDFF894B0002B14BF3F +:10132000032302238BF8053000F048F94FF47A7359 +:101330005146B0FBF3F0CBF800005846FFF7F0FA47 +:10134000182307300293124B0193C0F3CF0040F2F1 +:101350005513CDE903A0009342464B46284600F0C2 +:10136000EBFB237B2BB1FFF749FA237B002B7FF4A8 +:10137000F6AE13B0BDE8F08F4433002055340020A2 +:10138000000002403C330020503400207033002025 +:1013900054340020401DA12026812A0BF1C6A7C18C +:1013A000D068080F88340020403300203D330020EF +:1013B0002C23002070B502F02DF8094E094D308025 +:1013C000002428683388834208D902F01DF82B686E +:1013D00004440133B4F5803F2B60F2D370BD00BFED +:1013E000843400205834002002F0D8B800F10060A6 +:1013F000920000F5803002F05BB80000054B1A68DF +:10140000054B1B889B1A834202D9104401F0FCBF94 +:1014100000207047583400208434002038B5074D30 +:1014200004462868204401F0F7FF28B928682044C2 +:10143000BDE8384002F008B838BD00BF583400207D +:10144000064991F8243033B10023086A81F824302A +:101450000822FFF7CBBF0120704700BF5C3400209B +:10146000022802BF024B4FF480629A61704700BFAE +:101470000000024010B50023934203D0CC5CC4545A +:101480000133F9E710BD000003460246D01A12F9F5 +:10149000011B0029FAD1704702440346934202D04F +:1014A00003F8011BFAE770472DE9F8431F4D144676 +:1014B00095F824200746884652BBDFF870909CB30D +:1014C00095F824302BB92022FF2148462F62FFF7E0 +:1014D000E3FF95F82400C0F10802A24228BF22468B +:1014E000D6B24146920005EB8000FFF7C3FF95F8A6 +:1014F0002430A41B1E44F6B2082E17449044E4B2D4 +:1015000085F82460DBD1FFF79BFF0028D7D108E0E6 +:101510002B6A03EB82038342CFD0FFF791FF0028B1 +:10152000CBD10020BDE8F8830120FBE75C3400202C +:101530000FB4002004B0704700B59BB0EFF30981F1 +:1015400068226846FFF796FFEFF30583044B9A6B1A +:10155000DA6A9A6A9A6A9A6A9A6A9A6A9B6AFEE749 +:1015600000ED00E000B59BB0EFF30981682268460A +:10157000FFF780FFEFF30583044B9A6B9A6A9A6A30 +:101580009A6A9A6A9A6A9B6AFEE700BF00ED00E0D9 +:1015900000B59BB0EFF3098168226846FFF76AFF48 +:1015A000EFF30583034B5A6B9A6A9A6A9A6A9A6AAE +:1015B0009B6AFEE700ED00E0FEE7000002F028B8BD +:1015C00002F000B830B5094D0A4491420DD011F82F +:1015D000013B5840082340F30004013B2C4013F02A +:1015E000FF0384EA5000F6D1EFE730BD2083B8ED69 +:1015F000F7B54FF0FF33DFF854C0DFF854E000EBED +:1016000081011A4688421CD050F8044B019401AF66 +:10161000042417F8015B82EA05620825DB181646E8 +:1016200005F1FF355241002EBCBF83EA0C0382EA6C +:101630000E0215F0FF05F1D1013C14F0FF04E8D1D2 +:10164000E0E7D843D14303B0F0BD00BF9336EAA929 +:10165000EBE1F0422DE9F041C56915B9C161BDE882 +:10166000F0814B6823F06047C3F38A464FEAD37E8C +:10167000C3F3807816EA230638BF3E46AC462B46B5 +:101680005A68BEEBD27F22F060440AD0002A18DAF2 +:10169000A40CB44217D19D420FD10D60DEE7134672 +:1016A000EEE7A74207D102F08044C2F380724245C0 +:1016B0000BD054B1EFE708D2EDE7CCF800100B6087 +:1016C000CDE7B44201D0B442E5D81A689C46002A5E +:1016D000E5D11960C3E700002DE9F047089D01F04E +:1016E00007044FEAD508224405F0070500EBD100B6 +:1016F0004FF47F49944201D1BDE8F08704F0070719 +:1017000005F0070A57453E4638BF5646C6F108065B +:10171000111B8E4228BF0E46E10808EBD50E415C36 +:1017200013F80EC0B94029FA06F721FA0AF1FFB200 +:101730008CEA010147FA0AF739408CEA010C03F8F8 +:101740000EC034443544D5E780EA0120082341F235 +:10175000210201B24000002980B203F1FF33B8BF7B +:10176000504013F0FF03F4D17047000038B50C4629 +:101770008D18A54200D138BD14F8011BFFF7E4FF16 +:10178000F7E7000002684AB113680360C388018963 +:1017900001339BB29942C38038BF03811046704722 +:1017A00070B588B0202204460D4668460021FFF738 +:1017B00073FE20460495FFF7E5FF024658B16B46DD +:1017C000054608AE1C4603CCB44228606960234637 +:1017D00005F10805F6D1104608B070BD082817D9E4 +:1017E00009280CD00A280CD00B280CD00C280CD0BF +:1017F0000D280CD00E2814BF4020302070470C203C +:101800007047102070471420704718207047202020 +:1018100070470000082817D90C280CD910280CD9BB +:1018200014280CD918280CD920280CD930288CBFA2 +:101830000F200E207047092070470A2070470B20A8 +:1018400070470C2070470D207047000010B54B68A2 +:1018500023B9CA8A63F30902CA8210BDC4681A6830 +:101860001C60C360438A013B43824A60EFE700008B +:101870002DE9F84F1D46CB8A0F46C3F3090106290F +:10188000814692460B4630D00020AAB207F11904D7 +:101890009EB2052E1FFA80F80FD8904503F1010380 +:1018A00006D3FB8A0A4462F30903FB8201201AE093 +:1018B0001AF80060E6540130EAE79045F1D2A1F150 +:1018C000060B1C237C68BBFBF3F203FB12BB1FFA65 +:1018D0008BF6002C45D14846FFF754FF044638B933 +:1018E00078606FF00200BDE8F88F4FF00008E6E77F +:1018F000002606607860ADB24FF0000B454510D968 +:101900000AEB0803221D13F8011B9155B1B208F12F +:1019100001081B291FFA88F82BD0454506F101065E +:10192000F1D8FB8AC3F30902154465F30903BCE748 +:10193000013292B21C462368002BF9D1AB1F0B4435 +:101940001C21B3FBF1F301339BB29A42D3D2BBF11A +:10195000000FD0D14846FFF715FF20B9C4F800B0FA +:10196000BFE70122E7E7C0F800B05E46206004460A +:10197000C1E74545D5D94846FFF704FF08B92060BF +:10198000AFE7C0F800B0002620600446B6E70000CC +:101990002DE9F04F2DED028B83B0CDE90013BDF89A +:1019A0003C5007469146002A00F092802DB10E9BD4 +:1019B000002B00F08D80072D32D807F10C00FFF7C7 +:1019C000E1FE044638B96FF00204204603B0BDECD6 +:1019D000028BBDE8F08F14220021FFF75DFD0E9908 +:1019E0002A4604F10800FFF745FD681CC0B2FFF766 +:1019F00011FFFFF7F3FE207499F80030013814FA54 +:101A000080F003F01F0363F03F030372009B43F079 +:101A10000041616038462146FFF71CFE0124D4E7EF +:101A200000F10C034FF0000808EE103A4FF0800A66 +:101A30004646444618EE100AFFF7A4FE83460028E7 +:101A4000C1D014220021FFF727FDC6BB019BABF8D4 +:101A5000083002200E9B00F1080299195BFA82F20D +:101A60000130C0B2082801D0AE422AD3FFF7D2FE1F +:101A7000FFF7B4FE99F80020009B411E02F01F0200 +:101A800042EA4812AE4208BF4FF0400A5BFA81F1C9 +:101A90004AEA020A43F0004281F808A08BF81000DD +:101AA000CBF8042059463846FFF7D4FD0134AE4246 +:101AB00024B288F001084FF0000ABBD185E700206E +:101AC000C8E711F801CB02F801CB0136B6B2C7E77F +:101AD0006FF0010479E70000F8B515460E4628229C +:101AE000002104461F46FFF7D7FC069B6360B5F54F +:101AF000001F079BA76034BF6A094FF6FF7223627D +:101B000004F10C0097B200239A4205D80023036029 +:101B100027826382A382F8BD0660013330462036F7 +:101B2000F2E7000003781BB94BB2002BC8BF01706D +:101B300070470000007870472DE9F74FDDF83C90C2 +:101B4000BDF830500D9E9DF83840BDF8407080467D +:101B500092469B46B9F1000F01D1002F51D11F2CA5 +:101B60004FD898F80000B0B9072F47D835F00303D5 +:101B700047D13A4649464FF6FF70FFF7F7FD20F090 +:101B800001002D02400445EA0464400C44EA40246C +:101B90004FF6FF7321E040EA0520072F40EA046476 +:101BA000F6D900254FF6FF73C5F12000A5F12002FC +:101BB0002AFA05F10BFA00F001432BFA02F2114365 +:101BC0001846C9B2FFF7C0FD0835402D0346EBD1DA +:101BD0003A464946FFF7CAFD0346CDE9009732462B +:101BE00021464046FFF7D4FE33780133DBB21F2B8A +:101BF00088BF0023337003B0BDE8F08F6FF003009F +:101C0000F9E76FF00100F6E72DE9F04F85B0924655 +:101C1000DDF848800F9D9DF840209DF84490BDF868 +:101C20004C7006469B46B8F1000F01D1002F48D1F9 +:101C30001F2A46D83378002B46D00C0244EA0264AF +:101C40009DF8381044EAC93444EA01441C43072F84 +:101C500044F0800432D900234FF6FF72C3F1200C08 +:101C6000A3F120002AFA03F10BFA0CFC41EA0C0163 +:101C70002BFA00F00143C9B210460393FFF764FD4D +:101C8000039B0833402B0246E8D13A464146FFF712 +:101C90006DFD0346CDE900872A4621463046FFF711 +:101CA00077FEB9F1010F06D12B780133DBB21F2B80 +:101CB00088BF00232B7005B0BDE8F08F4FF6FF738F +:101CC000E8E76FF00100F6E76FF00300F3E70000CC +:101CD000C06900B104307047C3691A68C261C26844 +:101CE0001A60C360438A013B438270472DE9F0418B +:101CF000D0F81880194E14461D464146002709B9F0 +:101D0000BDE8F081D1E90223A21A65EB03039642F4 +:101D100077EB03031ED283698B420DD1FFF796FD4B +:101D200083691B688361C3680B60438AC1608169F2 +:101D3000013B43828846E2E7FFF788FD0B68C8F85D +:101D40000030C3680B60438AC160013B4382D8F80E +:101D50000010D4E788460968D1E700BF80841E00E0 +:101D60002DE9F04F8BB00D46DDF8509014469B46A0 +:101D70008046002800F01981B9F1000F00F01581AC +:101D8000531E3F2B00F21181012A03D1BBF1000F3A +:101D900040F00B810023CDE90833B8F81430B5EBDF +:101DA000C30F4FEAC30703D300200BB0BDE8F08F89 +:101DB0002B199F42D8F80C303ABF7F1BFFB2274641 +:101DC0001BB9D8F81030002B7AD02F2D4ED8C5F182 +:101DD0003006B7424FF000032CBFF6B23E460093E8 +:101DE0002946D8F8080008AB3246FFF775FCA7EB88 +:101DF000060A35445FFA8AFAB8F8143003F1005342 +:101E0000063BDB000493D8F80C3003933021039B8E +:101E100013B1BAF1000F2CD1D8F8100040B1BAF1CB +:101E2000000F05D0009608AB5246691AFFF754FC24 +:101E300038B2002FB8D066070AD00AAB03EBD40142 +:101E4000624211F8083C02F00702134101F8083C15 +:101E5000082C3CD9102C40F2B580202C40F2B780E1 +:101E6000BBF1000F00F09C80082334E0BA46002646 +:101E7000C2E7049BE02B28BFE02306930B44AB4250 +:101E8000059314D95A1B03980096924534BF5246C5 +:101E9000D2B2691A08AB04300792FFF71DFC079A0B +:101EA0001644AAEB020A1544F6B25FFA8AFA049BBA +:101EB000069A05999B1A0493039B1B680393A6E754 +:101EC0000093D8F8080008AB3A462946AEE7BBF1C4 +:101ED000000F13D00123B4EBC30F6CD0082C12D821 +:101EE0009DF82030621E23FA02F2D50706D54FF086 +:101EF000FF3202FA04F423438DF820309DF820309D +:101F000089F8003051E7102C12D8BDF82030621E3D +:101F100023FA02F2D10706D54FF0FF3202FA04F499 +:101F20002343ADF82030BDF82030A9F800303CE75D +:101F3000202C0FD80899631E21FA03F3DA0705D580 +:101F40004FF0FF3202FA04F40C430894089BC9F8DE +:101F500000302AE7402C2BD0DDE90865611EC4F172 +:101F60002102A4F1210326FA01F105FA02F225FA71 +:101F700003F311431943CB0712D50122A4F1200327 +:101F8000C4F1200102FA03F322FA01F1A240524205 +:101F900043EA010363EB430332432B43CDE90823B8 +:101FA000DDE90823C9E90023FFE66FF00100FCE644 +:101FB0006FF00800F9E6082CA0D9102CB3D9202C1A +:101FC000EED8C3E7BBF1000FADD0022383E7BBF12E +:101FD000000FBBD004237EE730B5012A144638BF7A +:101FE0000124402C85B028BF40240025012ACDE9DA +:101FF000025518D81B788DF8083063070AD004AB57 +:1020000003EBD405624215F8083C02F00702934046 +:1020100005F8083C009103462246002102A8FFF77C +:102020005BFB05B030BD082AE4D9102A03D81B8811 +:10203000ADF80830E1E7202A8DBFD3E900231B6803 +:102040000293CDE90223D8E710B5CB681BB98B60AA +:102050000B618B8210BDC4681A681C60C360438A20 +:10206000013B4382CA60F0E72DE9F04FD1F80080D0 +:1020700093B018F0800FCDE90323C8F3C01219BF45 +:10208000C8F3C03BC8F306264FF0020B1646B8F162 +:10209000000F04460D4680F2D18118F0C04305932D +:1020A00040F0CC810B7B002B00F0C881BBF1020F0C +:1020B00003D00178B14240F0C48108F07F0106915D +:1020C0006AB3C8F3074A2B44069A93F8039076063E +:1020D00046EA0B4646EA82465FEAD91346EA0A0612 +:1020E000079300F0908000220023CDE90A23069B8D +:1020F000009367685B4652460AA92046B847002805 +:102100007ED0A7699FB9314604F10C00FFF748FB68 +:102110000746E0B96FF0020013B0BDE8F08FC8F3D6 +:102120000F2A18F07F0F08BF0AF0030ACBE73B69BC +:102130009E420DD03F68002FF9D1314604F10C00CA +:10214000FFF72EFB07460028E4D0A3693B60A76198 +:10215000DDE90A2300264FF6FF70C6F1200E22FAB1 +:1021600006F103FA0EFEA6F1200C23FA0CFC41EA5C +:102170000E0141EA0C01C9B2083609920893FFF733 +:10218000E3FA402EDDE90832E7D1B882FB7D09F0A1 +:102190001F06C3F384039B1BD7E9022198B2002BCF +:1021A000BCBF00F120031BB252EA0100C8F304686F +:1021B0000FD00398821A049860EB0101A74890425F +:1021C0004FF000028A4104D3079A002A5BD0012B0A +:1021D00023DDFA7D4FEA890302F0030203F07C035A +:1021E0001343FB7539462046FFF730FB079BA3B925 +:1021F000FB7DC3F38402013262F38603FB7504E0C6 +:102200006FF00B0088E7A76917B96FF00C0083E740 +:102210003B699E42BAD03F68F6E719F0400F32D0D2 +:10222000039BBB60049BFB60142200210DA8FFF7F9 +:1022300033F9039B0A93049B0B932B1D0C932B7B6D +:10224000ADF83EA0013BDBB2ADF83C30069B8DF80B +:10225000433094F824308DF840B083F001038DF8BA +:1022600044308DF84160A3688DF842800AA9204669 +:102270009847FB7DC3F38403013303F01F039B02E4 +:10228000FB82002048E7FB7DC9F34012B2EBD31F6D +:1022900040F0DA80C3F38403B34240F0D88007995A +:1022A0002B7B4FEA9912002934D0D20741D4032B5B +:1022B00040F2D080039BBB60049BFB602B7BAE1D78 +:1022C000033BDBB23246394604F10C00FFF7D0FA8B +:1022D00000280DDA20463946FFF7B8FAFB7DC3F334 +:1022E0008403013303F01F039B02FB82032013E7E7 +:1022F000AB883B832A7B033AB88AD2B23146FFF7D8 +:1023000035FAFB7DB882DA43C2F3C01262F3C71319 +:10231000FB75B6E76AB92E1D013BDBB23246394682 +:1023200004F10C00FFF7A4FA0028D3DB2A7B013A62 +:10233000E2E7F98AC1F30901013B0529DAB259D86C +:10234000281D002307F11A0C9A4208D910F801EB56 +:102350000CF801E0013101330629DBB2F4D1039915 +:102360000A9104990B91934207F11A010C9138BF1D +:10237000043379680D9134BF55FA83F300230E932B +:10238000FB8AADF83EA0C3F309031A44069B8DF8FF +:10239000433094F82430ADF83C2083F001038DF8ED +:1023A000443000238DF840B08DF841608DF84280B4 +:1023B0007B602A7BB88A013A291DFFF7D7F93B8B4E +:1023C000B882834203D1A3680AA9204698472046D1 +:1023D0000AA9FFF739FEFB7DB88AC3F384030133F2 +:1023E00003F01F039B02FB823B8B984214BF11201A +:1023F000002091E67B68002BB1D0062001E01C3064 +:102400006346D3F800C0BCF1000FF8D1091A081DCB +:1024100005F1040C00EB030905989DF8143001EB5D +:10242000000EBEF11B0F9AD89A4298D91CF8013BB6 +:1024300009F8013B059B01330593EDE76FF00900B7 +:102440006AE66FF00A0067E66FF00D0064E66FF071 +:102450000E0061E66FF00F005EE600BF80841E0094 +:10246000404BF0B51C6C404E44F000741C641D6E73 +:1024700045F000751D661B6E3C4B9B6AD3F80052FD +:10248000354045F00105C3F80052D3F8004234400E +:1024900044EA002040F00100C3F80002002952D0B5 +:1024A0000020C3F81C020546C3F80402C3F80C025E +:1024B000C3F8140203EBC00401301C28C4F84052D6 +:1024C000C4F84452F6D100254FF0010C96781488D8 +:1024D000F70748BFD3F804720CFA04F044BF07436F +:1024E000C3F80472B70742BFD3F80C720743C3F8AE +:1024F0000C72760742BFD3F814620643C3F8146225 +:1025000003EBC4045668C4F840629668C4F8446299 +:10251000D3F81C4201352043A942C3F81C0202F142 +:102520000C02D3D1D3F8002222F00102C3F800221A +:102530000C4B1A6C22F000721A641A6E22F00072B0 +:102540001A661B6EF0BD0122C3F84012C3F8441294 +:10255000C3F80412C3F81412C3F80C22C3F81C22E7 +:10256000E0E700BF003802400000FFFF8834002091 +:10257000184A916A08B58B688B6013F0010104D08A +:1025800013F00C0F18BF4FF48031D80506D513F4A3 +:10259000406F14BF41F4003141F00201D80306D569 +:1025A00013F4402F14BF41F4802141F00401D3699A +:1025B0000BB108489847202383F31188064800216F +:1025C00000F0EEFD002383F31188BDE8084001F020 +:1025D00017B900BF883400209034002038B5124C61 +:1025E000A36ADD68AA0712D05A6922F002025A6172 +:1025F000A36913B1012120469847202383F3118852 +:102600000A48002100F0CCFD002383F31188EB067B +:1026100006D5A36A1021D960236A0BB102489847F6 +:10262000BDE8384001F0ECB8883400209834002030 +:1026300038B5124CA36A1D69AA0712D05A6922F054 +:1026400010025A61A36913B1022120469847202342 +:1026500083F311880A48002100F0A2FD002383F3D0 +:102660001188EB0606D5A36A10211961236A0BB104 +:1026700002489847BDE8384001F0C2B888340020CD +:102680009834002038B50F4CA36A5D685D602A0756 +:102690000AD5042222701A6822F002021A60636AC4 +:1026A00013B10021204698476B0706D5A36A9969A4 +:1026B000236A13B1034809049847BDE8384001F084 +:1026C0009FB800BF8834002010B50E4C204600F0A3 +:1026D000CFF90D4BA3620B21132000F0B1F90B21B0 +:1026E000142000F0ADF90B21152000F0A9F90B2101 +:1026F000162000F0A5F90022BDE8104011460E207A +:10270000FFF7AEBE8834002000640040114B9842B1 +:1027100010B5044609D1104B1A6C42F000721A64CD +:102720001A6E42F000721A661B6EA36A01221A60CA +:10273000A36A5A68D20707D5626851681268D961DE +:102740001A60064A5A6110BD0121082000F01EFCE3 +:10275000EEE700BF88340020003802405B870100AC +:1027600003291AD8DFE801F0020A0F14836A9B6874 +:1027700013F0E05F14BF012000207047836A98685F +:10278000C0F380607047836A9868C0F3C060704788 +:10279000836A9868C0F3007070470020704700009B +:1027A00010B5032925D8DFE801F00225292D836A19 +:1027B0009968C1F30161183103EB011310788406A5 +:1027C0004CBF54689488C0F300114FEA410148BFE0 +:1027D00041EAC40100F00F004CBF41F0040141EA9E +:1027E0004451586041F001019068D2689860DA6005 +:1027F000196010BD836A03F5C073DFE7836A03F5D0 +:10280000C873DBE7836A03F5D073D7E701290AD0E1 +:1028100002290FD081B9836ADA68920701D1186959 +:1028200003E001207047836AD86810F0030018BFE6 +:1028300001207047836AF2E70020704710B539B96C +:10284000836AD96889071BD11B699C0704D110BD15 +:10285000012915D00229FAD1816AD1F8C031D1F805 +:10286000C441D1F8C8011061D1F8CC0150612020D9 +:1028700008610869800717D1486940F0100012E02C +:10288000816AD1F8B031D1F8B441D1F8B801106102 +:10289000D1F8BC0150612020C860C868800703D10E +:1028A000486940F002004861C3F34000C3F380016F +:1028B000000140EA4111107920F03000014311710C +:1028C00089064BBF91681189DB085B0D4CBF63F330 +:1028D0001C0163F30A01137948BF916064F3030399 +:1028E00013714FEA14234FEA144458BF1181137037 +:1028F0005480ACE7024AD36843F0C003D36070470A +:1029000000440040044B5A6810439A6858600AB16A +:10291000181D1047704700BFB43400202DE9F04166 +:102920003C4ED6F85C52EF682B68DA059CB20CD5A9 +:10293000202383F311884FF40070FFF7E3FF6FF457 +:1029400080732B60002383F31188202383F3118885 +:10295000DFF8C08014F02F0339D183F311883806D3 +:1029600013D5210611D5202383F311882A4800F0BE +:1029700001FA00284CDA0820FFF7C4FF4FF67F73F6 +:102980003B40EB60002383F311887A0615D563067C +:1029900013D5202383F31188D6E913239A4209D152 +:1029A000336C3BB127F040073F0410203F0CFFF78A +:1029B000A9FFEF60002383F31188D6F86422D3685F +:1029C0000BB110699847BDE8F04100F019BF23072B +:1029D00012D014F0080F0CBF00208020E10748BF80 +:1029E00040F02000A20748BF40F04000630748BF06 +:1029F00040F48070FFF786FFA4066B6805D596F853 +:102A000060124046194000F057FA2C68A4B2A1E7C2 +:102A10006860B7E7B4340020EC34002010B5054CF2 +:102A2000054A0021204600F025FA044BC4F85C3228 +:102A300010BD00BFB4340020F52800080044004059 +:102A400000F1604303F561430901C9B283F8001343 +:102A5000012200F01F039A4043099B0003F16043E9 +:102A600003F56143C3F880211A607047FFF72CBE5D +:102A7000012300F10802C0E90222037000F11002F4 +:102A80000023C0E90422C0E90633C0E908334360EB +:102A90007047000010B52023044683F311880223F9 +:102AA00003704160FFF732FE04232370002383F399 +:102AB000118810BD2DE9F0411F4604460D4616460B +:102AC000202383F3118800F108082378052B0DD00B +:102AD00029462046FFF744FE40B1204632462946AB +:102AE000FFF75EFE002080F3118808E0394640467B +:102AF00000F03AFB0028E8D0002383F31188BDE8FA +:102B0000F08100002DE9F0411F4604460D461646AF +:102B1000202383F3118800F110082378052B0DD0B2 +:102B200029462046FFF772FE40B12046324629462C +:102B3000FFF784FE002080F3118808E03946404604 +:102B400000F012FB0028E8D0002383F31188BDE8D1 +:102B5000F0810000F8B5154682680669AA420B4666 +:102B6000816938BF8568761AB54204460BD218468B +:102B70002A46FEF77FFCA3692B44A361A3685B1B75 +:102B8000A3602846F8BD0CD918463246FEF772FC01 +:102B9000AF1BE1683A463044FEF76CFCE3683B4407 +:102BA000EBE718462A46FEF765FCE368E5E7000018 +:102BB00083689342F7B51546044638BF8568D0E967 +:102BC0000460361AB5420BD22A46FEF753FC6369FD +:102BD0002B446361A36828465B1BA36003B0F0BD70 +:102BE0000DD932460191FEF745FC0199E068AF1B13 +:102BF0003A463144FEF73EFCE3683B44E9E72A46A7 +:102C0000FEF738FCE368E4E710B50A440024C3612A +:102C1000029B8460C0E90000C0E90511C160026147 +:102C2000036210BD08B5D0E90532934201D1826834 +:102C300082B98268013282605A1C42611970D0E9FF +:102C400004329A4224BFC3684361002100F09CFA19 +:102C5000002008BD4FF0FF30FBE7000070B52023D7 +:102C600004460E4683F31188A568A5B1A368A2693E +:102C7000013BA360531CA36115782269934224BFD2 +:102C8000E368A361E3690BB120469847002383F30F +:102C90001188284607E03146204600F065FA0028F2 +:102CA000E2DA85F3118870BD2DE9F74F04460E4630 +:102CB00017469846D0F81C904FF0200A8AF31188E6 +:102CC0004FF0000B154665B12A4631462046FFF706 +:102CD00041FF034660B94146204600F045FA00280E +:102CE000F1D0002383F31188781B03B0BDE8F08F87 +:102CF000B9F1000F03D001902046C847019B8BF328 +:102D00001188ED1A1E448AF31188DCE7C0E9051129 +:102D1000C160C3611144009B8260C0E90000016191 +:102D200003627047F8B504460D461646202383F328 +:102D30001188A768A7B1A368013BA36063695A1C07 +:102D400062611D70D4E904329A4224BFE368636172 +:102D5000E3690BB120469847002080F3118807E013 +:102D60003146204600F000FA0028E2DA87F31188A5 +:102D7000F8BD0000D0E905239A4210B501D1826860 +:102D80007AB98268013282605A1C82611C780369B8 +:102D90009A4224BFC3688361002100F0F5F9204600 +:102DA00010BD4FF0FF30FBE72DE9F74F04460E460C +:102DB00017469846D0F81C904FF0200A8AF31188E5 +:102DC0004FF0000B154665B12A4631462046FFF705 +:102DD000EFFE034660B94146204600F0C5F90028E1 +:102DE000F1D0002383F31188781B03B0BDE8F08F86 +:102DF000B9F1000F03D001902046C847019B8BF327 +:102E00001188ED1A1E448AF31188DCE702684368D2 +:102E10001143016003B11847704700001430FFF7F9 +:102E200043BF00004FF0FF331430FFF73DBF0000F9 +:102E30003830FFF7B9BF00004FF0FF333830FFF7ED +:102E4000B3BF00001430FFF709BF00004FF0FF319F +:102E50001430FFF703BF00003830FFF763BF0000F6 +:102E60004FF0FF323830FFF75DBF000000207047A1 +:102E7000FFF7D4BD37B515460E4A02600022426006 +:102E8000C0E902220122044602740B46009000F1C0 +:102E90005C014FF480721430FFF7B6FE00942B46AD +:102EA0004FF4807204F5AE7104F13800FFF72EFF85 +:102EB00003B030BD203F000838B5C36904460D4655 +:102EC0001BB904210844FFF7A1FF294604F11400AF +:102ED000FFF7A8FE002806DA201D4FF48061BDE848 +:102EE0003840FFF793BF38BD024B0022C3E90033DF +:102EF0009A6070471C370020002303748268054BDA +:102F00001B6899689142FBD25A6803604260106066 +:102F1000586070471C37002008B5202383F31188C0 +:102F2000037C032B05D0042B0DD02BB983F3118820 +:102F300008BD436900221A604FF0FF334361FFF779 +:102F4000DBFF0023F2E7D0E9003213605A60F3E7B9 +:102F5000002303748268054B1B6899689142FBD873 +:102F60005A68036042601060586070471C37002048 +:102F7000054B19690874186802681A605360186173 +:102F800001230374FDF744BB1C37002030B54B1CF4 +:102F90000B4D87B0044610D02B690A4A01A800F0F7 +:102FA0001BF92046FFF7E4FF049B13B101A800F0D2 +:102FB0004FF92B69586907B030BDFFF7D9FFF8E723 +:102FC0001C370020192F000838B50C4D41612B69C2 +:102FD00081689A689142044603D8BDE83840FFF7FB +:102FE0008BBF1846FFF7B4FF01232C610146237401 +:102FF0002046BDE83840FDF70BBB00BF1C37002062 +:10300000044B1A681B6990689B68984294BF002023 +:10301000012070471C37002010B5084C2368206938 +:103020001A6822605460012223611A74FFF790FF2E +:1030300001462069BDE81040FDF7EABA1C370020C0 +:1030400008B5FFF7DDFF18B1BDE80840FFF7E4BFA2 +:1030500008BD0000FFF7E0BFFEE7000010B50C4C14 +:10306000FFF742FF00F0AAF80A498022204600F04C +:1030700031F8012344F8180C037400F09DFB002381 +:1030800083F3118862B60448BDE8104000F042B8EE +:1030900044370020483F0008583F000800F012B9AC +:1030A000EFF3118020B9EFF30583202282F311881A +:1030B0007047000010B530B9EFF30584C4F308047D +:1030C00014B180F3118810BDFFF7BAFF84F31188A3 +:1030D000F9E7000082600222028270478368A3F150 +:1030E0007C0243F80C2C026943F83C2C426943F8FB +:1030F000382C074A43F81C2CC26843F8102C0222D3 +:1031000003F8082C002203F8072CA3F118007047DD +:103110002906000810B5202383F31188FFF7DEFF8E +:1031200000210446FFF750FF002383F31188204657 +:1031300010BD0000024B1B6958610F20FFF718BF3C +:103140001C370020202383F31188FFF7F3BF000012 +:1031500008B50146202383F311880820FFF716FFE6 +:10316000002383F3118808BD49B1064B42681B69EF +:1031700018605A60136043600420FFF707BF4FF0E8 +:10318000FF3070471C3700200368984206D01A6849 +:103190000260506059611846FFF7AEBE70470000EC +:1031A00038B504460D462068844200D138BD036816 +:1031B00023605C604561FFF79FFEF4E7054B03F178 +:1031C0001402C3E905224FF0FF310022C3E90712C0 +:1031D000704700BF1C37002070B51C4EC0E90323A8 +:1031E00005460C4600F06EFB334653F8142F9A4206 +:1031F0000DD13062C5E901242A600A2C2CBF0019C8 +:103200000A30C6E90555BDE8704000F049BB316A97 +:10321000431AE31838BF1C469368A34202D9081921 +:1032200000F04CFB73699A6894420CD85A68AC6001 +:103230002B606A6015609A685D60121B9A604FF09F +:10324000FF33F36170BD1B68A41AECE71C37002044 +:1032500038B51B4C636998420DD0D0E90032136039 +:103260005A6000228168C2609A680A449A604FF0EE +:10327000FF33E36138BD2246036842F8143F002162 +:1032800093425A60C16003D1BDE8384000F010BBE2 +:103290009A688168256A0A449A6000F013FB6369A2 +:1032A0009A68411B8A42E5D9AB181D1A092D206A7C +:1032B00098BF01F10A02BDE83840104400F0FEBAA0 +:1032C0001C3700202DE9F041184C002704F11406AA +:1032D000656900F0F7FA236AAA68C11A8A4215D80C +:1032E00013442362D5E9003213605A606369D5F84C +:1032F0000C80EF60B34201D100F0DAFA87F3118855 +:103300002869C047202383F31188E1E76169B1424E +:1033100009D013441B1ABDE8F0410A2B2CBFC0187A +:103320000A3000F0CBBABDE8F08100BF1C370020A6 +:1033300000207047FEE70000704700004FF0FF30AC +:1033400070470000BFF34F8F024AD368DB03FCD401 +:10335000704700BF003C024008B5094B1B7873B9A9 +:10336000FFF7F0FF074B1A69002ABFBF064A5A60F1 +:1033700002F188325A601A6822F480621A6008BD2D +:10338000D8380020003C02402301674508B50B4BAC +:103390001B7893B9FFF7D6FF094B1A6942F0004238 +:1033A0001A611A6842F480521A601A6822F4805234 +:1033B0001A601A6842F480621A6008BDD83800208A +:1033C000003C02400728F0B516D80C4C0C49237875 +:1033D0007BB90C4D0E4608234FF0006255F8047B74 +:1033E00046F8042B013B13F0FF033A44F6D10123C6 +:1033F000237051F82000F0BD0020FCE7FC380020CD +:10340000DC380020703F0008014B53F82000704763 +:10341000703F000808207047072810B5044601D9FE +:10342000002010BDFFF7CEFF064B53F824301844A0 +:10343000C21A0BB90120F4E712680132F0D1043B43 +:10344000F6E700BF703F0008072838B5044628D8C3 +:10345000FFF726FEFFF776FFFFF77EFF124AF32302 +:10346000D360E300DBB243F4007343F00203136163 +:10347000136943F48033136105462046FFF762FF6A +:10348000FFF7A0FF094B53F8241000F03BF9284642 +:10349000FFF77CFFFFF70EFE2046BDE83840FFF740 +:1034A000BBBF002038BD00BF003C0240703F000899 +:1034B00012F001032DE9F04105460E4614464BD1AA +:1034C0008118334A914261D8324B1B6813F00103D3 +:1034D0005CD0314FFFF7E4FDFFF73EFFF323FB60C5 +:1034E000FFF730FF314640F20128032C18D824F0B2 +:1034F0000104294E0C446D1A40F20118A1423369AF +:1035000005EB01072AD123F001033361FFF73EFFEA +:10351000FFF7D0FD0120BDE8F081043C0435E4E76D +:10352000AB07E4D13B6923F440733B613B6943EA59 +:1035300008033B6151F8046B2E60BFF34F8FFFF718 +:1035400001FF2B689E42E8D03B6923F001033B61F9 +:10355000FFF71CFFFFF7AEFD0020DCE723F440730C +:103560003361336943EA080333610B883B80BFF35F +:103570004F8FFFF7E7FE3F8831F8023BBFB2BB42F7 +:10358000BCD0336923F001033361E1E71846C2E799 +:103590000000080800380240003C0240084908B515 +:1035A0000B7828B11BB9FFF7D7FE01230B7008BDBC +:1035B000002BFCD0BDE808400870FFF7E7BE00BF55 +:1035C000D838002070B582B0FFF76AFD0E4E054670 +:1035D00000F078F93268904237BF0C4A0B495168C5 +:1035E00014682EBFD1E9004101315160041903462E +:1035F00041F10001284601913360FFF75BFD01991D +:10360000204602B070BD00BF0039002008390020FC +:1036100070B582B0FFF744FD104E054600F052F938 +:103620003268904237BF0E4A0D49516814682EBF68 +:10363000D1E9004101315160041941F10001034613 +:10364000284601913360FFF735FD01994FF47A72F6 +:1036500000232046FCF7C4FD02B070BD00390020F5 +:103660000839002010B50244064BD2B2904200D176 +:1036700010BD441C00B253F8200041F8040BE0B226 +:10368000F4E700BF502800400F4B30B51C6F2404F6 +:1036900007D41C6F44F400741C671C6F44F400448E +:1036A0001C670A4C236843F4807323600244084B70 +:1036B000D2B2904200D130BD441C00B251F8045B3C +:1036C00043F82050E0B2F4E70038024000700040B8 +:1036D0005028004007B5012201A90020FFF7C2FFD2 +:1036E000019803B05DF804FB13B50446FFF7F2FF41 +:1036F000A04205D0012201A900200194FFF7C4FFD8 +:1037000002B010BD70470000074B45F255521A60D9 +:1037100002225A6040F6FF729A604CF6CC421A6060 +:10372000024B01221A70704700300040143900200B +:10373000034B1B781BB1034B4AF6AA221A60704751 +:103740001439002000300040034B1A681AB9034AAC +:10375000D2F874281A6070471039002000300240F7 +:10376000024B4FF08072C3F874287047003002405B +:1037700008B5FFF7E9FF024B1868C0F3407008BDB9 +:103780001039002008B5FFF7DFFF024B1868C0F3BF +:10379000007008BD10390020EFF3098305494A6B1A +:1037A00022F001024A63683383F30988002383F31C +:1037B0001188704700EF00E0202080F3118862B686 +:1037C0000C4B0D4AD96821F4E0610904090C0A4345 +:1037D000DA60D3F8FC20094942F08072C3F8FC207B +:1037E0000A6842F001020A601022DA7783F82200A8 +:1037F000704700BF00ED00E00003FA05001000E094 +:1038000010B5202383F311880E4B5B6813F400631B +:1038100014D0F1EE103AEFF30984683C4FF0807356 +:10382000E361094BDB6B236684F30988FFF7E8FB50 +:1038300010B1064BA36110BD054BFBE783F3118864 +:10384000F9E700BF00ED00E000EF00E03B060008F4 +:103850003E06000870470000FEE700000A4B0B48D8 +:103860000B4A90420BD30B4BDA1C121AC11E22F0EA +:1038700003028B4238BF00220021FDF70DBE53F832 +:10388000041B40F8041BECE79C400008983900201A +:1038900098390020983900207047000000F080B867 +:1038A0004FF08043002258631A610222DA607047A9 +:1038B0004FF080430022DA60704700004FF08043F1 +:1038C000586370474FF08043586A70474B684360B5 +:1038D0008B688360CB68C3600B6943614B6903628B +:1038E0008B6943620B6803607047000008B5234B87 +:1038F00023481A6942F0FF021A611A6922F0FF0296 +:103900001A611A691A6B42F0FF021A631A6D42F0CB +:10391000FF021A651B4A1B6D1146FFF7D7FF02F124 +:103920001C0100F58060FFF7D1FF02F1380100F5BE +:103930008060FFF7CBFF02F1540100F58060FFF7D4 +:10394000C5FF02F1700100F58060FFF7BFFF02F1D3 +:103950008C0100F58060FFF7B9FF02F1A80100F5C6 +:103960008060FFF7B3FF02F1C40100F58060FFF74C +:10397000ADFFBDE8084000F097B800BF0038024036 +:1039800000000240903F000808B500F017FAFFF76A +:1039900065FBBDE80840FFF7D7BE00007047000098 +:1039A000104B1A6C42F001021A641A6E42F00102C6 +:1039B0001A660D4A1B6E936843F0010393604FF043 +:1039C000804331229A624FF0FF32DA6200229A611C +:1039D0005A63DA605A6001225A6108211A601C2079 +:1039E000FFF72EB800380240002004E04FF080427C +:1039F00008B51169D3680B40D9B2C9439B0711615F +:103A000007D5202383F31188FFF748FB002383F3B6 +:103A1000118808BD08B5FFF7E9FFBDE80840FFF7CA +:103A2000EFBE00001E4B1A6962F0FF021A611A69AC +:103A3000D2B21A614FF0FF301A695A6958610021F9 +:103A40005A6959615A691A6A62F080521A621A6A8E +:103A500002F080521A621A6A5A6A58625A6A5962A5 +:103A60005A6A1A6C42F080521A641A6E42F08052FE +:103A70001A661A6E0B4A106840F480701060186F56 +:103A800000F44070B0F5007F1EBF4FF4803018671F +:103A90001967536823F40073536000F06FB900BFD7 +:103AA00000380240007000403B4B3C4A1A643C4ADC +:103AB0004FF4404111601A6842F001021A601A681E +:103AC0009007FCD59A6822F003029A60324B9A68FC +:103AD00012F00C02FBD1196801F0F90119609A602B +:103AE0001A6842F480321A601A689103FCD55A6F42 +:103AF00042F001025A67284B5A6F9207FCD5294AB7 +:103B00005A601A6842F080721A60254A5368580455 +:103B1000FCD5214B1A689101FCD5234AC3F88420B7 +:103B20001A6842F080621A601A681201FCD51F4AB6 +:103B30009A600322C3F88C204FF00062C3F89420EF +:103B40001B4B1A681B4B9A421B4B21D11B4A116815 +:103B50001B4A91421CD140F203121A60164A1368A4 +:103B600003F00F03032BFAD10B4B9A6842F00202C9 +:103B70009A609A6802F00C02082AFAD15A6C42F450 +:103B800080425A645A6E42F480425A665B6E7047B5 +:103B900040F20372E1E700BF003802400004001069 +:103BA0000070004004194002083000240094883856 +:103BB000002004E011640020003C024000ED00E021 +:103BC00041C20F41084A08B5516913680B4003F020 +:103BD0000103536123B1054A13680BB1506898473C +:103BE000BDE80840FFF70CBE003C0140183900203A +:103BF000084A08B5516913680B4003F0020353618A +:103C000023B1054A93680BB1D0689847BDE80840D6 +:103C1000FFF7F6BD003C014018390020084A08B5FE +:103C2000516913680B4003F00403536123B1054A43 +:103C300013690BB150699847BDE80840FFF7E0BD34 +:103C4000003C014018390020084A08B55169136842 +:103C50000B4003F00803536123B1054A93690BB18C +:103C6000D0699847BDE80840FFF7CABD003C014055 +:103C700018390020084A08B5516913680B4003F051 +:103C80001003536123B1054A136A0BB1506A984778 +:103C9000BDE80840FFF7B4BD003C014018390020E2 +:103CA000174B10B55A691C68144004F478725A61B5 +:103CB000A30604D5134A936A0BB1D06A98476006ED +:103CC00004D5104A136B0BB1506B9847210604D5ED +:103CD0000C4A936B0BB1D06B9847E20504D5094AA7 +:103CE000136C0BB1506C9847A30504D5054A936C2F +:103CF0000BB1D06C9847BDE81040FFF781BD00BF05 +:103D0000003C0140183900201A4B10B55A691C6854 +:103D1000144004F47C425A61620504D5164A136DBE +:103D20000BB1506D9847230504D5134A936D0BB121 +:103D3000D06D9847E00404D50F4A136E0BB1506E56 +:103D40009847A10404D50C4A936E0BB1D06E9847E6 +:103D5000620404D5084A136F0BB1506F98472304CF +:103D600004D5054A936F0BB1D06F9847BDE810405A +:103D7000FFF746BD003C014018390020062108B578 +:103D80000846FEF75DFE06210720FEF759FE0621D4 +:103D90000820FEF755FE06210920FEF751FE0621F8 +:103DA0000A20FEF74DFE06211720FEF749FEBDE86A +:103DB000084006212820FEF743BE000008B5FFF7A3 +:103DC00031FE00F00BF8FEF751FEFFF751F8FFF758 +:103DD000E5FDBDE80840FFF761BD00000023054A8E +:103DE00019460133102BC2E9001102F10802F8D183 +:103DF000704700BF1839002010B5013902449042C5 +:103E000001D1002005E0037811F8014FA34201D051 +:103E1000181B10BD0130F2E72DE9F041A3B1C91A1A +:103E200017780144044603F1FF3C8C42204601D937 +:103E3000002009E00578BD4204F10104F5D10CEB46 +:103E40000405D618A54201D1BDE8F08115F8018D11 +:103E500016F801EDF045F5D0E7E70000034611F84C +:103E6000012B03F8012B002AF9D170476F72672EDE +:103E70006172647570696C6F742E41524B5F47506C +:103E8000530000004E6F20617070207369670A0054 +:103E9000426164206677206C656E677468202575C2 +:103EA0000A0042616420626F6172645F6964202568 +:103EB000752073686F756C642062652025750A0033 +:103EC0004261642066772064657363726970746F01 +:103ED00072206C656E6774682025750A0042616403 +:103EE0002061707020435243203078253038783A72 +:103EF000307825303878203078253038783A307866 +:103F0000253038780A00476F6F64206669726D77D4 +:103F10006172650A0040A2E4F16468910600000045 +:103F200000000000392E0008252E0008612E000830 +:103F30004D2E0008592E0008452E0008312E00088D +:103F40001D2E00086D2E00086D61696E00000000D6 +:103F500069646C6500000000503F00086037002075 +:103F6000D83800200100000059300008000000008F +:103F70000040000000400000004000000040000041 +:103F8000000001000000020000000200000002002A +:103F9000A000902A00000000AAAAAAAA50000024AB +:103FA000FFFB000000770000009009000100000501 +:103FB00000000000AAAAAAA501000080FFEF0000EF :103FC00000000000000000000000000000000000F1 :103FD000AAAAAAAA00000000FFFF0000000000003B :103FE000000000000000000000000000AAAAAAAA29 @@ -1027,9 +1027,10 @@ :10401000FFFF0000000000000000000000000000A2 :1040200000000000AAAAAAAA00000000FFFF0000EA :104030000000000000000000000000000000000080 -:104040000A00000000000000030000000000000063 -:1040500000000000ECC1FF7F010000000000000034 -:1040600051000000000000000000070000000000F8 -:10407000640000000000000040420F00FE2A010022 -:04408000D204000066 +:10404000AAAAAAAA00000000FFFF000000000000CA +:104050000000000000000000000000000A00000056 +:10406000000000000300000000000000000000004D +:10407000A0C1FF7F0100000051000000000000000F +:1040800000000700000000006400000000000000C5 +:0C40900040420F00FE2A0100D204000094 :00000001FF diff --git a/Tools/bootloaders/BirdCANdy_bl.bin b/Tools/bootloaders/BirdCANdy_bl.bin index d99f6c5a275f22cfb6cbece63525ebf7d055f13b..4c3fd6689d9b5314097a9b144904d79e256c41f0 100755 GIT binary patch delta 5069 zcmY*d3wTpS_Mf>oX>QX8=?l`NkK3e$w4o(!K+*>`P0OWi#ER(3(@lA-0YoaWyJ#1Z zQuyqj$6xEX3lv>taeZt*p%1!StQc6<7mB7Je~YjyENY;j-cTU7dENiH0l#lI-}jqy z=FB-WXJ*cvnVazMcD3OWq(JOpb{~c0ZCPe zDF>{q>MYR}G#4P|G$3y>Vtx(igqHubm3VpQfcX$2l+`2Yi8FNw&@U0WhiAH~xV^i9-< zQ1LashUTjT1xNGG33BfEkiq_{ZH5<{rE;=;3NKP!w&}w{AL1K=Z*jJps*Q+$FwFF# z{MR_d&kn|M_5F_waomhvEm+cCnqMrS{K*_*XI@vDn|E<4@3AqzmL&l-RWyo znS6Sp9h2AlOnO94Uz2T=A3S>v=?hyG^_c%;yt3!mRlfc37;1;)!+b+%8ORkexdP-Yscviq$>@%Xu+NcdY(?kXV(Pi9Nxyv*Z0Zerz}sloxVRr*8yFi!zJa z6D0qu)KS}mq)$0jkNNF_OIpL{|G4@ed2KtmAE&|=F1a5u*sgAAnS(qQf)h$>R*cIz z)6pq*W4%(RX+!+g@b^Bvs`vJ#X>sRu_Q~}LG3h_U7k#xmtvAzc#HR3OH6*f3TPDZW zBi>6kBNK=a+ntjtAr!ujf9*vZ-)QuO9GXHVVr-!hdj~-N>0eD5pz+Dg9C<{ zjo^?Q@FIXgVxR};X}}!-oT#REdg2bdxkTu6D}*~TEE?8tp^Vz$D?6}JJ&gFFa2zKF z4v}ez^E8-u2seBRe>(KDM{>xYjxE_Ve?TBziH1bXR}15ttk6}qNIp;e4Hn7lq^6v- z@yciAeyr@aHI1H%W6p=1jLukB%GJZl%Ew7xk|B-}#XvMnE+uKG*F_RevY4L}89!!j z_Z1>ya9@SHGWl5yIjckBNbtDEayw#m*&5^O^JXXJ_GJPL=%j$|F)N8U-+C%hdL z1CydmuNX)s7m_nwvtWJD59pE%1?4}ll@(;BfhHb3l5f{JyKLKg6kdwY56if>j-q^) zllPd8<=B^P*0ki*>o?OIvKv0pCBtw#gDiImf)??6gE)WGr*6!goAzWak2a*O_{-W! z_Ac8u!^Wp@?RdTqvf|Jwy1*be2HD{p!k*$(>p=0Z<9#}19?PeEqKG`ptCDl(fpZ6`ynNXNM zvOY&qw9A8ZShp_|hvf7A7V-z)o838#ik|nrBQOu`7L=ylLV}~im&W}7YbYHEX`QNV z6OeV&W~>6fV>)8DGl;1IUdnbUiHMQbStdmTc|pbEe$uP5C%D3j7Ys)Y5YfAcBE^Vz z5p&98sj1>;K1s$!j>l+An7o{lgS*I3%1rzRGL}+K6^2Pk>SX4pu%xF7lO?G|)Q>^Z zk;=NRhxDyR2+27?eJe7$ZrwSQ7Rw@%ACH1x2vOE5;2#2B+V@9*TC)$ZliP|C{2#t~ z!s}!vNd8N%N&zj+uVsLOt1JG&__-!G$~QR89ql&eJjG`VMyIR}n~&UfR~sES&$b*x zk9)R(@d5zFj|?I|a=eJpZMTWy7Y`yNw`-hoyS8J$rrk!JSJh!Y?1Ap*NcxhUWk4}k z#u=O}v;;(08sLsywOA>h8MGWjo&{ukS}tBf{+#xOb_wWDI0B2>1Ich;f6q^2@oi3> z6C&pigrpm>n|PpHO|GYBCQA1^#Ty1G9u1PjjHxREa3n?ZH^L*w0hiGAHmu<~Y+2ri z1eqPbgb&N|Ckc4*)=~K~rr?5lB4kE!#=|wJCBr1NH(Nm~mPW_`w zDF$ATw!>v{!h3hHDUlC{@cwRrF;OhI>1U{YnTvy4Z(`H&KE{B(hipY>#lV@U7&sDz z!CADwCWqp4Dcfj$>LDBBY2vNQ#lbdz<0z#BH*9SJ^V&{9Wk;V1GIk+n0gt|n;OU2K z`F$Pm@B){VL&MvBi2c}?BH&Lc0KV_9eINPO@mTf@#F8+O!6nCDNir@MtiK8qF7rhy z5FuY@*5@6MFf**hG_c5NP0^Qn8|K7Z{kZrKBPcvPdkt4 z-9jDkHI9Q$GRS7R>V2WjpNv^k`>vnOwenxPPmXP)`(gYMw_=~MwP|!;Dk_}rJ)@}k zk~Ugzdfh({U~D~wGlC&9KTn_eI#`~)P}GVm{_PvzabheU<4Sqq*L@4g^Lg`YCFkD} zs@R{MM|x>0(iCgmMh_o!D&JnPj`L-A+Cz-lz zMd$aePGsyQvvkGsu8}U=Ub0S?k-7T&b-WTi0c8?e0cA33hH?UWy#Ezl8irl$)h~~q zbQF1{6#D|n&Y!EeF(~nS$x9R^M~28n|!d$(pm%>v@0w52vh>d> z>ZK@EH3?3=A7_#B;#?>f7C#1MZ*eC5z)%)BT|6@`YbIjjrx9gInLKR>d9p}N$s|d! zq=3HpZWd`Psm8C66D4WNI#9I0bF#jg{Hx>*P}x4M7RpbiHKj>?vEfzm&aa^ zPZ6W305_0%rVRW5dBRi(nw_SaIjW&9XPdKNwX&|uf%vr0@>N~V8to3-cjvp+*_!xb zcJ*El_A4prCQ;QPKKicIxAeWIegI98`P1W8u0ni(WR@1-uSs=jVf?4>LZ)kz9DllW zJ{}<-lvYBC%c>`aEr`7caDda;K3`x_%N$2F`9)coOA;vzmI1iHM#R8m$VR_`gt$da zZ5-2|bzDIg`f6{X4YfQ)r!}Xos=ehlr!~tt79w{U=)ES!LwETq#8-|RK=wP3B}Z9K za#{=w!8IvefT)VHO5>!$(UT@1hcuMV+2Qz`U5%Z#=8-bH)|m#4OT48`V&L0|7`R^? z(}x^lz$BJ()|=QsPgRfPLmn%9FtKi}1|;eOHuEa^b*v~F)`O+FS+aox7{RQx3bHEM zjBkD2if@N2SLttAH&ixPuBshn=*s4Kf^1ER!!y?OycodZJfriEwyR!Hf=()-lPu`u zPz09CCz+X>%`r1`b3#X@`>v5MTHR49P?es(A=`UibIj7*d>KOW?~r5}a@d?<{3?p| z2`$VqWDI+cU1h9Hix}{|k31u=2`vL#d&I!%Nb-a5X~AzIKbZ^liz9d+%TYb>7V)5R z#4>=pdUX+1Oz&Y*IlWLb1N7iwxE|?QE*$tQ60?ZulP%Lvxzd59?b=P+UfSMx=Px>q z1FmL}ArrFF@QTqKr5n!(8x4|N6_Zm|$_|&<`ja4qE7(L?nE^3yO{DB<=kYNsOhM9< zWZ&GAp?flTxwOk08rSG}Of@cdn%l?X$>AC8)YTA~e%}L`N$<}WWLhiWhHsY@F?*q# z7|6c+e#&q|G%y2*p9CKxD?`!>LFOW_%q+yEZHTx7Oj)o7d>IHFIx8^ zXcRUsirKi=Ya9iQ^PLN1;d6PgES)%fPTzP^*v3jdx|c~krh+WDvCBd0P71qNxNMf4 z6kcMrpsj_^7_B{TSlgK5)I#`X4O?&0sT#+`#xy6*PoQaIa5#ZHR$h{E*KHY6A#k0^ zv{EV)$|o_+@-s>_kb{ zKUA!Jg1g%Ss~eZc3^0)HmPX_96l^rG5GVumQK0ca=K)m$oeOjl(1xhA?s0hr;A`K9 z$c0TVg}-8jf2o1lH2=+sL6>CjS} zP<5(SNAa|86lxvEw&OgOl^=0X2hs7l{F%B2WE@4?SEs%|6Fv$Gk8-pNBUr29^SnC3*pYz2J%jCKO`fc*dBz|5AsoP?vrh#3Y* zOAu24Sl-gfCiJj~(WfJ3Bj7S%drPN^N-k=7n1{VBo%wt5JgR@LCkCYn_UmZfc zH4w#BcP~E8aWgwpz=j^6_%{WVp2;B{#hZn=PQ>T=5x*F8vm%Jkf!U9rM05BDA^YRE z8z_Et$mco2q4XOb31{aqGvsk|FtXvV(7)~xxoaN8#|xF(!`8dQh-ds?c~C#`N0m$t zL?~V%z$*D*IMGJw)D4I)By(joSVvl9`HR)C+C+e|2kDgt2?xdz+D89H=csq6oxgP` z`%u5~ZDA^`zEW3rx&!e$BJIxensH;{aMZCQ4kMRkK>8vM^GQLeHx42GPMD|}!$K6q zv%DUL#fGaRRH1Z6j17}dyG$%1S4SipB!`Da5S!f;RgHOiytJe59>2NqC-XsaLv0NgT2Gjq_Pxdf7sg?jqlP77m23EX<_sMctE~{T=p#9Q(gPt~E{;dF+ zFHcgg3E+JaFGDL0S$fRB8Xz0w24zmT8eBEO5;MYx?+KI7R4|DE%v4cV!c+HF-W{hEcT}a z654gpQ&04ifl2Ka=oL-$B0EW^Kvhij}0b+ z_EJuq_1kgsPIL-2G)`Kh4b-`Dawa;LMf^FzB}Q%cC#(Nc-@Jo+m=2q`xC}Aarfz7M zi~3C@Hl`rqRlk%oot$PfR!18&&4_O$_L#}q-J!2Ml^<9hC0dD9Hjq-I&@s))oUR)2 z?-5rF>pB%GSL?qnkd00#J@|{$LXo=0QvVg1GbTq zajfg*fZ&@FVLAohq>#}muWgIQW>qs{+Z~CX%!+mEzR9@p$C(f%^aHvhK>_JMBUS`5 zrpL~sMKQI>&K7I*V07)$O$iP1YV9U^y}ahKAr2nM1^VtWmOGHkp!ovgUpUozwCV!9 zSvql^B!o=5=_5i#pOyD zkEo!EmOe>pPzE;h**3L<;{P6CI^$WkSt6!q^SRM;vS5n+{@u9qV;AW|qqHIp;wzT5 zp9OY4i3=jMB(O8o5e&Kbjwkgl*>F>usCZd(wQFfwe0I{!)oHSvo&Csw4NH@8Fv^io z(;4Kw3DwQkj|Nc=893@^9J~C{rd|G+7j}A7+$dzAn0*RB;|!#Gi$Tl`;KgbpRUk%; zlNe_->ve3g#*1a$-tzk!BgB%R#^q#bf-NQ-l zY#?-Yf(C+6Ewiczzwyt5`h9f#wjY`HSuI0||NXdZkKlViR!*9Y?~%4i#Z-5Y{C!d; z^HxZlpfyM&svN2;NK7hS!NwrlWQ0uFA7Go1(e?PJFH{rnwdn8j;6Fl?Wd`sT;Klu| zBF!q5OI(?_3Nees)i(5CzFm;dm)DuxC_T$*-nQAw3}e1cFghhw*nIqndydiZXs7|o z`3A7922i}U7kQCmFG5e;Cd?1^BP6wHoKl;1TbpLHl^Rx7VgBuYn0>COFWMFL$m%3q zmd2@rp^XAW3Any{MHVQ~{Y5Z&HJP3`70)AeiGQ6s4-DduBb}t9VXkBw9K^rx&h3nALq@1xZdtDJCgW8=ok)g5wCLnoo|0y9bVuI< z>I1P_Cc;}I_%4PGT-<)CSFA+G!YfmZ8(LxWa;s>5D!r1QL$m8pTP2SwRyizOHn{q7 z1ZUj#Ag_XkH%`txRF>Fzmpf)YSI4OzDx(G8-G~fw;nKj<>Gy?EUdbSCD{ZC>acz{B z!Sra9;-x^dSIHf>QeI45>%uoFPIf27k7KZiSy4GBx_%r6HXLv&I{T$FUL4Px8N!f&dN8(ep_(Nh$?ZhX^*QphFADNoA z2G@7*O52R_L6V^RrT#**nm=19B^Lf{a||Rq;{Viq+DpaL1Gur%YTC>k|8r;^KRk;In+i8cSE$9$~AA3ab z{ekEW`siJLOUGg7E`NoCEHR|G=6iyhE{|Czb&U>9weWv)pC8*ocf&KUxn=u}P4@fy zC!y>a1J`5~f1{0-M_1hcoYdI#MJio-yQ`2Z<|Gq|^=$xU#op3EU+`TC1UHE@VY8Sz zZB+0*T9@o;-&VRgr-?4<-Epm}0JL0zAQ>~T$tS>a=ypyMF8S6ozT@1O9OF`H_N}f( zB%fUbchaM9;}G2zo!;fQx|n-(Bnut^!uM}nJrcl`fHmVU6$n%JM9 zL%IXsH#w2pmS?P#`+8BoSY+Jfaz>r(%U+S+Mp85DWWRgHzmYjd>Jqg`Pv#u3dXj0! zjor^rJCE_FM4sJEjh`WBvN`FeVEGl1=V+ruu6SB^PEHA)iNl9i8LJN0A9lX1J?zNh z{C$dHHCM<*e2+uNwEu9HaHgt=?_ub!v$Sf|qvmp8#6$KQS$v)J8MWGrA-w;N^O&}T zD*(N-6M7>aQR`Iqn6Z2Fvoo3MGH4)!FRKSRGx}>OlNhfc&~pNB`F;1?enQ;Tz;aVFo@$Mhmm$?We>; zRrEI1ya-<+Zgoz62}*->_|wUxymzE-5F}j6#YZi2z7y`lf zXE+extDuasi1Ye;Ch?d~WC^gDe=a@bGy8*7^G+WfGIevsvaDG_GPFTh<%v zjjKv4jr9xsl3zqQ`p4|M1)oZwou61o20#OBjbNJxpE`FuqCQ*Cf<9Xxv(066KUEJ! zrf)0oQ>90{+O6jY*a;)H{=Xr+F2bcj5M&H0qAcRdm>Q-hrZKBids6U?Mlc)G!1N(w zXh6JpF%5z*cn0-ff?zlFyw@T4-VMjim%_pR+IXRJk9BsxqBf>xfiuyW*FQLdDli$I zsbbfL@qQggb%+Nr50?J_wO$fdPFQm!epa@o2K3-|v<|U4F628A5iOqTlZCUcEWpJ` zN87ZG+D_V5`_q3IG>(S3q70dkMPeKBZzyU*)_&TC4VsCs0Azv%!8m2J^X_%${Ts62 z+W=(nmCz`DB_OhO_I`>95na)|k;zTdXOAmPhm?jbk#YD=mo^U6LTB=$6t^!H|zjOJtfes0>>qWj~YehJAjj^`vag&lf)(fp|vgnH~nJ)CZ9$ zy-XMJy$_TDx*4b(s0(N`&?cZ$fc`QfMmkEL1^gRl;4h0F!8Zr~BZ%>fx^TAm1>acs z>29ngm&>(S(*0d|KIS&%B4!ofGe8UA0AM>{BcLA830PCV>U9LaRj<7EZz3&%Eq(!t sn3&oa`UmpJvNU9I6V#f}Mf}*0R*cGs0xr!A;uMUb5Y5BeHIWqyh-}nE%?|J@to_%JowbxpE?X}z5 z<(#bfC2`NO#AW{M9kq5%vuXBTnqPBj1zM`+(}bqYY6%FfC7>BCf#vntZLI-KV*oAw z1SvEvM3=ZPM=}QBehF~xdJ9prp(+hGm4Z$BtpOy6I^c6LlhQM@(DWG-z z>nWY3PR;+VO*011xk?f?8z^pt<}Q)dbBcNtx{3BQMRxB5d5%|UqX4>iRuf{rnW zfBT1e%r5c^Yo10~p(IU%m;r0hvZk4c%SB+ZSEyrlQPXzSvEzGXNI#~1aeS|)m3|)@ zq=))-ID>lV+75g0g)Ie5ch>jbA*%SyZ|V{1l&k7{?d;f~g@zqo8?67uJ68*hI%=%f za@Pd4(2&E9VB-_xwgiK{*QbF~-=T*lc4)czLCvp+QafVSYoXE(Ep+oyEp*v&Ep++d zPJ3|U(^{zivC#+rK1<^F(B8E{E%*Jsw&3WHFm~*14oGIX#LWetryp4cS)DYLZDLN(YahKT}ogH!ZO@T1h}2;L{P zEN`|A>YkzAbqZfe_Q);kw9seAA8rl&^6%`IQg5!W{MhmM+Lq#ALAI7v9TcI@j@S>x zXj-WJxbhkzd;InzqR)Z7MvlmS`+Hew@8xL{H{ZLVJV(3v?eA@K?f1O*y_QpWxwBD( zUjL(R-E;dw3m*SfYoGUf)txuL@%9a~sx<$V2n`6co{SDA1~ZYz@ITZ-LoA(e>XGcr z#0tGACs?pm&(=F^J^fiSTZD!knF74i#5;kv_sq`Hf$x9h?myujJ@ZU_#SyW>cEHfR z*#%i5RCGkOxEXDC{eV(T=>5a-4+j8Mo6y^b6CWn! zJQLau&M_uuy9qykSOwf7HEG@9_lCF*2-mMWeV{%)NU8T9R&| zZc&mWLW?@uXv=@xG4187`AdR7m7p>w4bTkHlrn49P;G0q5>cXJ(9}Qd!SbhjG!-|x zvs345p?ycc+bhmr|J$CPj*qjO@*AT}?7xnFws-LLfnO5qZ?1RlNFV0$8}ofZf1?)K zq2@%19ItV#GUyE2Q93`YC61~4qeSKf4;;O3Z#0PRIVy7R+N*74T~ZJGK+X89ljDyt z=0c}-gqsiyt@F@2{JxFwI|6#x=ttK%9&ZnNAoXfAf34t*ojF&@;ElJGIc3 zqmE$bp~qBDj~J6GLM^yY*sM-@P zQsup!>S(m6uDZ6%-^Rn~G+n&eFHKoAysnZAlNlh`L?%Xu~Vjs9P$F z$-%Ex%Yb&S7HA%{X59q*rXa2K`_7?FBV^9m7hyiSd3e)%9oh@e{`U8_`1cx{8b@zB za`gArLzZrJ=fve?Ovrrd$gjWYN4iaUaf4PgV23Qwg9mrt-Vs$ka9BXK#fr4H{9{^< zxVgHiW|($!O=C^d=!5NJIXG4~`R2u~?}c`ECVK4yhwW$|tB#C5_}ZLxM}GZ99!b#c zza`D`9$pYCYf5R6p;af9!a)!)U&t!(DzuM^ww=y$Qj8!-(u0G}OTQ zSS>K`O5sI(F=qQkhGz6boP%JTM>A+>8=He#<`sVNgj(+vO;K$}oy!WIE(t3}o zN%5jc{&ZL^ju-y6ACBZ)UX12B^l;AQT5;RK-ycgbhZkyeD*X{n2Kpi1j5wRXIQ#}O z?(*l)FN!*_{ec&Ev>LH236*BsW=Ur~DKWvhM!80ow`;ittyPN`)EghmP)0{_tF1v) zZTzi$U~$VY@6CEnJ8SDgFi${`q1~Ic16s>Xn)X|JxeQI*3{R&iwbkB~w)nQx1@-b{ zZ-WRO-3swTNl(7&C!Hg0!GcylQVw;(M2J1NuR0(@#^&nhw5=1=q6E=hOj7qHhLK*e&Z;ju#=U^BNBd#ENDVHoPv6npVQUh%h{tWY$sJe9g8d%->isdYTd321V5jJp80xg`+qq}&?!h|;>+3>L_2xeNAN=9VQ2nOrZeo%U zbJRbRL~)x@ko>W$z_Y`R5QX4Dh(?GhNZcCmm%|hu1TR7&LJEQ(0S*uNK>M*5ArT=3 z!H-a0uykvzKL#`|1UEty0%b%aXqxxQrq-ON-&k_?uP*t<)n4^(l4uj-)aYb!rRb*K zP8M0hrhZNq`EAY4;PRkWq=jy3FI^vVn^vraZfJLwR_=Bl@L-Vs^0+Nn{q)R`wE2Vj zy61kB*K^KF#9-BV=*(j=D~p1dl|eV;SZn$V8#)igVGREDxPR`-4y{~NCmwDO>7nlW z*`a~DK3lsDL)xIpE!Dv+)K&U%0mrCKJK)hVcp1h|-rCWjc6Tt?5KQ^W>+U>s@9_ng zPF~z$W!B@^_ATPJhdYdN>4%yJ47t)b@$l8Zi1HUU7zJX-WA#PvbspMvyz|h;#cYkT85#G4X9^m%H-t9xgLgzYjksg5VR1!GK~$`e6Ob@>Oy{0Nr`T%oNgHwgdnR-mW&8P|# z%iul`4no3oi}2ba5S9bJR2@r2wcV;NPZMcku~KPby|`EnOBaJsrPrs6=^|s(AL(L& z5EIpvnc~u{o%2&dFATQd902asEvD|aT#Y5xnz*-J?AQ|Xre0giwse&hWbTSLgfzqwk<8T+m5ub zTB6r>9;#9|_(fLIEgiWSDFf>kgoJ)WM+4%6TJC98oBblat#R*>D_{RL5WnGv_qqo{ zpR~X9>q7kr=&`%qxw823(fe1%YlXdb>~aozUoQ^)vZFncMe*g2W$1OEZm0~_pqa5b z?l`O!M1SH3+<9p8;Uia4lBi-6yGh+!Xw7MVk2UiUB2i*^L9;#8kY?0wy{@vxRTJd_-KU+p|T2K*g@l5of!SpVqc=9A2g} za>YpJkDCdsP!|!{76x9)6$9;eZU&)p<%xk{9hN6Xi^ZE(=82bt*r?9zA)b)Ftt!4i z1nnCk;74_4fk<;O`fb6cs|rM|=-qbop7V%_EsS_*<4&tZHCRtAqBntk-~ zv@ct0gXIm!4_$cl>(;u4ANHmMi;!}T`gfto5vQoQB9WFhTrn|AktM$DF*O#b$Y2d4L|Ekg-;)`C0)H;D*8Io`)YnyPvtHX zmG*=!+SYV6s7xf2x=iHhzdVz!=9i(Ex2qS+M1s2k)YWiM8waY-%f$U4Thvnw#?R|L z#pnden)PJ*s29H1bL^&DIzttgi##z*jVTx1#0Yg>xhMqn@^Ud~sCUb%*7>b@)7%ZK zTC`BYk+aWV)jB9`N&UAUKb(@0>MHe5Td`Ffa7#``-Ucmn{29vU!ZKxq*28un(Q(fC zTIi7SRUn5?)W8Z+=z8-R43YC$ii;}5DPp&Jr9#xrNghy z8rm{Yjjt4yZNxbXs-MJ6xV!UEK8CVOFq&S~89)5jocCJ3);`@Z_@Fjta7gM2lM~J# ze6YPLVX{5w4|X1!0=hMwF38LEYN5J63V^!;xR~SqAXV=?v<34mXHu=73^1Q~bW;|? zjxj7T*n|oFgrrhsrxEf`MN|fnJib zI~Rjs;V4hT^i*I{?y9dk-~NMe7VXRdXVql#psyYiep!aonwky@2KCm14Ix~2O;SY^4KxhYcJWVCRdqk zRg&dIvbsm~_EM^pzE}?nq=PFT{Gx3;M=?}_vaBH?|*fTK@ zEW(CkUXyUFs_P{RQ+tCD?&fSQ#~(a;=&fTWi^-s_>LuQ2OGI36N}HFqUT+%y`$O5O zEtubd)5^7g4woqPX?^&vj=2TIdriCy$R^B!vtI%h^LV)o7*BQ{`o2>(rL?~AYXGGn z9dTcTaatG$Cuo6v*zgOoLqZNeyR~Y;uP*g?Z?Qn`JED5^5hHxj&!5s^OA8?UxR<*` z+zm2=TG>bZ+^rpr!gx{(C1as;&Jo%PBb~avFZxP_dbKY)Z<*TLSMU|>*OOKNL9bV$YTGUw3`R<&k_bh)afQthl38Ewn;qaU0x zlLTj+SI~6E%$q<`IQ$HgseN{8OWyEPS|`|!evk$8EEh*V$Qk}p$3j0vwojqRc;LDZ zU)7>#ztpkRk9lg!OC7iP(?FeuWl~yG*3Puyajj`+%IP};zv%I)ElI-@TJ=z@t`{HP z8Kq9E5oO&h#ci5bgYt$iAid>UuDYd0)H*%SYyCtZMSV~s@|?dvX8{;3^PGFaKq-MY z!ocYSHim&kwc>BC`DQ1hh=13L>E{KlG}zjaWjtwMXgu?{Bg^SOe&}@k8u&dGzb^ca z!LJ*?Bk`Mz-(kmD3687+#0Ni*){IK8z%El%2WdMGjp^t-^zAXLq9&^s>);!tZ2DK7 z=qZAQWt!0qVGKflgkcE15wZ}X5za)IKKc9^Sl?^sPrdLQ{svJ0Eg0L_D+J(S4{Hx#p17S&5@p1a!us}e}VegO$j+t2-%#gl5%BcW(zh0 zFGR}V7-NCFQC#@WOEFx8Xe|hD<*7?@Ww!L^TfcMj)hfc9^VRdYGF2YUSD)m{{MzOo znlS+MiDABFfGKlOjIj_hHBT6S6!`L!=t>GyZJxX&`zGO_Jwsb*l;&MeGsY#n=0iQJ)K;Rq)J zcAo_M|B_ztm-JqLN$>ZU^u|A@S82_EA(#X>VkIij5{7AE!c)uCfFe0ut}RnbisaSV zyOAELjR{Uo+XvVWT3VH`(NmQb%cSf;PYch25#Ith9dUmczM`i(x0tP^r&>}B>xFu% zr|_GJCSb5;!>wcVJ1@s*fyrWZgKk%4d{;9iKsl~J_%3NNWZa4Jyb3;+d7cWEE$Nm^|I88$c0d|qt=HzFpEXHYT#QDuaEIAur1UV zA*VwMr7gIl0Bho9irM;~-fDEYk2#z}7 zGc&=D7O@G1nJ{gT;PxZFCmLkE)r<<6DfXxvDrB02D6NlrtU{J$jbq=#AIBLoN8lxh zGy8Gs7+8D}G*J=jv6SOfMWyV~#^iB*tRtStC}wg4KM(j3Agzz_SSCQ6h9l3*NMn3Z zjQ2({G{zR;*$q4^0eFsxBVy1fPu4Ed#=~l;=f4SrYd5wFcM-T>Tfg88C+c7KQYfaKgnp(awmU`xksf9@w<-80R(x zmSDAKf)ovFS+cGV|1h zz&{IoCTLjMgt3M&RwEv6W3quQ9e->XOrKX{4P4>$V5B23Z7I^MB9w&yE(16MY%~(# z&j7Ph!!x%jl6YIKx~8|x@6&(*F|k%N7J`Oq6J7y09WyN9PW%Z?UJ>JoHhJeFePf+E z-diR)7~gYJ+|%y_zWSv2)RW@N`l%UxWKs=zJ8-Nx7kPvSkl7sLS!AYN*x&LN!hQdA z7THWw&-IbTj`jptzrXsakIavI1o5{(PHVELkEp!9vRFK##`KkG_?ZsKD2GAzfk>_C zA^naCfQ$iZdtX`9wixjdIR0QoKZaqO4FFHE5najS$72yduWn+^hqT$xpD|a{ZB96H znr_(7gROMKHX0MSE*w8#wzLJ1;w(-XM*?Ad50ozL$Cxq^RL&aT45mou^Nu#)t|M+_ z%6#H%_6s4-wj3EdNBeFuQBEG5+Qx(3dA{!fq73IoqU!N5QWDrJY-d78>xzfitSPsHQQ3HKJK#m{qFrtPSGw|!YBn=6{RK`&z2#XqO z0ysss)!Koz?NYOA^A|AX8S;WRwJ_tlc8hG z!3a5WkNGW3z7Qpe`8`6uLFL!Vaxp}mRx1a^Ud>EAB9pmzyuH-}wX(1HyZWM5rhD%q zUxLVd55m@oxP>aJPG+>NWj;w3hfT)aNL`XeCT(NLxDpg7CM7IS#;stg&r0>mxMqT> zR%)V*yPIH|m6{^s#t=-mQqyHzfZ1n=%w;ILAEKCVrce*m$t&CbPPLuVDy?@4jf2ct z2qSv%u(4W<%Yr=K#Y}Tskk@-IWky+0qIWt)dMqf#`*&s^E#p{{bZ zEhyK!u#Jf*Pdm^B-o-3)oRwJaeTrGcTTm}=foVevs`i@3Lcy6uKks8q>t>||ynaGS z7S!N14VP@?(1=q7q`^WgV6(T>jwTf2v%qoQ;XuZuTF?aV-Ku|onbvkAi6}pmA2G7sEv&I7P!p2oQXXw=oZ+YatfsG#V^K#Umz$^N9lDTzec>9Q*fk+JvU|; zkV*8nwWK|0gCgl-2$D&DVDzMG(FANs-#~_4vkd_%cDkMf23r#1ZQgCElK~_XDDgH5 z<0nAg&Qv@`hzttWymQO&Xcn2A#cbXb$n%a9841Y4@V<$aI@349GlDr$z_l>H?F~DNNQZlhqI=Zg*>&#T=7nHLv5&Q9dGccQ+eptJ4g}D-=_ngu^Dp12POp~(M6y~@ zj~QvPdbwU^wYiX^_`njtDORkGgnD@-=^}ew?;auh4b4Gp zvB)@uiPf{a$hJht@>jvi;#oZdZDOSM8_0g9f_dwh*IeCEg0=K!llk9Zw#qVB&te`u z!I^_#iMUwr&Su3S=4ynJi`DwUGNo8KRrLD+)2747JQMl zy;xm^l3V#q(hHamJ=YG3G*z9X_hmj}jFxvMbu7^xk^KU(n*P4UTcT%^w>t;Nxd@h$ zrMjQYj{tE$k(1Tz23aUBQL7tdR{a|^Sc0WbWKTk@*04ITWG3i&lsOYEU?PI0PlDbn zqL0h2IvZrS0<*q8lbIxDef6N+hos{uE=;jEi`93SGF87JGNr8=yBeYc+K88=y<&Vf zL@MXuNxaSnof|21z2X|CRRw^W18<~AG>hsYF-BA%UCVm~8j16yV>2>oFf(a}r`T&7 zz;;l-jvT=u=K}W*a7%&vlDKGy8B8&CTCWfD)t(_TDfRO(K^!D7n<3%!@A8#5D3j#D zd{q#XnYnEB$eQ_!+l83hl{N1j^*#pg3tK+Zg0f1k&R2H^W!}(-KoV9%t6k9rP<}b2 z)NKH21ys}WgaM)d5W)|PWwd+Ih6(qQyAldh~-0Nma_p2wuXH5^iWyYeFT8}EDPK96)0m^ zz1~y7HX>h%MwvK?M7DNjIt7_-H&fvAuVtq9Ak$C6824+;^n-Aw#YEW{Rv`}xmT!Ox#M)+2g^1~hF;-B8i0O!NZ>9kL zb1nv$*$C!ef9hF_d`{S{LaX)ury*0i3^|U5oFhbJBU7wqyY0OkhHS}G7Y&zzvG+r$ zXh8DdBQf?_z;xM>80S1-y6i}d>;62YH^GjN0I)rhr;3|oLEAMitQPT3 zRAT)(OsWwvOww>F0-W$d2-~YEqz#}crQuPImT{I8ZO9AE?vH4}`Usl2{b7;~{hs69 zVKXmn$QRIDyb>*5>Y{q3m?|~SWxm@@s-dPruX?GEHe@pArRHeigCQx`4agK6bgEBA z$g)^YVq&H_Z$aT@DtV;rH{cMsWT&i}OBqecc*+);s5hg#NboWSZX8!PskzJp>-x(~M0+X12Q! z3*3xUoWP~&ZVXR@?gj;*O4Fz#Sa$5nM~{-dJRF$s9Hl{pWw~nPD4A1q5VQ=QuY{}l zO$cO&{mN(_1tdeWO#9FEp=@l!Hab4 zOwL29y$TO$EqbN3=#_t7^qxZkYtbwJyy(SRQ!*?V|#5Q`F36 ztW_dQG$OrmktG_T#JI>3jZjKlWQj(d)8isbG^Y9EB1<$vxp52aSYu#?wgj0M#6?zU zOe>Fztk4MciYuU38n=jKXz@`tRWVkccm5~?M6XyxDO&tH1h-55 z26SVzR4&IHcJvAxZvJUc}(J1X7FylAYqCQfpIQVAHQb%N^Kt} zv&a5O=E;e-P^;tQoo>dl>^ukAdMCup26K`dR9ez0c#5RoSS+|M`0-BC#~|6Zo$<*T zOAt@qu4+$}sRj4KDfM2Z$C;KeSDIJp6y-G@f)u3G07x`mRacxU^9J8y;h4cB>C3br zcIFo8GZ&^;>8ArZRci6E$SY0?FS}xFVw_jFu0u7)eWt!S6`M7ADt)|67x}7Yyv!_E z1)c)$ETbB~MsJkgdy&pDN;96yG=ZB<^^#QEc$r_A44)MQ4wxd*cnu>R0Isf6jImZq z1AuhnXta7~yd0LoJ2vSTmcb-r({6$Q`SQP z@xE|)aKM-gDzW-C6r)Efz^~xcW}AL_Hhti1G1jhsi!~QCe~w(Y3E?ROh~kRXA48Gc zPCRg;dCLSja;zUoVlzr1w%YGP0gD03(f@~UZdPS_RA@kHAAExqTEQ$5aUdte-Wt}VqDe0bZJaX z)Z9W9JqbIRQve)=sGA@YCbH{4(ri601e&%4N>u;JpoJwsUyWqZ458v%yC3b+EG3aT zPh-1ENsPUP+C)l8jI)*6Sf#}LPn3%fltZ&UD0P23au|bFJsQFMGwbYLkcf8Xx!-;> z^P~lc)Ymf4hy{po?qHq~3lQT9G0#ZbBqoXv?zNlCF*DCrB?Tm8R1#F2kF#0i+EIOfR>8r?~Ls~qjS0_N5fX2QAc-zft%pkp6n=Qt6t8iTo8yknAoo$7PzaD^Q(BHGI6!GQ}fbksD zR#|E7#UFWFE$#Po#L}qZGu&oD8>{8oi z;Y!5ZX#7OeTq4(*xnTJ6QEhMv5nAMB7QcY z7R%6Yi}*_c%{30Hx2DMXS+A1hU7>%CqK5bB9k+lWyiXtZh?;S>EGYIOH*p3>Fb&Nd zQ+5iPT*6xzbi^4YJXd=Qa<(HR;FjyzGQrP&S^yd?>NZGY%u@m=U2~8#?mcx7V!N~D ziyB8c90sO})k!k|`zcT~LsZ?=$dYY66j%gwJ7zQf9mnqB|F%UNe`(PtjwOwgjwN9S zuF#CXo=Rc}HU9Hh()iC~N!LlolCG1EC0!>SOS=AgEa?g#OLEcX4xdDFi54{^iwk<3 zK8{2FIDX6q6ghrm9P0A;Q6K0lBb~WY(}x)2Nye3$9!zE2I)2oLngEX<$%~cRqv!me zP9Kxc{Xd;PPCi%i^zo*1|NGO&)bnI|Tjca{3rfP%M|8@VM3MO>z#Sq!I1;B;-K?`h zO~$Q7!}t`^YIKv`z;tY#W*BMp@q1@=RC(qElcZbhbs-uCp!jcAagJ8E0D)FOc&~ z`*Og!SePS@A7vA(Z^c}eWA2n)EYjk@cPU8bAmk=1*M;(z(hLj@cxUM(mM1Y=S}c=9 zDp|IJe4E8mFkSu<{04Dc5_SHJ91da-8fE4rA;@CuEpo}$4G6pzr{$O@HGM=jYpE{~ zJk!a!ShdWMiAkSPR<-Cc38kRVeJKJ|TQWlyIZJS}Qj`>{Ei+`6>{YBPE|R|bY9g1B zD=3`#A7Yz}7<~mWwh>YI z`6Ag9<3aNAnKDDB6swF`GR02hb?S!M(x*B92BdI`60(f~VmOher43O)OV0x0LWEpSI2VAbbRH~mMzp4W zoF&topCM!MS&@pl2!{Wr$TIvsBG=GhNnyhiqptMA)go_$4X=|#A~ ziTktbE|Q7)ES)oj+!0d~;|h>5VrtOthYrE_ks@9|3hq1Wnb6lXHL?0@l)#E-#LS;) zH=(ZliSfK&q{hsKHfXg8s*mOzYW zb&(o+sm!Qb0pM%oUu>O(YL#!YbPbNk&2O>V29;~f3T^04(}?Ci?Lf5`sXH%~eu>`~ zA&9?wE=7~V@8L^jI)78wkY2DjUo3@8|){xKGaK^4D;XuUt z{jQ=elWDEPfUwWRTJ9&vF)QRaBAScG6O6e_07YpDOb#SKj{R&92e`Cz@XR4F4e1V^ zIrQV112(wR^#0I%AXj#PtTTr}lhuuv;Ru`e78ty53S%&DCop*55Jtmth-Q$-T!TM` zNCxjqpyK@k23#&N2J_&Z!MhazPxTqRF96^j1_tk|5Y`jS46@T0{C5%Z&J=NCaD?>+MGnw%5fGEX*)oyq-y zw$WO4>)^v~9dOvK0}eY6IL4#$0-$ger<_AV>x9E@op9K#Qw^Oyvuk9^HKruHl_ttu9NMkLp~D6lzo^51$MnD>40lPw-$A7F2*X{Tcmv~fgrh7+Sf?ZG8Ituo z@P>I-5q*Ubhl+Cjgh%w1KE}a9KiD?VmjTEy{b1V=V+hIU2itgV#VboP+^r7UDK( zuEhRIy{lzC^q1K?Zqp%imKNfBctk9##=Z{U6+VF6gh zW74@E224FCjMbZgl^D3v7)f0nM!N}d;7aAd`BSO-@=BTA_B+KarkE$BGal-t-h@q}3xit0uFvh-_5-@^xqXg18`2dsVz>Zqd^T6g~5Y9#5U-~wr-U*&cken#PC5Vre z7h~V($I>=jKnahQzJXw03l94VSW2vr`kIJ3#5!L9wo4siT(1Gsr4BJsd~ma+OP&5u zr7o;aD$TYF6mkd4y9p&brz(uH?Pg(VGe%6Ckur)9ZDy93nCM-os5V)HJ@^@Ia(xW9 zg2WK%Sn*n@t5;`mTo4)QA+p+wP*b)JMb2 zrZ&Xt0bosS7%{aW+D)iS8)7_-rKs)rV>Y|GySs(!}r4 z@Ne2eWc>vD>LOwF)e;q4X!g}*3*~?W_SJtOIldKxI31~k0qV>fS}{ofP9`r9B{= zA5e_hJ1M66JSsPwnY8&8tJd!1wc+J08AXUKqu$ zcqi4B#iT{9upMGw!ntp@yRkVjCodTp~kTDd4ItkIX2@THqDDruENQE)j$Pt-Z%RMF`MH1IhH)B4b@#`}zk zdLO#gl5Tb7?}(XKby)o-J)l~XzOu{Lz7lD1G|LT$XqoHqV5~b`$hIiW$@D%6kZwBL zeY%|OK3&dsWtX%47HE6_$=U8Z8JxCv6LpSg)m$dLTV{G}H4!IUKjx~r6lP`6j~g{T zkuQVZY1Fp83GNpWFyN0zF>TtvFmZ<6;J3j*H@F&-`FB?Z${l`}C49l>f8horqX3xk za?5{u!FpTx?G_%io$L~a@A=rR^Agh~#>*}Dk;*zR3A@C4Jy3X|<#ha*_k8s5TP<^d z&`-MOV>-!rtA$>*jpt9Mr|hcinX!XF3r1VKy}|_BA4L3r-}CX;(P_rJEC2UvKKd^8 z)iQZr+n;Zkcq2DVmY^Nd%ZKCXknrnu-Y^l{Gg1+^ZkULVj8Op08zv%r!z2zjOvJ}N z;;kDdB67n-pGf|)r{0u6rj0y6&A2=(=}8$$!0f0yq8An@`+3Sq@DiZaU)$UH49Af;rK=camt{ zJ4yTvZk*Kg@V%2!NY=ymPOe3~+jjNC%_r`i#6S<55x#dqQC;^=`cN+Jos`}p^Gd?^ zP6ngPPLj!gx_6SyjSbvW;oA|ocjApzSKT6$Fl+tu7W1CT+qcN{+I;AQFQn)_Sc%u* z;f!24DF9pK%1H^DS=W`5Wu|6HD(9akuAJy7dDoSb3;1y zLpceK7tVh}sJIQ;q|cd((QCZuP5}2$zWJy0i;FkeZM)=e@|D@D`Y)hH)viV zhq`!PDnkyuvAt%6tTIRPkAUj&Oqu$8g-lL=8Lw=w!|448kOg%96AQ4i!Fr(N!!i}O z5@TzX3arG%`Q%O0R?0dd2CDm4$--{5nS8o8ZRY8YG9%gzotxG zvsxBqEh5vkj>t>tV)b;4WffDwlET5fvR3U}jd%SD)xTHE9F#xqc9|K?tvUX;wV(v=I({ryvue{EW(GZ~|9{V$Eq(2c8@hKR&$1tn0CYQfVy$LQfhqyarg3K>d z`FF@3(XUeO$QV{luQGMU9nx>f9a%!Ti?TFrK0--tnOb&-^hGC8V53RiSf-x3LsnYk zjeSY3XJcQIeagw)^e6UI0uT16MDg8ZyDCa# z6x;V6vF}>xiqaY%G9AXGV44EWyHgnZ9f4v z{hT?t^zg7=&Ed(U-484l_W&95ajHqs9Hgz|W*1(aNYv{%D+|ubb)1z2XXRSXygGNe zc(oiYt?e$ES;`4k=WbRVCs^~MLLDbq^P)m+*A3daV9B^!y>XZ9*LL!afdTZ;7-B1g zYrx6dOaltBhGB@CC5!>*!-@>CwE$WWVTdgUFc>ysh?@z34jV&k82~Pv81Q8kV*oFO zF~rSi1M~!PWQgr8+yf3F!Vo(U0G(%s*edA5wJAevKjG52O<~hE!qp1TjE=xDm`GAI~T zFMP9+t>#{n4rs9OQLHCw*5uhZsTQf5EP;8*)IG5v7HqCz5aNv*EsWYOAy-vkj@XaFd-3J-AsH=o=Hu;7!^J_%z%gkJ}easBbNu5c;7a=4Nb%!vX zBH`^Myj%EAg(jMNwi$9>VL>wHurZVWB5#79w@*A}%+zYsQzyA#Sqz3$_tQCu83cUbXn8UM(v z$heiH`8<{M&}A@W#y`pADv`;TVqH@Kc%snPHv`IaoA<(NL@FW8ZC(towIG{2O`W$^ zW{F?ab!%m&`JuF=o7JOhWvcsrWP-f~hf%I}uEi%oyyfbTwX&kSADPmvOVT9XdtEuM>BSl@copJ9+cHJJW5XCxQ36GIm*F-fe;52 z^Ffl`2cQ>YjX!{h8MK8^;5q&)WIqI~FqP-vcBO3{B7DhBU8 zX?PtaVItfzrSiKR zk(MoPsxgnq)I{2~1Dw6M z%)?QKAsTwBwnt=LoxvJdX%a`ir6X3Kg?XrIE+*2IS(GB0%|o6U!1h`S0YB1b`ms#? z1_4>)*b3Smkq`5T)sK?wE07T-cbBO#k4m2_4!uFtMdSNCa~_p_>I~?h@4$*s4A2{3 z!0H}gAkw}YNZ#bEt_x$Fdw}6h&gvm1Mt$?B%&MzoA@~l8n5eIzfD9xOX)gd$$FBAb0(*A>RPt}BfHm1oeu%QIjF4B*n6@78cs(FR_lx<4+9@NvK4{HaqH z^XF2v=5e@|iR#(Mae;OljLcyBn>6Yr>X*UMWYpV;L~x8Ww3&=+2QgKJXYAkLye&cw z&EYM-9|wW)&ns`P!)(^slg&DNvRP+OHtXdYJpck{TX9MY39YjyTlnpolg^&PUy-o= zd7*s^dBPVH$HS80uSnRebEwFodmN}DYwU{|Kl#fM+ktTGY9o3AB!s^op?`#7#nmH& zsro5s9P)Qjs;rT zT94PU!_#yW`bAXNG~EY~$DHdiO@9MLHBV%kZsFvk>ET(ro@`yS^xw!9nWg)nT|$qK zQ3TA=<}$r&mfk=@bCzzyH2nay(mb<47813}L~-wz}<=WfpsBLRi5h}(jx1gsN5 z&rlP$PSV*Y7CHV>BzF}GTf;kk-w%7Q$e z6hbR3$m@9u{VZms1tog;)?v&l3rg{v#kAY>_>Yihx+g@w)mEC{b9NgOZ@0i)&-sMz zu%H5u`3BaVdi*>nUgG(kjBBj4a?dbAcUe#`&m8jIZ9&zZh3F;v*dZE~?8%Dng&g^hA6lLYgPyBN4KB(p1_e(?{yR zN%~K_7bUd$M8YXZ(+8p2YmV8zEdguRho$OH1(GDt~%kh!GpD>vAw7T&0)b7-TGmK%o8mh-%Ip zuF5vcTbi;+e2;J(1Bu`x_v{()QtlDCq{|zQ2@F65f!uRuOeGK?fPvfm-beb~$O4~u zcHj$i(ND|#wzY7?1Rs#4iED&wCPZZ2f!;@?&Ik5OG`Uj77d#H^nT7Z`0RkMT3*fJ8 zAp4#quwE;iJHT4>V;F1KFy-L#wqC@87ScKWz(`EOSyRFo*M_!;e7vfjJqzgBkUh(k zj~v<{>sjVd4Nh^raOT3XC?;SEWs*V2hVxldC^2q6Or0Y#7en4PVVNaN)iK^_rJuvX zZgHV-9|W|lT-7}z^PT)sp9IkQjP#p;9l$@Hk(pVyB1Q1QrY2?y*Y{Ayv=ov0t>x++ zkhQHQ**aQeF1}>zMv;ioUjxR5^Hft!V(fgFYU1GBEc!1{F%(uay)Wx&q;M<^Q;F55 z!o?}0dOQu(dR{?-eP%-ds7;_!tKa2?*77-O<`(%kF^)Mpoxm9juDU-Fj;G0>&gJMyczCEwZYbRSN@l1&UU>K`+_y?pzMX5Yx(jH zF>rmJ0?!QK^%R!(cKt9|i*|>R4nK=L5xbYHZju>%(0Z~%z4fg0O_~c>nhzomOOA8n zfD0g>k_m{6M5NyoL{3MfCj=vRd}5+jjIrzdc5^LGYm9cSgo4ijiPpb@iZ@0#ma9R} zNq@--fchV}c|5W#pl{|1bG_@~H(+B3N zSDu&s+ah~bQMej4g1ev$c)fcBn^?G+jo|zA3}|vA1|Xfm`+am1*9r_#pM&`$D8>*s z+!(>LE(Y&cF-_b%V~}U!rJ5nw>9A?4JEZ_+T3p5)Xg(_V~r^?cce^@HOBfVnof^3rp)qc>9NLG9&3zFH#N3bYAR+FY*pRxg3LD^Q`ddw z1`D>T)Qd03lyP)SV`~!UfXuMP;Ax&k#SY0@L`L%-m+J~+4FL0f2G^Co-I$9(M*lT- zMCKz%vG%WA*uVM~5cjWMH2v3MFNXcs*og%;a9P_-E^IUP1Z3`2x!}LfL$A}^^j}@r zX5xNJ*nf>}@z;^f^j}@rWtu=X`mZkRGQB}I%YThMCl$vd;aw&dc9|X~A^leu{a0#_ zZ6-7*7yQ=_K#5WGUtREDsY$Mz{;LanPc%T-e|1-;Y(ko~_vEfNb^$QAliW3_jMHAs z#`Is^1AX5k9!uL;8&K{cMjT4wY@;*b4yF>YwvpUJO@KR4BsZN{_o(a~5Lg>f?o$oE z5oBdHK9%t>=V>NjZ9usv_I#DA#pn?V7t$t|dNm*UW2kZPU4(XzoV2UiV4Z$lQ%`?XcqXv|Jyh{!TnS zE!XF%=JumuI}a#xH_CCeJ=kvyVw)DHEZ` zCA9hTBp9pzNtJhiHiUp zYSDICl=cvSug!7Fuw8}>l6ZJ7F4i6MD%2a>A%~yrl>j`iLe5o`b5(`v^(y2Lz(Za1 zsx0y??UIwp<9IPyce1u7RH%)w%Iq|AGix%g)k639GL#U(KLh!;RHy^5;#I;K;12=j zTco}4DmeF~pv~8+*K6{S7^_NNmjN7KO@CdE#0x`PEa>R#cztN2Dt$xt#8K3lZ{W5= zvbyFCIb3|FcD^A81M2psJXJidE_hS^5PXy4{nIka+*gIUifFRc&!OFPr>BjiQYM8k z-t9-|&i(^reXsugjLdBa@P@rDuUdGSVbgqP7r9`)!)^jvOP- z7{a5C6jp+@YGzERB-(oP}Eq@K%2&ZFQV+C@n6f+eL86U7Ib z4-S^4?t2&5`YPywVE!dtub$W;Ukd&WGnyDLU2L5ED?t#e567c03#_DUCS3=<1qh|9 zh&)Y35xE73ClPQy_3TdhikqIM_)R!+E7bM7WSM#TIx7LLYI%ivd6&#_n_Fi85)OP! z`oJ!ke`YTh=mpqMd?ECoL1|irSlesB(kfm=YAGMgg>CpQ!R>aUlz`M#( zoUicx>%Vt4Fl&TK2BEGR zA;z6jq2BsXrYCy=e2diU!4y$A4~d;0;#(p;Rq97_6#lW+`5(!jqWKXY@qnorzR*2& zx9nwZr(TKLF1s3!eQ-Z*`^wcLyJce^Ix%8`Nh~%Yu`hQ~CuFl&)f}tDgi4N8)!eQS z69Q(H*GyGKAImD(>8y|OT{qsMxAP-EJ<(w5_>mt#Wo*u<bZiGa17=qh4mkU{!RX49i7P>&hnI-`=ONiS z+mbebXPi1+*8$TYM5pVDm$7S6MZB{AFYuW!V*{$(&NeXcXAA;HTv%i>dUzeXBQQPL zh3kf^nV-ni1g;H?qauT4(i=!MT&+fecuH;h1PA{m>dQ}XiSKn~`xIvqFPE#5Pi5g) z?o>0lz7Ww3-0x>_;fq5JJVjt|eT{qk-=k+UxV{814G{*{cfuIb57{y}`$spJZmfFX zQ`z167||HCgGDI83V53Q%bfsw{m+M+wg71V%Qst3KHJ3GtQUjsA}zWAHf1(K>au=2@y<{!AvCr=6aYPdhy)PCGrFzEZT=@M&k%f1GwkoqXEqfwvew z?JUNUDDfY#zUQRVPAa1&-78ZI6;h(cXT8E&G!HvHIP5&m0&xS>gTv10$Q6g3>h^!( zE!8}z9yPJR4OhVOH$Ct-djqn_;cr^x^fx{5H~)slg>Uw9zci}FKZtCmzv+R$c^28| zZ+hTwZe=dksb}dH{Y?-3&G4}%6!E~{q^g#`>4CpVO)P)Y^XIe9 z=*pDmkY@Rt(bWchL(>I}u1RGa?Z?J!%(Kqufxi7DHup=ThZylFghh<~QUaF47(LVk z=xTmWR_>QZkIK#kf#qsOpK6@m#X3Hf@i6CUCSbXm(I?J2Q$S&!b=pok>$FAAIw5{j zpD*MT9(S(X6Lv5?i+$AF%dI@mbv`QM9Ya$s@ll!rUwotAs%Q_>HkjS;K!VQ%AG=FZrT_*W=DR;GNR;P<#wY;Bfi1Y~3yC=!C^zvG?|_Gr z`FEgOYw&pz1pZ}(ki$VxhyZgKBJEkDKthb(Lg_fB%bF6#*jp(b$8@H2(l`%RsxQ8l zSv`&dkbF?67$%*Q5epaeNw7QcA0MQnO7-|gmSqsIw==&zVv0=={hmTTOfhlR~lW)u_vwe%H!=-wBE0?%=GKQTv2maH=7NCPB$Hf+h zk7uGUW4cUnQO;r!cL7@MfI7?(2q|2p0GUSQX+VrYNSg(FKvvFovWIgx$|HtXsxjZ; zrPgyR)!gr7>4bSihOlRWtyjB*@SJvFtm^t0%uASVuqizvVycmDHYH+pJ|a!&M#N8Q zN;g)jKfaSa<^7eaXs_(i?S7IrQ_3qs_tQi<>nhc`|BtgTfv=*v+n(*-%-oqQH@Qh} zRze66LI_ES5caT$5d&la#2qODDk>r>Dtb}Tf-AT%RHIO;E$)a^taYJkm0DctQmL<> zVv7ab>xzi~Q&*rOmv+>+_8x71(GIkWLfq0`{n&aPZ3cOl28@E)T{Z44 zd$d87-#{c>PZ2{+8GsL(k|B)2L6XtJQF`!}Qb_-%#{F^+-r!2DTd)`LPpfsC_G%gJO>iL2Sf@H=8L@xK?bGSaVx zwc6S9(FpweU{O~c@h@_pms}>X<}E=OPVQhK`9f4kR^Qp1oZrOsZrN)xl z4vkBNUus-dD>ROH7i}nYR_rz?=)6d+Syj+-Cw#0$#Wr+D97(&4Qi>Tbr5Y)P+}^=E zG%Tj`p^%(m6@oK_%~@@cczx^j?6?iF7{b`AR*^q^1QKt2sLG5UCGSU2V9Ams5u8MU49N(e(Gn6K;%%PoTe=@}s4HJLQj&{v6srJ~kB}{EGet zh2~^zroW#4MIrz!g1=iIi)5u7MeDzFZiGO}2d?oMCK0_)V|S%6aK&bSiE&S;1hR&r zPbGG*-U`ey^#3q6f0Ln(g>WdI#gBHK6=Ap{la2k<1BgvV%0Tl~sMs42SW30k4!>fr zAySjVd@x}da^|VAc66H|)?k@)vNzD!K?G@DlEJT>G-E%m-j9r= zXWo&C`7nykg*N7|Gq;mI0MRt>&fGzNEoJV>+(myc>OPpcmHs$`e=_p`{0aR_Dm;_5 zjDdLvi>!n_D<^LzWsiqHQkr)a+EHo7wQTS0g(bZf7pqsnMIfZU40Ca-&in?5eLfk3 zv*=f%r<|!+Lqy&h_LF1`u`dVRw6{XV8WOu)!L#nz3HQFwwSkG9AeLDp%p37k*v}#E z)+meOiv1?yXpI(#VfUtPgFsAM!yC~WBM`@al<^)bkPMs4l&oQq7%z~xJ)AK) zQXqx)AIX_OB%zcjnRPI3s@4?q4Mt)*Bheh8U$M)Gv{*!zW|dN{Epirm<6S$qU~D49h_?(B~khno1gpE5}|A0=G23hdWt*d3#~fsB+5Ny@iB7jkem1d zt4jAU%6}4fBW{nSz_tiuT4;0rw)J9BDXQBNn=`hp?L|#M%ItlN-YcmKfRx)Wxo>}= z4NA+P>W=72&>HLq+=4G*dm?SWCu}#_Bk`KGc1HLZHrqBM`LE2^aigc&AJW`zhf5Y_ z*mH^OP2&>p6YV423%=B95`PA*+^|m5OhzW&pCWTL`Es`nvX)B8=J+kd zF4rtR;#1?ZD0!Eb!o?CZ@}3?+bc2?~veBsKoy&;dqh+!zHK}>GGmr-~XDH0RqvcJc zrA=DKlSuMTEpMRv{#V)^iAjuenVP>4Ci0g-!?DU$^Bc&-#iLoFMu{l#IK%3%%BM3P zqgtgZ@)k!^7&Mw@Y{WJHWW*xP8n2qe+2($Of=8;EM3neBG%`VA;rMZdiu=RYm}9Vz zG5w_$XZcs+gNQBHGMQu}UQ3k~TEHPOCf{ zSabYcN?wmc9ue@2_$B1spyjai%!@ZLqi&Sgg7~)ynpr8)#qm{CT%~1j6OX0wKIGk` zWh@1@JboH^H_HIm#UEgfVo?r_-W9)_MsJna{qc74R!eMi{9^K4i9HctL2Ql0o{RS@ zaC?5M9hLqNLqhl27KEh4f9YQMtyZ7L0&d-=*~cIiMUS{oeybI!x4HlGt=5=0A0f=J z3(P!xBG}g;Vs=ranDvXD4<)-e5(c8#4dnEQhy`1=O-_l;C0eE(MSynCNCl9HeKN&L zBVuxyok66Ru$E?@PqDH{Cd4xA>kz12k+nfp>M{B4Zp!s1hR1y}Qe{^Pq~2~MXF&8F za2oBuQ>n_~Tdv8DyKnE)ih4|fRdt%R5;VDSZAlwc)IWzVM+zzjxjEly)l+%2j=DAM zA1a7VOy=Mn>STufV+Ymt-y`+C=dsuH4^gUCH9#Gp^jU^b)kjh|3(h~aP;D=n>)!Sq z)&-yEzVRIvYKB+u#}|FnE#0s6&v5v)w4yhyVCbXdCHEvyE5eLuqST|7v$|8y6R|W= z4fezC?&lS-wyFrboAzsEY7h42VM=(G&GM}MzPPmFX>qLsJ%;VRo+ z+YzK_d-Xt|`02B8%YJ5;qJGHD^7Kxd<#lu^CJ;cfPw@{7F3_cD1UAJpyiM^q%hHft zilGM~ALhm2WXe!KF&v#j!cKr#Dz7R6XM027DT-AX#*Cp8RFtqW<609+7@yFOgRR&b zk@D~gi5d0-#IBK;8NnGy;cF%0M8*NRP9hoh)ySvt^%BFjz_=IT8zdH2oN_X6RCR`L ztqB{NH5s*pFc8)lEYfdZj3k9|RuyAm*s~C`FwUwXW=42cRTyVg5p(S2UV~ji3};ml z8*i_nBsM~42#xl529JI7$ZJm6?4u3itSX{Y?TNUCaaI+v88$cC3>U~CPPFkksf2Ov z7kM*nZkZm&0anCj+YJm5Tc$IJc{UHQ3gZAP@)ksRfK|9eVvB6xqJf)!J^+~!8=VVqz^-gWjE>R}^wVk_-cCq$pvYpOdzq5Aw0OWQ25{RG!8)fa;p) zP6&l`o}Hfd9%X)&I#0+PAmvil5$zs@M;NDLk7zaCmLe8mt2%dWYMI(VUAOXeV zgj@!&m;s!ng?dZ?YbB64%oQe%0`?9xcF+XpZ61VYKcM6yq`e#_R9-aZ{DqqJEQ)0Q zP2d!dhddzvY9NE)sR1ZG^U$!Wpgb=Il`ht-lC_Yjr72#7smrhx1H#WwYy_(jsuKW( zTu6KpJSPCqi3QQ|92q2gy$-%i>RE;@OUv8 zl~tNm`Wa;AfS_(M)3e2&f#0+@2AR{*3r!@K$1}V~l4-DfYgxIU>_biWzHcdP^>Z!>3-l8Mcr7n>LA@zF2NPO-ti5(w7t3aKcpDbD)=3)8g+mwO`*0YA|0yi$6{2 z3u+bhGU-QIK9w@JsG=;_*C`2c_OIH05gJpu3ni~N1KEdUe+Z}$A*@wJ2*)0&C~bg> z^Ug*bUQkn6r(6QU`2bPW&OoN<^~~D4)lkV>u(}8oaS`|J|EWiPjg;o^1?LecB_75F z83rMdyN!OPHo+sZd4WgiPeb2AZ2+KBOleg5Uj`cYYMdx&E%qX%woKGqrC}wu3{2ja z{>$OveVOZNqN8EJO`svs37V4Q?o~(dF~3M=jmqBf-XcRTQp=ek1I5j`L`@TjxF1U- zg6zgD7ga29!%OQFnpvcZ+Rs2J29(wIsuX6*TuD}$4q-h3Qh!=hs?MgsBu{|xmcdVw z{C)!`)9!Q`OyAQ{QL9mAKc2iN$iZ2rYVBm+!GEQywYl{CBUK%gLf(t1{wdSA)398} z$CM;3!K;Yj_K!<5;DtHBTPQ>#6YzcojLL!hHgNO7EzAYd z1JoiwVVQ;W*TGi{$a@Q>-hiB1!fZQ1jgRJ2!rySR`~$w#cJ6~Jo}ejEI|iT(=ksf# zuvG(5+xaNvUklP=Fc$zC)&Oa^8<&xkj6qUd%9#Hw^Fn$fB<}|lQu+;eUM51V@KMNj zfOw+g)aC^_sHj({^-ufp=>w56fHGzil*%Cz29l206g~pNNU(+iMzVa7tClBl6&1rW zH7B3%f>Xgi2_W7DSHrUmfEY78<~1e*aSl2mrBrpsa&A5$r5839Y(>TqO!QFSgHmlM z8CAH-4~y5qxC;7Pd(sN(t22?N#7}=&paB&)5!joc(D4NU`&053z@E34yg2yRI6d!5 zlDv-zel&!tW3xdL=lO<4#4dzCw3_8#d_08v6ACRl%#}8KshTySp|oIPlcnlf2|_5RvJad)kfXl>L<}9_ zKMCI-iSf|nH{f{{U~=WH0}EIDwH!qF(G#B_U03v-9oCGJPo|DCSQ00ZWgo``qU( zy-FKV;(lZ4bq)PXU~XcRQNlt@C_cfv+muZLTFgV0lB~1Oxez%jVx67SoAOgc4T)zV zv&20kq8BE-F`q}Er006$I|7Nb0BlB+qlp2uGhfA>-;K!r3&1^OU z8M76dYBhsACH4#GTJ(#IIT<3yQ;d;LP|jFDxk<5!l%rqd%u>iK0f?NTSXhX8cb)`u zF=Tn;p99kA#;*k9M>qZnNQoQ&DLmedPiVaJdH!&Zc}!})Uo}{$8uIGu6-u zF_(frK2r^y5SNn^I8#lZ6u(7S?byEo{Sm*D0sR4v@mw$5-MM1WL{u&M>3O|LA1msP zc-9F(LeG=J&6m(k!AvnY3(Ba>T28;$WpO+F;m>dh@nSp=yGDH$-{PW9GDgJs^(f{^ zg(}1hdV}+%Lj9$mGwVZDMM9S|>q7&xivC3fPLvAQPWLT{yg=?>PVao^*#`>;L#kKPd|(7 z<8Z}nA9HlxmlS5LW|~Ku1$-zuFV>upPrqE^XdWdHE{@zyO-}YPCzlR@fLt7DPBUi# zDU_#|i|iBi-~g{XBKAdu5v5OPuL#p`YY?$l+`_7`4z(Ld;BOKZ%8Y5@Jr2 z{UjFpNr=e<<1F-(5R(VSS?DJrCJ&6W&`-i_l?TRI=qDjh9vEk#pM)3=j1$`zpr2$O z;wevz!|HIslqben=qI5lPK?9tvCvOKOdc3#p`V07$OGdn^plX+AP$VP&`&~49vEk# zpM)XE1LG|8laR*);|gFD{Uk)EMknHGp`V12zJ5>20A)ghY9YoP{nCTEi)F zs1Gf4iIBG-on0apxT!zpju?5j-Jrh7DNpw+kBSj}m zY;kluu_+Q;68)H1v&5E0vzVq9i7kts&D6I_Y=*iVP%^fm7^S|D$%1P93iQq#l(epM8Ax}5voexi+Vb(?^_aoFqL7YaO_NwNm`69KHpO5R&IJ*;2 zwg7=HQit+3ya4=JK<{H~Ab5eAF#^O7LD&Q6v*1*$jJ{Bn*@Wu<2M`lVUzPW4rWz@O zEPpcqYWqFO@MfyL7+xhb$^p{$ENOcR2$KNP_AjLEc_7RpQ8e3Ut7){n9)#5ZrF;m} zhOs2I;BO@eAKS@0d^!QI9h7= zNS4=&fn5MVp+N5;x{bn&+FRkNu!>uOScOq*Nvm%OtG|Zy?X&@_Z&i+t;0q7fi=@En zTSI8|zrel+DAj{$?uiJK!yoq!P}}R!M5|dw&NFFIP)ZU1539 zou#PHW$~yQZPm=0VU*Y=@@FBqa{z;Ak3VfEna_uM0EfFme1 zcm^nQf&Bt7cs7tTQ5}7YKiPoW*8+kGc&YJO|Db z0BBwTv>vQfxP!06#h488WcS$uy?^3PMs6jJxdwfhNa)IPrhYgRxUzxd5ij$e-Xu~t z;!TEev+$3k;SmQ!pqZ@clu;v&r0D$;!`);4jNV-W+7+qwxGCx*P#Jy^&|;DK)#~-` z*@b$qqsKBDr-_GI-ryc+a5p3bwUqJcgE*H0#)wbGx#DIt1D^~Sz_34of>JfbeH-Q~ zc2Tir6z{s)($I^KZ_N~9iy=M_fVlNXe5%&FeT&fQ74n6m>_PrZY8OtX{8y0OMfvf3 z+WFG}YWtDDbHGG#l;|!5KH>Kusm&9%(rHnS@;}BJN0cC&0O{yUx)Sn+60O5kewgh; z+^*t_VF&V&zw6Lx@-v2lf3m{{)AAO;T$jWD_?e{3PMXkVV263pu@4|;#QOth28m`& zt+PId$2z7}Q3s39c(iulA0qsQ)!GE4ULdB0wyru<_?^fc=JzmxU=zDDi4E5`z?3~0 zUv=u}DD8*z4D7QJ_7dFxREIB%6=mx|nKK0+vVhcq zG`C_9&8-H06Ce$K(QHoUUTQCYLojP6*?dR}gs;Mz$o4 zUp|z^KLNhiH_nmdZjGlHH$paJ5~II3%mghO;@6@Yl%f{{t%pa+IF`{kN628S4>KXF z+CCJT;|pc3q@lUtW)SeyARjW%C*`DYJt&xTLCXE4w1rDLD7#2GGQ1CEvs}nzvGODs zXN9v^Jrj&G!pgNM(S|ba2vl)iVSJ@R_7d!Y1b<&%^s~yM(E!;{cheGrE1g0=Ug;F_ zW+e>jbV8-qCy0-K#M?Wl@(%dL_kP%4_2fQRf(Lg5V!Y6-?~A-P|JDa%H0CG$@a{0r z1mzO*e&oOsa}(&7nK|$$tRC=V?hdoxIup8@6$M4N9)gx(o!B4gupR(CY@Gvtigg+2 zru7^smW4BRm56mQ=(hC`>^s)B1dOIy3Xn8w8T{$i88DY&^#LVn^@eDswF3Su>uLlU zv+lz++iCzk$65l(T&pLH#;sGJn`h00Ki}#Fe}VNI1X*aE2FW5T1(Lmvl*&-yjgDy-e0^tYaZ zztXCP)&Q$FB&)12t^=)ppx0RIL9eyG#C4E$D(Hi)T3m-%m*84&RUukKtqc32Z8FSS z2%+JYg&;>*&0vqT8bKLly^ZT=>kb6mVEqF@jnf?U`CV_--mvNHm%fL$PCnLack|}vh?GK3IBvbO%IsQqeLQfU@ zAMqqpp{P1dl*LJ=e`%qr>TvD__7IV}s^gzzDwH+HKgsl6hN`7{Cz&1ursxjmR2VqP zR5!5xS$9rzAL*r^p`PFtm+3kEKV>*Lx%96PRc!y{(&vE~Hb;Yaaw##>e$PFnOz$|k zl*BUo42t07Qex%y?}*{#Qeu_%o5XN(DY0tXuz}&^Qet(se{w0YdK)K~eq5$^oLssZ ziEXx@p*~J7-3V-^%vhXU`g>rD?62HOz4go07u;8R_}F zhRuDn4D6#t%(S_WmVte=h&eX*(K4`)7O@PQ`)C>1M~hg@<~~{m_R%61w@)Sy`)Dzn z3T^JAWndpI@=9#6kCuUbw1}43r&ARBXb~%y(Z)VnuOZr%Huupou#Xmb)%MZEu#Xn8 zI-C1w8Q4dQSiP9dV_+XG>W#3ukCuUbw8(3)rx3$FTExcNmuBG+HL#Bs(MBX zRBW=jkCuUbw8(3=r%@66Xc3!ga~~~CDi_!c?1RDtV;`-5LNPOL^bJ|Jr}zcBUr5+f z?BnnR85JR+W7s$L0oFg15U6SYp*PS<$#m@96de!}S(agcLaa(+F(vu~o+V>oNFRY9 zGBb=-A?qUswt@z4mKGHIJ=(j)59~eiZk3p67tp|JiA5c3TTEFuB+ki>-Rf%P`jnys zX!dzRTd1PBztvP|wgQwqZibAN5~j+FkXxvRVhFXM37pd* zGnaCOC176;ic;jFg~(`YixF9Mu&U34bn$&2)mRFuj>U3`;!=oHFua^3EUYi62IVrC zTSAjix*kfEFswaBQ_FB-;jcZ4@hWw&qOS+7P$tX^BgicWgr!?yBTZ;7IJ#B|bLaV3DUqj-al43Lj?dWO!I+ru`<#HyS%bEHnm+<#J1Hxhne|ctqJgV`O9Q|Df>F;+IYle8rpnsuh z+V4+&1u~ZDR?5F8c2PJ-=kwEV5S~t8!^o>BBp?la zoSRc=Ts*~W>9-!uZ(z03ekRu{VVkl7omt!OMTu1jX65x2;C(Btuy_sN##GvOB1EOC z43(#Ld4WFgqI5+eV3Q(K;oys*48hC@|Jw^`y$bOVeF|`=p{?(&fa1W`(IKF6Khy4) zJ+a#9cG;6y>XcmRq~GjjROzwA{7T%-!TEoJmJ)6wGDILocsnAZ)(gZ8H- zjw3HpM^Jcgcsc722&wI9INS!)VZu%I?mJa_UZM${5#Xe=XYa*y#ykq&UxjZk3fcRB z%(Ef&6zpYrM+T)B2u+EdMC*SHHzuv8K0xa$!sC;a?Ds)=3xMZ^i&+fivZ2Ylv!N0N zkhh0J2mB3K%IwA>vPJx=eF_v^ZcD3s>yf<0Ydiz}tq&Ca-(mEBUjqTQx8_xQ{oi5qf0G!u+5a6z|2MHh8~xu8R%2Dzo32`; z7pTv=MKyYUExWogX;;Whwb|8;NxOOXL1PAXA1VckuI|49^*+W^*SM$G=mU@80I+(q z%`eQ&$UhUV+CB%jEiD@vN0P=1o+nKBizull4}j-NArF!w<<~+!J=gtLjXo&BMS|*i z;ro-u*tZ+ar`mZ13`T8E=Q8Mrpp*GJc$5tBBEq;(&w3xko~@`?0cfIe1tPj|)}w6B#jpli@7>Z=**o6iDSbnLCg@sS3pjNer!=7^qWA={0E-Zb$=(QA7VPC^3;`{xDwY;0+A! z5{pN~Hx&kVR|6}Ia1y+M!Cmr7BAf(oU~rdMS!5Y`7~B0Fu<{7!z#ACbC9gWdIq(L? zc8S$R%BK;<;4abn2q(cC7~EwLBO;sxZ(wkjyoN{#F%0e!8y^uLVi??I5RDN|f;TX@ zOI~wiK*&Ai2t6Nr4PAVMH;!vy9GA@55%C#@aa=0Si*Ov*z&I{>3oMS~8W_hVwkX1J zTm$2{lw2I?K}n3`lD9O%ne7I~afvO9aAv!Kaa=}wd4x0D4UFTGcU@%6WGZ4Dm*~pK zDl##SOY9cK7N^f+YWo(TUT=t39XLklgwQ)0DsRr{4xK3K5hZPlJ8Q5v8Ww8HqZK8M zqhX<`0x{A!8Wx%+5Hsy8_u0XEpCKF#3!M_$4{Px>j)sM13W35jj)sL!6-Y@MN5ew1 z1R_Sm+>#-9H8fY@8LQ7@+LW|iDA4Lqff#AG5g8^BGi?yXh6}_=JqMf0s3QcDk#-C@ zBLxym8%ku9K;nuqir8rV4js?FhAee2p2!q|OihbXtW_Wj(iV_2O(55$-9u!CKsKZmQ0xSOJf|3!k~K%?nLwEgI%_W3 zXBMu4Kcp{BVBtXQB$7sBE7qa@3pD$Cs(l|`42o#cwtoWo9K6-c8Whx=E8rOgPwqAF z90QM%okAmf!dQ1;wgXq&KSQyJ^ForgpbWGR^$&pOr9o*QJhfO%|7g9QUYPF@nn29Q zB(Vj!wrJ-+PV}|k_nkZh2?nZE~LOAn!5t*=+YIi7!}N+_(Se8?W-9Z&FV-EIGbCbIpCCf!aS}7Fe-k@iVvb^K1Ks}_ zrZHzsOe>Khj4+?beiLrktsGBM|dm}9Y8Hc>4T%dl82V`;O%Viv1q6V)<#af{Wm ziE5eM*M%0VWfRphc_kLBWfRphu`)}{xiL{M^G#N6v0lceT!L9`v0gS&FH^M6Dkp|& znOMEWYS~1!%pgWstd>nw%j7j!td>nw%f!Z8td>nw%M76rOGJU8S|+dA5_4`$%()>t z)tZQ_i8(jKW?1Xph7n#}ZKAFwakj;}+C*JVYx6AD)h6m{^5ilk6LmGQMHcI76LmEu z7h9~WP1M!oEwxxzo2aXaEwflxo2aXqmgN@fY7=!edDmHEDyfLNn&?W4b+w7QnwVEt zo2aYVqGny~hDUm{B`m+SXuao-9_h6fEx)yBv9)OVtwjs1#j8f@tqIPSurOPK*2hOU zTLP1u1lDLhTLNB8Voi}LU}a0B* zK~YryfgxrdQkQxjD(3%Doy+*moAv`)@2DB%V5g=*a5C7KQSVa2EAfaofvmRw8sz93 zKv&~-WMZ(TDHH8~d|YLGMg3K(Y7FyQYwdr9Y*qy5QSihv;aLwQ^x;5KyPYknT;!fM zQ;Q|^>~_45v}q)6#Z3_>Y-DqX4{-`+Hos#He#d6>JJ#TLY&O4R#c^BNXTYjBZYw9b z?eQ2G$>oOK1Gob#y@WwNR!Z-Okj$elFyY^_%#$z@(V~R8_OVy z>?)+Eh|)zADt-?a3TS3;2z;Q1{)^xWzVxU@2B>H<>IEqvQ!X3_g-4ha6nGS~1QZ@& z(xW#h2?|AoBqCs-%co=z$O{px1px8u$?V(*IYV4PQ~#}odLbEE2S9xY9;M(-$PiQO zVP4&>h6?t26ukiyZ5AT+gGWhe1V#T1A{FKQJE~dDAn{xj5i3$`UTE4t?~#bq3Pk9a zex$%Cos3*2L?)ZP*gZ!uPS(s`+_RjZ_rz!TK?H%_$wuQUhd|>4z2-22?;m~fH!(*xH4KeYFGt`^imN9ydWY7EkAS$Nc^Zq-En10VYG1Gj{ zy?soe=bgkd)9-mFR&M$|@5Cz2HzP2Do_Au^rr+~Utj_d%-ig(l=y@+68|ZoOgD0ce z^n2bLfXy^T^BX@9e&wzhD|+7Tae86=Dr9W9Daw(^l#I|c)!GUnGdj>6JxF@|%C zfMGQ*MZ{`c_p@<&4d#>f9PiC14dLA;4e;F-!n=)_X>mSj2=6vxj>&gh2=6vx87ALt zA-vm&#Z122LU^|ki;Fae@NQ$m3QfM-LU^~4S7P$r7Q(xYSeYr_Z6UndIPbOG}2Cf{u#yxWM?nS8f}@NOekZ}Qz1!n=(@j4=6b3*p^HUW3VZTL|wqV&hG|+d_D^ zF@#2w@3s)$ZR9nZ;@uX)yN&2nb0V%GyxWM)FxR^$9qGN>LU^~4INRjAErfR)t<5v} zZVTbvMxOi*3E|yFY>~-#TL|wqN-j3}ZVTbvM&44B@3s)$ZN!$De7A-0Zev=On|!y0 z@NOgTI&(~dig>pXU1{>&7Q(xYn770>gm>Fw@!|;15lCBzVAZ}6IUy{aEi(1C@EX+Y zg)pJEGt7+A;j+ev-im^#M%6UN0KfFb#21ER5Kk-vL2dsC@|nEM22JCo59wFxSvSD{ z6&Olv8np(es@V(RA*9%-PG%v;3}s9MuChA0ADpWozpghf_!ZuGxY}e4>y9u({y9}h zcl}20;G^^!IS_4&foNL{MB8E@+7<)Rwit-E#Xz(z2BIA?5bcP8Xh#f0J7OT(5d+bV z7>IVnK(r$Uq8%|1?TCSBM+`(eVj$WP1JRBgh<3z4v?B(h9WfB?h=FLQH@es|ofUoY zhH%6{v?B(h9WfB?h=FLQC%S3lju?n`#6Ywo2BIA?5bc}>3q_6?h;~kfogR)Dh<3z4 zv?B+i9WfB?h=FKF3`9Hc!)PBz3`9F(AleZF(ax`NcPkt*5bcP8Xh#f0J7OT(5d+aq zKgiTLVj$WP1JRBch<3z4v?B(h9WfB?$bo1_3`9F(AleZF(T*61cEmumBL<=!F%a#D zfoMkzL_05`n2dMEAh08yPjQ{#h=FKF3`9Fy!9Cg;gJd*2Vj$WP1JRBch<3z4w8MdD z{S@a`ch1pzWszQulDNcc@uyECcB$9mPyem^(9wE9;qfFcRrTNF8S4CbZ+z|k`)IwY zA_j@jeflswcUdRlauFbgCYX}0WOMnO`H`A+I%qi`xuYiPYvOzw)Scn^Bk_3&DYgB7 zPy^&%Fw*^WqMnm~8OTjxj8%Y$)6CT&JluV-l6&GA*U@sE=Hyn8)5kD52jB~i(*LHyLM>b?<*xk$t%4G&kr0<Iu~9e~2w zP@jQlDfxFW`oC3Ij&NT-M$aw$5Zqmq_!Ksz1dbzNn{xIDS8dXB6TR9{907&PlM>@7 zu|%y0ff=Q?|IQEgQW5NQh&NN^#H7kFs?1fLq{`dA$~>X60^(Ovr8=onPLP}Q` zGMH4PK%}ls=sW@WN2y~Zb$U=|q`G;y`x4{+8Mu2Xu>&?l>WW^suxT40JjE>X8&ToO zX0hLhN4A#XP`*Hg?DSH0U|HnLfQjPM{#&nhnlXKWUJaU2vXBC{E5`C`>>eY2Que_XTI z!5H>B7_->H81_0Cv)I8H_Bt4|&T=1b_NLp1PYI3JQ0-)~gE8!NFlMoXG3<3PX0d}Y z9Oz&K_6z-JC>gtu#*}9=*~uuT-lx1E{eL0-Z_@vD3H<+v^Q?_9K3E`mCX$x&ZsuwT zDQVvzO@qJ_M@y$GIjlXgs49y`Z{(7p_H#md9P=-CM|G5qxkwroo9X;BEs z7xThjAI8Hmjs!jiP_Pe{)HfVPZa(liq+N`l)i<(9TL=73(%uqsg{1u+_#=Qq4y@Fn zHbv_iqZoh!g_`fCtya*P^(sWRQT2~heOH*vnuat0il+%dVXoj$f?r0{4d9KM=AP1u zp^%#>;vxt%rvPgEcL*v^q|5Ecbj^nFOn^++nsTOV1@Nl?dHZ`p&fg*MYx6nuzi3B69nHe?^*zaQ*EfcD<&fnE)vG4`X?IFUGP7cq3^d=C}7J?NZNdStz1yAA=HG_n`ZBs|bKdu~aeTk`5vj)@y9pp8dP46{K-&xa13-c3mOe>D zH0@YCH-G}sJAJaCRROOgO>|P9B531)k0DL;RnHW(8OP#7bUKOWGcQjS#B+e32`HI$ zI@a3G611Cu-vH>TpnfS>LE+OfRwGrv2k~JFpM_m2PRl3he}KL&NVOmplhkttv;ZYI zEI~c3l%(lEn`h9RQp$Qz9TVP;+OrTiwz0yN2;&{Tcy5Q-%`=oOm!GU_>XvJ!>FfKs zQ&yrED(bGBt{05R3@LirFnf4tsC{&n7Dqb*1T#p8t++x}qAJhD%9h?+$L0~sanDWY zMc6g=^7(oej#CNh!BlJyXwW%nvCwS(+pj*y=QJ3hO#ZMG^Qpce}Uuj- zvG%mw8TN6ZV^O2`uo3dKTovJ!xDU_QbM>A@iu<=kdX`%1HZ0Y%!@ZaVW$w&N&^L^( zk&e`$Ng0e2r#S?o$g!oQFy;-|h(JY9z>9Eyq#55NHCc>83#D`kha>CF9vd{&3)G_y^GxHmO4Gxukc6n38E%NXV{s}W zK3Clyr|H@1Naku5@^BP$ni80f$s2~cf;S0;-e;&;ZqJjTGR|GFLT}o#`U?H-u>0E8 zdSP0$qm1D2828j`^=v$kSz92tFk@V0IhvaQRcWTv05B9B@3EG?>5@TbL6Sk=Ws*T> zSGoJ}COzjDG*lHyI?FQkAv~;5;t9Nj5yKNr|Z!!~1r* zd*^L>(TNB8V!T;MH`B=k$QaWXlMFgJNd|rSl0l~|$)K-7GU!ywq$E}%ga-l06FR#9 zG6ebU%$Y{{$fKK7{kJ^nL;7E9u=2=RjXqZP$Ayn&F=($jx--17vFGTOb*9E=V%yyI3;l zEKM@#TP7KFu1PZJyG}Cb+>m5=y@5!EswxNiC}xS2qQi)(3f__OXQ_+8zZVczbnJvv zvexI|o516!3f@rirz-0_qNY@LhzjZkaQxzimw;laXgZp+0Y5ff?r{R9EoBsl;{iG% zoV0ByBsBqoA$Cwe5GN{LG+ zChkQGof9kEQy)XgY2Kor>CWGR9?-8qjr14ErZZM}lQpymE-BH{A?N(NJ6i*!5+YL{fc0n*zQTh{$nKd2^_0YP6b3pO3bsY_Bj zBwBn4R;4Y;O3_v*as!}CK!gt#n@)HL^!j?CyxE z-4Us=W&r9v0MbY2fbe#dV3x2Bk(LgTPIsz9qP0V!)7|cnXzP%`=bZoBkPQ1cpTtUX zLk8$nK>DPfyYctvPkaWX7X`VQ&P&4EwiC{Wl1c{?a-i2n=Tm^Dtm;UY@-ir|34uV_ zc^(8ZRLmgoeyY+zOsY3I3N3yC2;RFC80+C&*|yzyD#Zg?jsv+Simodi5-q+&=ck~vP^&Mb zDjg+Bn*qBsWiWmk5aIykc$1ufsGxepWHcP*L{C4MH0*~5Due5EQyircq{$-p^KZ^j zO?TZ3dYN1O4?S)5vwEI<9qinO7sIR10R7`c7h)IV; zYllSVr#2Z%n=f%h;Mu_QnGC!0W1H0TEPGEa$S(pAX*w?nZ`)2dA4)2nk0h1Or+_X` zEhw0lLknJJqCNIJ2rmm_@TpA_ljVa?EvR1rf&~OmEhzf|oeBt^T97t&CF7|DX-DOu zp4ys36ZkU#h$9{5a`4vRS?Q2y>5vHAnlqqswNUH08`=#ZtQN#T(kVqo;L(_P7w|5pj$DsoU!> z(AOv#bS74c4v%9gKSGdO==kz2Yw4RP867uu7zho3V02kaksb)OK^Q+dV;`C+2LO0a z(22Q!`y={ON!5hqHad&}p4GsENC8s_Bs0}ex*>+UBQmnmp0B2! zIV7uubTgeAfQSq|wVp-F5QjyNEG%}ATRk!@j^RfJmH;q$bWDJJbm)snhAfNnN_kC? zTj)eO9-Sx~Y_Vf9aO89efF%h|ne@`>FTBaVWL2eT4gkFzh3ErGmVMphun)Mv{d_n2 z0dC)|$K2DO)Kk?$_rfPJukZ7Lh%9u`r$^mDpPsG+c$J-b9VvMX`mX^HO*(H#FP(o2 z?^HT_C6x|s%8U^<+^5hpH;zW05i%`w{szE1Ek>6{Wew_^Cx`am5>ks|bUz;`Yo2uv z=;3i&L3kN}I+zZvU`U23pb3hDLuZHZHopt!pVCW*u@qhG6-(f|K{Dvv3y@K#?*Yl6 z^VmREXz{U>UnR&bbbR@iwH@*;Qr=IKpO31s1`2DCil_04p9%jwVW*kS*#J#BH-j#N z2xOnopAFiD02I`uJ#`KUROnhJ6xBD0JLZeCgL6ED8-+qbmLej@1CUw1oGPRwv6J%0 zD8iou{Ltvvy6RF$5(Bz?tNb&diJ;tDAJEf>nNTkjCYtG#0%XGJtCb8ogOd#U8UXHs z*O7_;cl|)dSPIQ#06H}$EyWKx*p$;0KL)Vi9{XoK!_D{=p1HICq|Zs*g=g$h09bS$ z1IXmihv9ZI=sc5T_?fP#_k%_VynQWY69}{vj6p5xdaax{g+HJ#N|<<6w7q-2no{B)>6M?OUF z5&Fr}n4(gjj6hs0Npv52P0u-VF?h?R5}j)SnzGQKdmTXDQTlEG1hY`wOlngU4{A`O zWo?H>@(I@5ojWjK&d-C(4yERo5DH1LpNw0F%Ce-@i6_o4Vrg4GdO^QgPc*<59sDB!5TP>)&^2-t z83thF+%a49{!IVSEh}HrXR1*HjC26>>6A*dBV+LAN-~{N;Y~gUDGJpbK&J#plsHJm zHS7-&kia)=Gwu@iQ1XY3zZ_ zR{*@m9lHC49~*IaKSK#f_(a7JdXyEmlXQ5V192NbRhnAqGHx)Q9Gj#p&v1|Mv&H$0M@fGUno_8@oXtNMV%HYjfA>DBl;J5MQGOaDgzlp!8=eo%Vp7&PEX z(a+#gfG;Wu7p`TXB>{nC$il-@<)zVP8qI_tGL||CD#70&|4#@|DA;ACF_wg&WPq4_ zz*YTKND%^k>p^0NBu=nb%y-5K(PUj35ogSQ4q~vLPZ6iZ?+3M0Dbfs?r_9LGVh`yQ zM?P4T$sFX~@S0v2X2Oy2cwKy)Xn?udBJ^fDa{%&rpf4dAbe1L=^j$9*bT%g$9hr9v zXb%aIWIZeE&NU#m11#lyjI@?6P!fe}iT_pzPopFCIz}XLNIX7n1V4FuR2&wMu7J)& zU1gLPbtYoA83>F|@Sb%LV}VHyI)f*;fBQ_&uHS}d^DO`r>FgKYX>@)7pk7hv9t6k? zqmOZQYksHaj_=TJ>Cm3)YlonXPk^9-mbElsikMEN(@-nAA)+0YT4@Pn@x9tXZu7|N z-qeeuJ0S860B=@0`)k}+-qiELGGO=9H}#^h4AqVPUC-~)CCoa6%ha^29qh#bAF|>g z`!9seW;(Q`DHoJbH=~xtDEAl!OFABWKaJhz5JP~FG%vi#VxuzEIkoPqZ|gCZMGe3w z{an-s^cw(F8+0-PEMH$$b|C^@+PfnrA1^&bsc&S~xC`FWBMAit3BgjLbr6%qSX0s< z6%&%l>Rk_!NC>oNg2bdc=F1BMGXCc(i_j58e@3l3 ze!_;TI)6DGnQwH7T81 zfP6N{UpTgSI2!bI%iH-x%l|@U|($gU6h)GCB<>7Vsl%k#PBOivmd@gCq&xekr1p4r0 zMayXUKbj3>=pGThXgh^fPD^*!fCnZK24Q zF1G5#kqjX+-48a9*}VN75^Z%5943U5kA*6Z zORNyYWX=SM$=In0aS|eJ1Ts-kNsz>(QF-tok1z~O&JZO-26Vh~;Pr;jd zHgMe3T<@+jlf+LY8z4eIyV92GfLkrl0j#+@J^+( zu8!X{WroG-}Dbfm##CSm?9ev~02gt~H2*;+re5Z((w{Sd>PMhjf1@S-h(4k7T! z`b?hO_HO__`kLwP0mvxR_aOibIy_uWGJI8etQ(a&6_f2C{8JE%C6N{6?hOR9ca0@KQ%RwAvG!od`8 zSZSqG4G=BrM3-n5gCSMgs3HSS7APDYmq4|GdWnr}8PMf@2l{uuq8VE!A>AK}vN5(LG3k)*_r zKmEmw5^DL19iEU76z5Aph;I;#7CIXN zsJxA}^hkwd(+P(|N%=d4d@G&xgIx{^B&NcDq98ZZnJv9^7@uIMjH5#FIfBtb=WKuo zl^&_k5vnIXLx{K1VF;`hI)=pDnNR3p7k%P}d)&YEjGHahp(oQ_`hlLdWz{bIem&t} zN1r|bM5Y{0rSyvE;KIC!999BvBJc@;7ic$UcWX71#8f%MWLV0dfET6(xkB%Ulk zppe)Pa8p+z_uIR(Gm#p*uy@EF0J50Qr(!FS^Az*H`D?LwTK*zpgA{IQ(&^6GVm}gX z7(116Di2~+Z3N4qa98ML0N5i&HyaRAoY)Apf!>)T-2)%$dDB)yPJ|oJPQ5^g4{x9vhcRiY4ap9;m;T3 z7CNQE+eW9aC-HAA0{#?$Q0ROkylr&8A1*_A9R6nnxrNS)!rMmY6)EAq^1gnQ`^QIg zd&}C7^}a?0*NeRd;B!Iu4FGOGowot#2}`V{XU|CY^Uw97<2k`S1CK1{u+K*ZaE$@K zn7HtPF4tDDz&+-9o(Dt{N|7H0tGbYo5q z!#rb?RfJRFKK&moGOey~Kl_h9IP*lz^kk(!6K*NoWfks_FZ4bUPBfiN{R(&P7kbr1 zPGmS9_&T_p=ENBs{zR5c;GEks7%qQUBP=HNGzkpvc(|N~G#l=LO831lu>PHs(rBO4 z3+sY%1Kjj4_2O2Jxu${6k$$$~I5><=Toewm6X(z_@j4IZur~2V59jbS@tJU$zS&i7 z;!8b8!*m7rOMO%;$Fi6|d*GJ9<=9a@TnyQ@+x3GOq*9@K?fRKHXLAUig(>6=6>(yFIs8yAMHb7&~{l zz?;7Yam7+mxMiR-p6pO&didj7ME-IB4K?n6ztSgUvWL6`_+q%EudH#8{aPj~ru^nv^bQNW4*VE= zjc|D%{d180qmBK;h5ZAo**s-AWs7beTz)!#tSHAR*P(&I4CRjz;LmsIXQDM_*DHR& zn<4KPFeZepn5lHV-!izE%=Cu)z&CnzK3~^mpzzg9EjA1o?Hz;NZ@Ec7)P0a+EK5+kvl!HSZZllorkQYQ*FS{>Ud6v{zDt@+P3B(DZbEEhT>{1i z7-)q1Iox1!>Oj+!Uw`9A#NW`j5fwip+ys~9l&_3n0+?7$Idh+{=kL|Zmj)v&w~j@W z?+3vIfj_#N>)jsva29O)0bkYMP>_2hwnJhuUK=dZ{#K0eil0myPPOuih`)c?JCJAI z`FoUk;tKDJraXmJFtSK-iyGcNv9rxIHfy}!iEuS#Yq~GE8TBy(^EZa@HAUzlWUo(W40bHkcIN;JWe|Z|$qV2=c zCt?MgL{NV4PVysiI72><#(a6-Mt3e3y>%;x^9wpf;E;lz+@bhEI$yYiQUI3`9u5** zQ0OlzIjP*p}gwLGTNhpRK+zwqpov#}MRVQrUQ>pG#2LmSH=Fz~#vVwqsZY z2yl-N8M*FDs)03IXwMKb09>7RIN-`P0+*|G2VAh$9dHpFf$PY+K%x_Y>%|TSTuVma z0hd)oUDn^&gsVBp$rA-hb!$q zk0suZc(Ab3p9m7}J&fT0M($8{f^@Gga<5a3XfZ1%eqa&&3W@iJfDf!cszyFsF5JY} z(ti%U6zSXOPKLCfk~2O+O+*y6+G2xoop5ydBWzBG(Q;1@0pXKBc+()ep$0cXfYC?oj-$bcPi< z4i~P_t?vR{#_LC^%Eb=owxB~7n0B@Vy5;98C=;L}?CUI~xhOzA+IH@Rc3|?KQwoz^7f{ z8{k3U0ye?{fFEJsPFG#xUv!a6FqT2Vt^z(kNDoH9_jeZ&pBHo~=SLKYOo)`ilXQ0k z1emHIMA$tA@D2F9u5@;Dbq9r@b4{qP9~*!Q;pYiQB1>7d8VDya?bb zXTkwMS3_NuiGT4Ea);sv>EG}s`4RBl?jqvzf-dF!h%%L3VMl<^&Q>lJTNkJfV<>^@ zX|TW+2obqtQR1xA*u(07Dbep2NtP;~?z^PT0icU)5O2^jH# zt0a5{cCb$bGy&MT?&pT@lE%(G~b36lZPs%c8)U%ttI|zf$xz&Csark{GhTlCa z%UJ4OpK6rH*%RLthIt}0got}mjcjUFe>bEv?xzCn(h5XJ%~2t~=61OM58g@8o?`eOJA zO8`p)^lRYv;Re!=@xVfa z1>mH8&b#;NEUnvWN_mktiZ^RBz<*}w=Nx_aj*w9=2D~`fYL>h ztXBbVc;M5UFmTlhIKu;<&H=5v0QYBx^%Q13a=eL0`M4Ycm;(H6kn zo^BlHexe!~36Ad(3;-u@5jgY8w}jDWGvd*GejIo{pcqgFC<(AVnw91p2-K zBrSNR_kqTNM*;^P2^@GNaNv=^fk(pfA@T-tVv0XGg;P@eSkm7W)(7|;hxCCndk7l? ze2!0c2Va1`uR|`O@PKDdS}F%{#*!Za`hDPFW_NT>H1g@oVbqV1A8=5Q{s{mMh)x7F z0hZUJ!OjuUYXH8X>)_|;CxN4%gnI$^19+ULpX>B<$kWGteGY#T-UaYbHlI$}w>=(* zMhVv>aN+pq4FF$|Ck_1+a7qq=2LK%oG>nxV&aODyC>R)x`U;0L?DK*Fa3J>`4>*WR z;K5S9Jr3M*(3ZflS9(zZn?gPvE=`^buRE#6!#wA$LvW00RlOJ@CclQ0O6676Cn_40zse! zOcYw^#R?{oM6pE?5)c$QSb+u=3sxM^ia_BaSF9q?)z%kPR4RJa+SUhLz~aTXp<)Xv zdVjy&ot(+`9zNup@BIGrpV@is?C$KYZA~q7JN6fAO*}5sy|++IGp)k|+)oFHm}$iZ zx?KkGNAo~gS|skW9v>(V7Ku^THv{FjMdCec>LB^vU@^m5H%N{iBDPz2Fzc})Vv;qi z$Q?XXJZ73r?vY`l)wb{`-B%NMpXHe%`DsEVtjxh~zf!z--ohfA7e|QQqQHp^l#9#6 zay&_lPxNs9f-UvCY~tROb9Ztg+4xm0Nxwc3RP4a`GhcmQ_8>?LS%U zGp&uo+_$HQ`%LSbVeaB;(ap4?#qMo2V!CNH6ytgL;wfumvHQTK;u^>LrdW2EA%$peI#ADvHZr&JelfJQ}nQ|C>o3 zOn883|FKO{j;kO|ipyh2dN_|6?MmfeS!Bb4N0IhwlvGe+$Wb+d8{+QJ9}C;U!X7s< zQ~X?5c(Td9S)$DPATB?iB^FxgC9-z5I2YmJmHoKK;-2bQ-Gi_^9Lhfl0C02F9eXl{BF#GUW z!#-@8PEORZ?Myy>6d4?ZY^_VkV^_h)_Jn)q)#7SlJ)e;O`k5GE9ZtB#^TaUI5~c3t z*NP#QRah#2F&{rhl)8K8qp4U`rS9+rqK{+EES1w1i@6r=&;7$w(tsg)1kzvkA2XKlkLTdds>e)ysp3)Da{?Cm+W$z7%LM?M4D_JFD{pl zP7sHEJibYcl=dodq5OEA=+vvJ`P$(ZTs5Nff`v=sbFuo-m@~I&X%l{?$(mInH;=Ow z`%hk4sL9^;R);k!Os8|iJ}G~`N)$&pk+J_S_pcHY(>R&2Z?&YkT8xWuMq|GsC#@Du z5zcDtQ}V!S5$(sBjeQ2y5#DCbbnJ8T%xZCI@`>fpquKcp)5(a~&m#w!Xvj`hq_5K* zKk#DHpftQS=Xcwlw{%#W{bEjfzT-sfjF=NefRiY3y4X)Gk30GH2WEv+)ZghcGvA5Y z)2G0yAUntDWbb2Yxsc7AY&*+zijtA2;n+fBlbP;ZrUH2arlT_faKPC8<&F`M<`5S5rb5SQU%5E zTMlUvnZw38T_X0`WhOG)V$-6u6tO=+0P@m>d5PHT?0Bh@9Vw8tEut`*NciPs+PmNw zCHK4Kad}UR7&PHxFY_7Lq^SKWJw%-XmMCoY+8j7~+g~7xSGgI;VLX8XdB!tTweYkw z*NEQeXfkh&=<|bWc-plL*@{LyH*W=Qj-=dd_ubs-H+E`bhq}Exp{Yp zi%mQ`LjH8E_(R%o?BQ%dOBR}6D zMn*X2w9m_-_lQ2ZuX-h!fqX=wUXjefiZYG>iCx-;ejDL@)IKPGBN3l-Qv1*Hn8bN6 z&P(m180WEV=u>ETXbdG@GwEL8m<6fEJe8KGQkLdrmaXcGvswFdUo#_J9VZ_?dcj>u z3>_oogR@=xtY%lH4CCz$y*jH?iH)72S>@+ec}bH zb(H(Ted1E92j`b|3u2qOelzKf5pzLH@SuN%6Bb&wV%_2@R(RLiW zmp}3jgfSS<{$AS)zI9oJ@1`%@aH?zH<@YYj z>1pq4Z~LLcL zH*FUaD>)^$zoL|l40d&F)Rf&3y)j~UpxNTMUs~OfC(fGfr_r)azYWISueXcGEbnco z_HX3gouV$n$*BF7?D?RmvRVzfpj1@l{mtN*6SepIn}bGZ|7m&L3p!`W{NZ9$ca&mw zP~hBpPGd3JNo$uFWZvWs*(E;0Z0UQl`$OU;YvcFaUp<8WoI3(zA@Ud*gHFir8ppw# zpYR%ohjxozcmv1_yG1{9n*870Vr|D>D3+JJy=E=6-X)I8XSZQ6STRwg z%R5W(D27R+MNCeOiGkkOAg_2t{362HvRxq$>=lFL1CNT@2&c{V>vH`b@e3=n;+{R? zcB{NXuElqRGiQ69e0HxW@@+YVws$a~R33Ru{Ev0E!hLeDC^xNnmF_?Hi6f@9yV8C0 zcX(mEb-2UDYHkt=3mu(L;b?W z+J%jHBfI?cn0Vdse`XyQJ=|B0i=CO>>Z<$$6B{EJKXa?(>QBX(2}S*l7#TM_@XOeU z*AVd=w}$XbV}HGFO6LPsu-X-VW%_4gV;2vDKQ`Xr^1x?el~q^e4*XnrMA}tj}XDc*z!OWV8Zxd$JRaUtm_n_w@xlxPq3QRVExt!XbM~{2`cYeFOU>pViUkkP%jD%;U6G;FeSb>T5TumPXe`G%czTQXEFNRJI^a9U8OfzVN z0a-an$^~accBk!nHOpyVitOZDS`b^tkr02PF*id*Q9#}YkB-21rp8{&(U^KdW9ro! zL&sC|wE!B_X-o$VAzl*VmJn~$nEv|Yq%usNsnyHv_N9J+s%aX`(hRns9Lai1TB+%~ zpp(%+yr$^-Z)A>Q4dI~$K`0f)Uj zRqoS*=<|ri^m$xk>J{BnMfs+`SrfFNqbQ7t;Ge^kLnK|ir0E|*KLoyB)609LT>caK zXmEk1&(d@gwKN;dTZ#hzNuyaa;LS~oz0x5nDH;0rv=b} zcM9kR`!uG`y9adrjKn3PZe4%X6!j=S~UDJ;4bN6GAx)O0QfRXR(8OQLk?I zBue8k-m49?0Pi&oI&*E5&RiR%xSzibsl{iOF${S;4-@7^Z39lym>L?fG6XXJ{=Cq+B-0KMqcSZfnFJk z&!t>SuZ2FjU3`p$JYTO28euRkdt z+%bktCOAN>Z`U8{1CV_pqQy}RhHsqaGC)w zM}VpqUP@3pb2&?6#^(wyU0XnM zHoOhe^p3_@3|(3P4Y-a>IUrBam|NHfPh|G>bYr7etTkMSJryH(QSg^*s63<#{Fte6NBO$x^zX#gQ_AFJFJl>P zi>c(bmeXT47pW)Ml2>U|(0eHGGBEBH#)kVlRUGlP=}dx`Y?S?3wh6F*FRgEnBfvJD_REIL zk+#z}$tyZ49!ujBjcLcLJi2a-NHKL@`B6HV7km`c-w2JV^V*K4Cyk9-01bGRMKdr~ zl%<$DFDB^vqZ+Tw#(m}G?GIhazP;a)agsE}5gZXQfFV4%?uC8CHz5aP$oTjU>a?y< z277W0D!al(^0e(_oBQN|h|_Pjw{V6LgF34o**@z=Z~}~;Cvoot<`C^2=HokE)2Blx zbK9vmcqv`WWi1H7$FJsQjagc}jN!?F{TpXkfBL8N98G)vA3r~hPSL42TyEX|G?X%2C@*6OHNX!TH!r zE8YPv!yJc)Gt?geAIZ!8p05HgE#(0M#8_65^R)b@3+)7l*C_pT|KYC!EZ16Sy8HJ>~_x zEEN9^@Ntgb_+-I;3x3^<65@db<0tooBK$tYheG^k@GfitC)spx42+NWHw$CY2lzF& zO0jL60q=pms*(==1uicVsx@ikAfO&qz;kzW*dMHyNHO>_)L|^pF9N4Zq=PZF9d|UY z0ILt%&CkHPzeVjhi6THiX*89X3!;s!)!hPHz<&E978rh@S$_ z(7MfQ;GEv6e*8Xo6H2HhNWjEMrDv!EI0seA^S8`ESMdHamLU2+1NtI>$9YRI*oHDEmh3&DqaVNGygf3wn& zzQLh#wF(G$9e6A15F6$t@avGiBgBt{_z5tplx?6Z_E67*_v2_yRpD!aJ=6Cx@D2jj zAVJlyKLhInhna~Y#GZd8?779A!8^2(FdwXM@z4;Lf$Ow!VLW(l74D^E2C}f*oPmG= zXmqO8)`1W8^Di1O!6xt>Z4OR5!Us3IO2(taOU&(PE0^Jv70K>v%U zC;gF%Rd5Ue*P@m3fp&Co3T%9bOB47ogI|Jopx>%ioPkqa=a66#blT^Ex8jKHlYmRW zTiP<5XGgM7ZYP-uHBVaUkm%{=Fwcr70j`cHcS4mF2L68wPU;3O0!CwZtadJMfEQt4nh->I7mTjsJ;IM$`~^4zTdayS6H}u_ zh_9+(0QgH}ShXo$G|{W4;efME^Hea(~Q@^o6rmTV4PqHoB*o@5?Lsi&qDEWJ>Pf*ZAw*H z1nV_YJFj$%{|KnZSgh)-Cs;3{0`PidP&pVL(ksC`&NL!R0QY_F9W}gB2`7W z82lmXFcvtx8Qh3-?5fCD$~`$aox2G^4S~u1A(uPBC(u*b5el$-*bg>V_=T2bWEwAm zM`OP;4+h-kzlGwT0H4Fi&>Z;x2s|whcXtH#=d;j}^lgZAg=O&JtSl3Js86bA=7H-m z6coUQ2@VDC*LJW~;B%NJatNe86U;-B6=3AkXaN7E3pP0D2MhUz*EQG!BV%5Kr7*Y! z4%OgzJ9r|R$+LmJ4y?P0>{0b)8su>o9?32R~Fj(sdV?zd$!JV|x z{f8m_a`0&M+_^!53&0r|yHp9bgzWDE4@MQSO`!yE_Aq3yEyTOQCPrhm1y6vNVrb!2 zbr#{vA^SJMyf8NxDX`Ap0%Il-!(Z$VBgW_L>@kr(*Dk;^Y@C980zGd6A~1n0aBA@D zVDtm)MP3x*Qt&v;SyZbZ1AYZ1QXcr54sP?8G&&d!2&h5Vt_Tbkf%OEIgV$o(r;2oA zNZ%IXhrkzVReAt?6un4g2#1nJCtmwRFYqSPrx4Jn)p090Rb+UlWVC&Tu!hFik2+K( zR0tl2Ly1_h0cGHa(ThaDlfkRdZI%8*a4>Q2hB`^(8XDtcH6@TWy__D&1ITcC5Pv=7 zM(k}=dYi%d@UQgUV7&xHRU+~PHj)bI08NdeM#W|6u21MgR@g!jei;qjZ6Q6 z!8A<5)Euj0PWuFNz~?Y{zYsVW7Scz8)rUh(CD~MM5-;=uj|pmwhr4w-7?(O>Tq=Ye z+u+SG&`09i!Ryhc*tV%}1FP#+N;UciwKmQqg5dKt&9#0}b zKjJ?fa_|T2-gsTB3K?K8`zoXhbkC*Qz}gY4XCNoU{X(1^jDXak%<1n&lG}>RF{Xqf z{3yh;z-zI84#pYds*pY(JPjFS(_o3+1U?G;ppzwyyAXik!uxO@{huKRk5F%$V?~UE z;1g(+4G5;gzlQ9Ofsdk>|2oh=0b|;f{$#*b5A55pC-5_baj7=~^dcJ&;yBn~1>(aD zP6X#*Z+;1w8Tety{_+qvhIl@BH1hde=7d0%bZ*G(~$DDWiHO!kc zeg1^{=KAW!>#lB|Yc#a|MDV;h?vL}G(N?Cno)`;^>zn3Z=ib@h*%|3pTRUeVS3A#} ze|7DZ^-YbnwQ|G|=k;#?z9iYjvgNz1EX}_0x`x_X_s(MHJ1495C!^;qoC9&r$PrU7 zzP8!DW4JTSOjo|;{t?b8YsDD3xePZ7)!{Ivu1fx~%qehi(3V~(KP+=@G7DwnNT**0 zC%{Gh@diD)7U*zDE|hylI{i~Wd&t(2PG*wQ#?h8B_XYp33I)Wm)B7yIU-lX*cOZ>B zsw|^o1=X z#&dE*xzoRR`e|XbB0Lu+M`6zb%EJE){7=;*F4q@0#+~wgM9F<)v}1e?S$$M0{|B5| B%CZ0e delta 90647 zcmb@v2Xs_byFPyQK2y$2Cds4_GLyojl13^eBmqKzp@b4piijE%H6khkDi%zzBBFw# z8zd?UDvAYA1Ph{K!46`<`tf0*=%<5?7)t)nbIwcz@4f5Z-&+5ywa@Ig?!DjL-d)bg zo@0qCe@$E*$gJ#T*EE}E@2LeehnA=Lv}8?a+RRD)g*K_bW=!h8q%QN^#{Qbd0ABnF za%ftXM9>1j8>s*p9K?s|fhwVo?zHWM9qr9Kl|t8D$7(+`V38#cj;ejEro}xD4TK zgj;}3kiN>he-_2!5P*_o<)%y;pT~g8>EwM!mRB3oSgpwrQLHufp=r9Z#=mEDYvlY8jP55lT8@KNO>B zp*_b`vLQ0ZX-7owL;H*@k@@}sS#IwcRWI&Zy`eNqyX*Y}wmG&{yAEhs`ByuJiIDzR z^|~$lL)Xv!v*Llf-g*DF#tJQP5kd{Z%#G2(#9%t~-_)iS8)A`M(yr27BFTSa``K5D zWzK@EVBS_|rru^R$;{9*Md-?QH5zp9MeSml?U12+GxIV;=)86{$0aho1;>UVzojT= z&M_^t@tAtVCDNPi9Rq*04XU`ou{JqaKU@#>`>Sha>9qV{Y4E1Dnn1_lyE?pUZ9x$< zGV_p=+;$`L!Q|*e19d{(+M^HE=|-mQkXI1y+8(zy04UjnyzTL8YXMc7kgGj$Ejj0! zkc8w|O1ciUn27ej)JV7RC;jx-t|6{N!u8uuAJ|U|QtjTq)HQCADtG;*?sbby`RZTl zWw$8G(n7CxG}C-{+&}&GjQI=1^^GsfvygWpU@V*@LCecdsuT z=Farb*FyKIF;SvR${k0)+BfLd+Fujv?y7U{NFDA881sF>z%VUzoobE}S<7^fS(nVv zF=Xhg4vsFun1T3QgoOx()^T_Ze&0a&83FBV%#-UJPqzd;NbTk0c9r1~3C@d-I)WXC zH>hfl80*(UtsOt@a~?{B?22Q^-JwKLnI=Q4I@BW`k>}AuS9kn1w-)lJ9amp?#O0z| zjg1y#(#Nz_Uu8ce+-vK`9DK)Ne14?K$SfRO790lq4s25|M~ghi;Nu;Kt!+%)mpgFQtbgTvwy75k zru|3TW7FF05#v@EhoG0qVinT^i_dpkv^mkuvL?OELD}m7L?wnv}lr$Zvnn9<=LF zvV3(z{~a<<5B|RMzP6~+s^Qft#Vb;p??0+#iMuN5dk)v`>N%`u{g~fd#-Y_6t*H0S zjay%W%KkUTt_R!gsG%Q@jQM^0>~%+e`&AxE(CvRD&GfckAIh&!Zj_-<+l}?HSy}UK zY_0QO{qa@jk<>8$bZ5b$ty-qYJk2MLI2z-IuRjtx6gO;s-Q7F2g1dG+*=97} z*5)p~tu3ly(Y}P>k7$i-i5`Txm@Ka}G@~ct9A)et&7h%ZXs!3fqP8xUL;jd0kA8MQ z)y9cr(NCQgC+ee)9dRzsdp-|MD@i>aCklmIeHkYvmbD&#Zy&Kcj=p04wjaSznm26c zzuHD(e1Gzv{z;t^FZzi6YDK*0E1x-{K8+W-#+65@JDXMIi9_C&lEan|%;H9tWl zG_#B>4;vKAJ#(W7t?h7)c8w}+(X#WJDi&N{XM8R~D>@3BY=cF`vwyS@ ze)-KczbUf`?P;m%(*;((R-MD2CUr*_;WI?&&aEOeeXDx5izwPOI7v(q;$ih*S5c^H zlEv*}k$N{-%oC&3aG%H!HR?j2$Zf7|3Emmh3bfG8Ek)~NR%nG7;98tTg}aSI9*iSC zOuLoOUlNk`yr8~r%TMxF4ylRgOdW^cI+huXS#CeXvFhn#=cE?Sxfj)0G*}OH)6WSF z(6eS~D+YzMffqE^2J?~W(~pasFZXGOJUTj}%lO$_H73;D5;ZzFDwzDU*WGdW+v85O zj_o@v^~;WF#Z^1Sit70?GiHT6WC;C7hi78@{6pU0vvu;2kRNRjyJgV1C$&vP35MQ| zg6B5;@;S2M#RMdttuwZc3DrhzP9Au0cwGHKq3g(!C`YCSUENqUr*E*f0{QYD7rmaX z>#^aMkZ>VqQJZ((vQNu=W8WnqVZUKt+r}=zm|!+aYzuY+_o!p)3%|HiRv%Xvr-)%0 zw&O;x+8%oF$DLKd;h@JH*MsjL&{TOaBUlS~kNPY{sH`=|^c6-$ZJ`YA2jf5x)*mw& zZ!Q31Dd6kXl2laBLn<~+q===eFior%&#SM~#6Yo3bxRi)iK&}zOBdG*@qmg8h$}MQ zn4cVaWsv=@+R!WWqtFMm&`a}QJS1cF9b)eKy4uiY^;kfRN4<9hM1`28Dl$Yr+5L!` zl_9F)9vrr~4yBa~2ikTy@6`&%t1THK4b9@q42-@-JM-)iGG{J5)IZRq2cZNJ^1KZCSCYH=y}$r7i)wS5xq3UkhU-_};wa&=?D)teWJs{FK$!`<7}Q&}P@XUOp*^ENL$ zIo0Z0m(?Y1jSFaj`+m5(Nh{o>4rGZQ6)Sh&TX+BGXm-^u1p5k6VZZA?XfH19@*ge< zxwYANMr$+ijL|N_Ggh0vX?V6s5Mr)6BS)O$I=ori%7Lgwy`3XQIp5n%V1Y`@1@KT9 zxHMM`us4%YZA28Jf8>fWV(z9P-NiN`Rx4M&ct-y9vU)jR1nsM#;49@S5Gf8u`*z<{ zP#~&AFL7+u1%-oGO@4ub7+%_L1*}O}`b2^->!B)FZ{B zzwoH~-GlFK)Iz_ttJ}*(K>y|ik6K@j3j0jGQ7&@C3+hM6xYoYFDVkYN zs}QG%?dsABQH`G`E5v}LpDMM$dkB+|QBi+NTR@vRbW%Shdy4XA;+%QjjkaLM?vBHA zJ30=xbi|JMEo)cfHtov|gMQa$4+=>=VQRt!gMM$RNSNAj_&o5e?r=g+wpR-k{?&1K zen%ikgLE8z4HF9%AT_dn83lu1{%70B+olqEQ`(>pOmH z&ib*j<8Z%@+FwO?y{OCX?2xdXmz*=Q)(=eA^>tYnXg>;j_RcH_Wg+pA9U{=S#a3f0 z-M8<0=f0hV7i9fjJG*dd;RV%$3a5^3vt8(D-1^&?J30<0bm&d@*B#ga4#*ZlwixTG zJ^RR?H8l(TSyQ7HG#9p5>_?CHSWwm`tDoGkwr<;wEK?|J>UU^&Z@~@e=x`cZrL1Mi zjN!({sJVITQ=acQ{Nr(v6*be?q7@$9L0`_8*>U*D%i7lMu=dQ2*RAU~eC@HA(RTO< zGyW=XT3pv>t7k;i@UcyaO~qSx9?%8~^?5Imkafv1u{;oz>o}8#AllX`KIxd(6c4J{ z-Xh;W9E|X|W@%Z0;L*d+9jAyTpvLtU?~D~#-0Srzb5qvq^&|d#Jk#Geb0~3Isa9*N z-_uBwAoT>5q{doJqGS2m_glGDSqiP7j<#Q(Z_%KYo`ukA8A*Q)Wn)_cj)(Emb1zj63zCpG>}l43p2#teKc2T3MMO7>_Wz)ROn747Dp<01-`d`*+M6FGIT`h8)wlLtT z0q`^W0x7)+><$A{3A`2tZmSVzx$Z!{Ta16yh>OlY)uO?;jtpa?f${9tf@-$E=#U zM7`Y))46Ta_x(hP2+k|gj0+GRMp%MyAHppNmmy3=SdVbg)C(@w@OQzq8Rzo15*6Nn zF^qnI9pD*&!!nu&@kWG(Tr3*!8vV8Rc;otMs&)sFF99f{^^JK1UDo|ByYsvRwXDNZzXE22>L^b*1@gMd zMLqtmj7d&Ss{tGUFO5og3gAk_QMuQnL6`+2ercGG1tt6}VD66w!tjnB>f%Be+f%IW zEJS4vE>s5^gJ2FBHjOd~yl*Of0^^F65^~#rKeP_DR5}t4n(zotZsk zQT$xMkw!?q1tsdK9%uwhOVkfN@e-q z{497D0A!IF2)6?6iFl-XBKRG^pIB|I8uA+qwOaBW>%_|Jn17r$B&sE z!jk~g?9f|hDwqNUJ9V8kK$v^tWLyONq`&cJyE$^yMRn4@1N~#OBPFwHDYCgjGx97E zHm#8=&_^b!Jz3J`*c`2Cdn(lZWinlCR$Izsiis-fslF+b#TiBHa`DC|rZn(yosE&e*Tr~l&~LPCMB7p(1RCQ5 zW4yPEp)t0IjIEc|1c2w*IWh+A@Wk9jnsqo^%2^??K8FV>yH*kxfPADWgn6|84OWYL z$>a-G$7)(Oq-jm2NO&Y*<}3nF3+FBZw*ZcG0P-&eOj?AFs1_NyNH<}#A?Cvo)mp(G z0FLa0UjjS~aTJf^am zh)=FmpZ1cuy^kWER|$U)cL%8FNSpnFi|1&%&3Ol4-LO}JNjGfc zF(vE5@eb1UdV15g-8nZv{8)|dj3_{ES6EK_x@*H!}1U#DU5mi(rGxGN%!I;(P@voAzR zV*ZSyvR8km|FjUKy7`&~H%_W!!9nDHb(Z z#$^&rwWz5wF39B5MEYVBJpffqH(jX1)pA~QJ+tkUR%yKzG!7DHA&lrj!^decE(`K_ zPb1B3L0<0+>Ws3WL~nnp^jJ``x1PyI%Q%)K)f-JH#-athi>M{mg0j8in@L1@T7b^; z-p(?|S;SKBOH3l(f_i$3O&eNJrPnkT3eF^Iyw8%>#iG@Ea|tC`&|t4=xUN<61duU)3!3Det#0clQ<~3W(b6pBG_PsObW2;4w}JWXX3=Iw z4>|?rf_)1tL_qo^~>f^P8MOwMepEDOA)%wdj2TjU+uOu<|W zTo0b9~W#O?S6Q|v{05(I2G$=8~_ulYH)bR|&a9VU#Q0eS5(oOh&1qhie) zRg6c2NasXm^B#b1@2Mg!0l$X#ZbFkR$m@Lq#X%cLYlmF2_cua!T2QJtk&5rJpn&%P zX)A=v86cCQZzS+myg6)qp@t2R8*)BIe!TnBz5ykzkmLhtKQn%e89JE829>r|b*q(= zo70PRf&USM)MaavG~MN zz=>A0#=<2pG8KHH2dF?aXgQxz;oB#&_3mVQTkjTOdlTjrFIsGz!bIz=PPP{#Y`GUQ z&6o5vw24vLA7J~18O(W(Y0c3cEm%ZvHiiEKVXG{2^bDp^0?8}{OT}e+H#VyUKwO7V z6jWlc%xYeZ#wf1P&0fypeTsm=eF&B?C680@6x|UExyWK&v{sg%Q=U6EYcm34FJ(fx89O1(laUB?J5Y*L9jB}Ll02+2_POJvRu73SmukN z>ZidnqizKamSE*4vdJJ?HLL|Joe6plbzXuNa5{pOp9H;UBtKu+)R-Z%OP*O@U&(Yb zv%cEEEFPDR25eNYH;dN$O`U4d5SiS(2w2gE+=y+`UN)f%BIR@OBwpu(E{?>JQQ1wT zRV)Q+HoTDn(I6@d#8^=Vx|VYf6FFZxJRlD?6KR06*fR-W3%K7#ija_VfXf7K5pe$| zE*feEQBAehvmi&624#}JJj@UW4NPW8m|m5m8bO!Ca@5?QOwVSkN777R+$Y3#+yJtY z=0%fU$KYOJU+0COtdOB>^-ECZ3_T8(a5l86pE?2RFNKzB2U1>!tZ6yefz`wvLd*e^ zWwh03#J%bgQJ>lB^r29CXSP~AR3`Sj6D)cuEGQ>Ry>thxM2x)>82vV(mwp$1+n@Spe*woXWN=DL;x7eJQL$#o(o|fY^}u%mldb<2MH(R;M0VC8hrTD zmyi~*dH~o?V-aJ|1E$kh#5g&NbsCEpSO09abU2JP7=UeXwt8u}%x@kE;D0n0iE>yt z3pvPO=>}whSldiyA!0gWjAhJ1#B{{CH!}+n(-Gt0<0R9aF$V+8EClnfAM;s-bpEzm znN~Fo$iES_;H;hjlx4`tG~{d{p2bzR86b&Pw%cAyU`Xs$zB)qIj%x==G?MmJe=bP4W8x$W2K09q7v)QC8@87 zAxYC}KgL6Np@i+#RMHI4)H3*5bOaG+Y0-wf$>jcu7Oan;nc80_+t3#{-nE-)X+yq8 z&cz$iVp}KI8->i$usKZkO_OV=nW4A5%#SvtobyscwD7@@)awRhDh@`gf>E+K_7t#* zOU!u-8NNrIKT6j0-wr9+AuHxkNBw?0WTQ;f>o4LsFdnhm9mtkro=DX1gW`ctqbz`m z$)A^|{yj>jpSc7&^asFEKL+U20y ze-tUY8^hDU+28=mvCZYnUu z?ll^?c4LSeZHyU-?2c3iN6SF7xyC&QvozPZ0RznV3u3~M;l-`B)U|!>M+tX!%>mV3 zfrqpfz0z9r%99tp=aRu%^vaVLy;y5%l8auHlxyB*EPC%G?&L-9!zRjH^vaVLx~GBS zcm|KiYIio{;nnUlvLpMfC z<#NnnM}}>;f*{{aOGDh52`}CwKQqPxFgU8n-9BzsXbI<>t;x@Q#M_K+=sfZZe;o)0 zHg`6SJw^5t&0N&k%AqW_%C{16=XHo@$JQHMt{Edp80m9UnGppAv7>wg7&k5@Wwd_- zRC|mBWFU5o33!dGNEvGaiN-+6j5C2`!$-DLeAJw3)Dt+>W#%WK+4ifX@iKGVuN0n| zxRklFk>esWj%DY$NY*H3~p zI9~ems^OG+uhrvBOPDLoYjvvf8tb41l)eBGjrY}i<7LjEdoA24_?aSo*%rjk+$eqK z!t`35iK7R{cR^ZlQh3=FV-w@N!gUL(Ic~4&IRTqBg=)qGnTk#ATPDc#yp@p2^UgFX z@oQA0blyvKj!~L%N4g2zWo9o`eKoy>$)g~rTNvTY(fUa$HLKw6T@t9 ziPr5XM)!*W?uApEY5L`v^no+QIJ@2#2a?G7HcG7!qLCnqD_TECMzTBbz!BwlljNvz zb3hWCQ3|ou{vZliPz!ZL>aPGv&!phgFvh+e7dV%)cPR~Jl{)y^bXQY7W6wUcG9 z<{ALELvQf`92?m6HK-^Udo-}W083+EjylNC1aLo;2rrtV*ri{MwWC>1BK2e-JIhIo zGXP9yIf-%Q0n=GdVxss^XPqwlj(Z$2xr~~3+YPR>SW+TwE2%kBQeqrys5w$nVvKdv z94RR=?#H{UgQvrS+X0B})Vs%iHTAM)h}1*W8>ty$oXPYrA~i#ds}h*b91|0@y}LSN z3U)G|18@|o?u1U5$gY1yvvq$3Xxb7eQF)R<^NWD~6lB3IH1WMXfOct?l1QCrvYn+Q z#@@)>L`q4Fvx&K}N{RWOC>I|nhh}?B>VX!dFcz(P41)P*)>#`^L<`eAXupeT(gH;4 zYnW!l0>n6XFwKYsh;fCOW~6Nr6U7Jj+Rf#dnPwBxJ1>%E$#tO91kWHXm_c`I@9rIqzXhsRTz=_HVlygOXlxZ!T%6rGL4lSl* z+82&#v*nr6RMipBl+&`-|4U?l7|P9!4AJ@y7y^F+)dCY=gD`;o)M5712WlsQ&uo8I zw;lL7Ou!I7#uylJRpUn+b^Txn25*iq>V5>nkIi^^3jyUI!Vud}xav%omgC%+($H>V zM0)~a;X|CukUSmSNkH-MCAii9*Fzlnk3*aS^AyKCxj~~l!EcpgeAhrXPK6fo2Y#zs zQm1!mrbDWiCe8%X&LbDcqj)%NF}8bzi#9eijF7fvBK}qYnn8chwp_$71{5&1A?LPx zEn185BcT-*ZKdl|Lakvr;o?R+j&m$Q;TlKk{UUxG)wT#1ch_wXgk^=Rf{CsS3riP| zC2S9h_?1jmx`#JtwueOgE6kDZ;#RtCm5ArUBBZ-6RM$c%YWuFON=HyiinQhD@%WD>tXFN%0 zn*|jaUlMxVf=UfH<=(KMo(3;N*xt0DO2ZzdUOZd&E}Te-cSZapUPur;->#jsJQqY5dn?N!JO-lCBeuC0!>ROS=AfEa?g#OLEcX4xdE!fmx%5WN<-` z)5lYhK8_zV0Y#1<8Ao<`{HPD`6_d|gsp&(E31s6+O%M7Rw~in6p(bF%G)-Bo)Q+9| z|8)9z{(1jTr;qE;lRSOgd*1(i`Z)c3nc5sVeS8rm;prnPI3`h~zYB1Oh!2j!sZ|&2 ztWcA2|NkFKb~jHn|L0Rlu5NiMsfwE9#m!taC2-F>+&2n$zhV{C$RX1hl`;un7Hq zhV=^s|31Sa^!FJSq0Td`G(y?Y=S;m=^FAcfZ$b)rn(d@BEug$Ry+>?n*#+|P$l2B| z*dn!%qyuT+G9EtLYJ$a5x2p4}TW4FZPB+iC_Dw(GY>T3uXIqr*Jlmo$&bHQGDChU+ z#{uUuVU9QflufkWgSjjVJ6c4i#6j)~u*^otPE)7NkiYiG!q9+Ma!wH0h}qH-nIilY zc@ylfSt4^UlD`HIB94om&R>wiuzXxeG*gmLWQp|>*%a#nL>!UkNlkB&$y(|I1kZG` zf@`qZkKaNDX-eqpz4+lB$&vh8?yhn8*aB8Rqh!!dUt zShjVDEZVjRh&&?8;Y$doD5BVCAVv^rTG|kKwDe3MW*}s9!nqJ!J?6p^lcP0NbE!;m zma=4}`RdF|Vff0h;cJLoNrNSY4Nr`|1{l-ujF^Te+D)jlMLY>JI zM7g9v z(;PEf_MX_pYJb~d1Bri`2%_~Gc&-(uqPGK7G#mm$5G+OSq*D=%eLitPFo<_E*lTVB zdGcd;5BgOS*cds#X{U=r=mg4JuRNNJ%NI=L=+hklOSBYkh7^2Cs8`TpqC2|rX z5>4j#@FTeQvFzNP5lv+rak;K=A($=f4U8zg*P(MzhrJY#n4v!vj|yX-Bhpz3WM@kt z##IXpTY}TZ5^up@GlTeRAc<-TTx4(>+Nh=$%RKz*F2ez`&hB!1)OqDkSm zbS|0{en-resr+pU<8Q`q#!wot(ygZEuw4C3Vk2Jbt<7_6Y0K^}7rdK@YlyxYLV z>jezn*8%X{ox!^m01x#UysrS@6$S?HTToU4VFuY@3~E6E89d(!qj}I5U}5mS3>kVJ z4BjmOcq-1|eMuNv97wLp*op0EAzA*unR|^hTVe+|Tv<^7z)&YmzI@HkVGP_14-(+esFY@WU$mbg4GY=%J zwV~ZQ({R|JW1AXrHD(8%V7QACu7aG-1*5w(@ixZk2S-_cuuebNGbH0r;0^PrBKjI5 z4)x*MibwQ3AL9_A8*CfkO9Nz>Zm?~LF@$V%gKa@SH7Pg@#VAI^qHVW&jAR2R} zd3ncedlhVM+eSR?)+Hr9yq|qJSdGYj_D=v@;r;A`;MH8={p?D_4cDdcQm~(W2Dm(~ z@P0M{uPeNtO(4;gAKuU=ly+&r{hWE3lgF_GRXEPps z42@7Mk>=_N?@sKxMh+KyRoXn+Kc5q#AB`Y>4+gIRXhjKw(JaIYX)eU>K-0JpA<W3%EKm=0Q|EC7pD4{pW_UdIPW$ z16K+o{?;(sO^5@Rp95!Ikt(}ZW;TzYngvwzjC95$d;U8?C01WYHJD5NkAyMy&D4Mq zvEzKz#r*quQcoY}=5JH4oqiuh>7BZdo7*W>4&UThqL3S*>aI3xr621 zgp!?G5k}c|v#_)oBc{#B8AXUTGfPZNbS^5YS@y*i{L9U9eGGSk#1QG^l|H;ZGp_{> z$(x1h!4{eB{0g}dUlpo1TV#&hTd4liBD==)VeO|{0!@V~e!lF5rZQ~4?Co4l{?&!* z#`!W|K2fOF&4+usj>x&RNVasEbxe%T%}X=CjF|Z)+D)i4zr=Xf6{@J~;GXhTAGr>b zsz{gme)K}KqKUQb2Da0(#5gpnK}DNQ1-ZQ7IygDk{QjM3D_NZ`vLN#XZq?*bT zD~){iT3|Z612IlMPU@Y*y5f`_fq$tM+#o!KEK>S%%b{w5iy%SFO+5=*rfO^m}9F}6mQh=yUr>ViN= z5$bG2WQ@*5ZQP75n4#w1j4t>DE3{OYT@Ww3)h@uE^MLY>JIY9*zT*&Hi`p2R$}a)`AL z1Gdu|#5m^y)7hhtOLYqtd^O++HngOe5AU!Q!X+95#s^*_$_G1 z1UkFWSt@^_oE-fpO|i$c1YZAJyioRW-p({`FHj#Wl-a(e0Px)$R=56}XuymjO3@Sw z&r?yi%IU6g%*uKP-XOg8R+)FY+4I&{7lC66I^iUQ9xR90VD`TGfaf6;o4s#Cr1O=q z^NmD8au0L!te(^zh=;(q62U**q$+&LDUGix2JjnQOrekz3dNX}#317M%qmouW3I&&E>xnvNoC-SKfCg-VGOah=YP_JBsP|q% z8M@WwM-ek`>ahAven3?we|e{`JrA@vn&mb`wDfg&FxHJOWOJ0}B)xa1v)#MX+3r0Z z45qVP-sx<=2i{&MIoo~CN4{(yCh8o~D!EMfu-Nq2`bL~={g|uoJeZY1KW_9bfm#N= z!>DTB3F)^GFyK!>F>Tt{m^j03@Gc0@4X%V{{;e!Sxx=rrgzxwKAKYLhR10Ce+wxz% zV7-|a(D8B$&)H6NiNn`??ACFK=@R4JmWRo8l1r@D0fqNjPQ#CR%||~63h`3QY#{U# zuKAcwGG1z-S8e0*lj$kD%1hFA5GccFi;v4mXE$l zl`oO!H=lgL#2dL_!mBOx@?my|gkP`of{A!D&5yWs!9;v!j0Rv{FcIMkCULl6B0l#K zZ(T4EkqajJRLZ~aoeYx8)`+M3LOh~2CEqE#Th~q^Zu))jMLVyZB)}m5?X?rrO^;kV zc^axZubmL+ymmsM^V$i4&TA*s{LgDAaMRzq>+fqPOOTU@o6dMb=e3hdAe?AkJ4rl^ z1xvK9o#^3fC!;~u!`Du3M!d^=s(R_)*G^)P51SFbc0yI1*G_sQnpx+Klf`DvQq}Cc|GsgeqvV}8PSOB$-Z&Wuz`Swt+TC(gj)B+jQ$}$T zoFJV4fl{#o$)wJnhS6(+=tcn7PkP=X1LEpUxPk?lC=7@p9QUH5uMsuac@3t(+kUGwGczppZCiAF0`3^z87ug>4q{R_Zm$Hf639dy5rL#@kAmUyJ{4zrEMhsG~z^NCn6sZNR*l5{Pq#kL-JM+I6sZU#F zLB_2Vy4ewV%UrZxgt4q_GenX(n3vb8jQjAyUy15>A3iparl#H})1w82CYf_jW06{P zpB!RtO3Tx-uSKuCC4dzlLeY6e>hzT|kcf%+6pY*xDK*ZNnp31w?w3Q*9H!nc6XaDz z>hk+#_vrVLwj341s##E^9=~4(EWM+OsQ1PUO`DHUbbFE7bHDUOr&8fClfAV_IUbPZ z7W=S1WY;sXFG==?iqz-_zOkD*0~*Y(7PB4vM2W(LeFwc#rV-)3przYuac1#)THcS?~O`k>4>pB|c6i`n)I zU{lYXjXMu(^-2y;ChtLDvA74wn2%FUg61G^H8;EP=0u`i%~@G+R<7o(EI2Dyapu*z z%f*}JXlWllDARjzg4Ma3Rm}<3yg*yc3D&%)P{j$>yg)k#B5AXg{~=k^eBy^h?_e2Jx>V(gH7s35VDxg8aN41`)nNw%spj!AhSqcl0sQdK1SctjCK#4bMj4-M? zl{7?@yv%$|6257e20W+2;bX=d@#P@Mt%VL-)V0DmhjOEs_)Q}Fb(9(Se=&h`{g;sO zEhwptx?dR2k#Q9n9}>Rt$cg5jtCl~EJI4XAM?EEcv<^-+&x3?4Pks&j_FtKdtTes` zU>=vtzJA7OOe!?b*5v@-FJv}173C14EAneG2mOp&XX|pP3Gi(FTT1?k(%`Eq`T9hN zo{4;U1i$O~B?{g_bQR`95O$(8I~nRz=Z^;>{MreM--jX56)H)51sO zxk4;Zp+{vw^K2G!fsdJvm<~uojk|4!{$IzP=eyy4$=K-dFeuX>Rjg zc$Ed&+!<=q8kr%ssn6HQbn^pcNz2u-HPY|qfe-c;9QbZY#$))N$FyQK{4rV9Z4SOa zax+Gm`RF6vFh|eCE@%sIsTdTr%3pVA#sZ1TZ_ zl%n5ERLl$@rXdu31wpJ)HH>~*;pKC9Rpm33R~+cr<*6(X|<|lmnos z7uQPn^dCUVhlRchXK=}sZelFv_p#l0@AeD|rc<2R3Wa&*U{U=iaJ?YMLM@qs97>dw zOjw5sq>M1`)ndi~#`5hbe;1klH zNV~Q{vgcITe;LJ=^-v!^A*-unSOd#V<{IDwELrQBn1?FnU?N?fK`o-eJmk3;*q)1^ zpq2`1d#E8#%Ib{CYy~ZjBH)lNgr6kaUa%1*i;#mSrO&mOda9$R^ibQLl)bB`fTr)j zics{|2g87s-624veK(N2$XQt(#yIx?!;74iLrje7`IOA4z6rd7ub_z2^_5hR1|pI6 zJRo^1v$8CVaW(+MTbZVUn|i3lP~cn$Ub(P`+WeGEji(@+Tew+ieRrxKo|0MTEkScO z-=?YVytG*J7`t?WOk}gK?!3WR-FbuYzw-?Gb$SM@fc}vSj2u<80gKcnPs;-Fjk=dV z1JxV+nWsWe!?pA)&pOtK2Q8?l=Ss>wWI>f4^Y-2HF!>^;A0kas^>P+9=D)LT4DnWyjIs2 z(B`jzGqx%ID-<_s+ot{-Egxz1bAS6tY#d2pABnY{6!wu=Wm*^dNZ+90Cxv~aY*c7c z*hezsVIS!j@nIi{{HSww*2IX9bO^YpP9LcNeInfx@sS8=o`{b`$mYpV7j80rq}w*h zz{JNPfk`CTTs$-M4lx14 zY?j{PUDzx=u9uZ$yk0O5SFqH?C;#upq9L$L6lkerEDJTRwu%D=g^age9 zX1TP!JDFDr$1$)7KC;ij_ke)OCSOhiCNKbH1hUV)cp8CP0vNc>&wZq>K@#||vIC!! zJL7qo+k6y$hTsFbG;xz~T>=#u_oMd_sq=yT5=}0b@p(@JdqzIKO@IIg>O%M{4tym} z>`8*?&BD0@q6I&Nv33np4(@L21w3dWpECfA#3YXFi(zyWoZVKxxB2~wDr}g*_7Iupn z!u>m-?~2u}FUVZycPOv~u=xcUFabM&f4v~nGY*0x_+V2LGllB_GGkhbNc~{3N`Dcn zxDeUa(IRt%v%rla5uTN`7pDIgLAX!zaopFa5hu>v7SZ=N941BqIEo6 zoML8=r-53@b!6CYHUxm$Bxb7hL*8dCy;5y`QJy8vSKVI12iSV3UN6aj>uI)}xAk61 zo%xa+(EJ0MsF>zh34OJuXkXa6ycW5<3+_o}3|rrWx&KCUw(GUn7wr2nIC~=HD!#iz z3|yb*AoCjVdNRv}ga-3TO zTrJWmdIFJAh}1lX$Z3d_KrvFs7bdF2Si8>8Bv;|I#%R%ssQ6s4X#M&yaV?V1nyl<4e)CbmMG%DS9BccWm6=QP5nMroP^S`s4PF&AhA>4~y9*fjlS+ zww7^vI(w2{dn@qLuHzEZD#rfcb*95Gr~H~Rx`UE$TG!GYlzh|Ly!IqJuRTd#dm6L@ zIg?7JuoLTpzo({~0W}k>gTbr}CO2|0=nUQi(e+#_FhqR=VIC1N#EmdU zZUX~@cW+GnRZzqr&%ir1L$K3f(>}(|q2<#yd)hJ4)2Y?4Kf}M%hlBM5r#5P5*Z-4K z>-pb&T95S#!iheuXH2tiDbhUAsr6W|Ae`vbMkgnn3NF*BjkaDpFrC_H>$L;Zla03C zIMC^_#uS)4Ql`fmV|^1%r^gyoYtS<) z1zS}r?p2w5Djn0U%)kzk1d5Uzfic_FrR9&$B_w+GcWLo2dklxmV?a|2mho-jmg&SACpfW&<;2FAo|d=CbX~C}k0c z_*e{cH_CO3S-a+Llxv~S+%@ysT(9ZePBeF;TyOg%Y-H|6xpr7_dRnf}{C^TpPs{a< z-`svQZ07@I?nb$=8+9XeSi4a!>_)u|sM*|&a$z@$khL4-!fq5H>_%bEc2x`Cbd-*z zWHT{rpW>Oo4Mr?7d2AW`As&h6G-<9+L?YXV>4w>Rdf61aB-{JC>d`Blbi?fZlFXe+ zC*3gnz@+-qL9sc_yIVt&$Yz*Zd-ma7XjzZ*Wi%lBs4n}Eg4Z#1*7=%!Y!XX}|4(MS zoHl=*4CD0Qn2)iLPbz_&xB=gSDpAF6;*H>g@FV2G5;gNp>?HgOK>S*wmc5BfTmQ2_y6D0~C^T!5f>W1y|u$ZK-d0W=v_-f1BaumKu81;?`jeG}h551t~y(3F-6!qjg zxb2XpK7B`y5I?DmcjX{J=e{e)i;e2Vcjb@4ogD9@vD$G?tleKO^yXi(x8>Aj2 zg)rV7MCis|0AxQR17%noA@sOy(@*coyM&mmT0W3TNw?9nz#$oV63bCr2tDdHJ@J7Y zD<+R2aglT^Md6ao#A;D(-AtK9vKx)@F8JR-2wZ_Fk9^qOh0yI%Uyo5KYf~6&YXlbE zN6&iI#Hfio;Q!qZe%t*e>e?MR8C+eWT6aj_=+#uxLM4l(p3O+slaL|WMM(03CAAO} z#Rr;?ro+MYeF$t_1$NI6%)g{1O5Z8B1<%EdCMHN18z=vY5JYQxJPLD*Mam@U7RX(X z&|@W$6J->UF9NX<0q0YmkK`Nf4``G>gkx@r`tl=LY@WW(On|F;Ly1b7^X%%8^+kvH3%<>Qep>TQNwVLI*s3+b)K)_$dogl_S z0pDWAaUnc|f2-Gb;W#%<{kBWKB-W^PAEPN6U8d^$H%C}v`s@lk+C9%9s*uf zj^cbZsYFftR8~p+uKZLcCi3If_Wc+&I;#kj?Vrj(&l#WyR=&VF`Q_lqCScYGNd}?L z8X?9#qeP{CCR4jk1Moelw?ZhA;T#ahe1=bn^itD5lcU9*>iN&)&(Ymzu}96E;d9&1 z?v_2x?bLaw?c(e3sDUb5O^G_XTMp|@Cq_&%nZ+g$`*0U^QYMR4$+1dID(6^L$?Xa; zsn)FWzSGrJpUVo^>8a20SvP*;$Ifs3l%T;>KMkKAP%)b`DtSLRnjb|a+Y^M3TJ?UH zLzQEWIltD5YlwQTX``R4#cwElE#B^}%TuRz#UfdkGOF9cmVGI2RL^zag~-?bn+ zXIa|%U(E#Qy7r$AB|2SKyo+6h(Un*Bd3nct7aLIdn`{FE{=gt$#D!IMMGvoLcLb({ zUAVeIZT&*}6LN_=D$-aceK5Hi)K4IYmz48M9Q>E7vM+Io??ZL!mpGG1E>YJ2JB~Zm z46c8P=)rFwXAIa^b`9n!0)y*Y+~entIfLup0A?b>;QB!rL+X$$gR@`sVAG9Nhrg8F ztZxxbL_45YxeT5r|FR~*UjO^yrmYsd|Lu#dC!THMW!B5UcjDP5-ep~2qMXeO`Fq0W zh9WPp@^BL`tzH1KdANy}R7@gD0#4F4qiNbK&9g`)ek~Kt(@xKcr=6a^Pdhywz8)}5__Qmfwvew?YtUG zqC}o{dQLd)WM^#l_aRbzY!_JG4s^)$~{pV}Eq*{oq zN1dMMhAUwCn;!U^y#QJ4@HZ`X`kNm3n`fbM;gh}GFO6yp45XOpZ+hTwo=Gw8mwMoD zZe=RgsbU-0N+6}oBO5FLyUM7!eYjLDFMr2j2>zNbTz-B zDECXFM`vb(!E!aD#~Y`0icat|9+sSF0+y>8{r6dCGC0h$PTL7*owmqXC)97c>0k0} z9(Nwt6Lv5?3w+EsUaNsUZt*cAUT)=i7W$}7i?w-P)6a)-z1+(4SjU}SZsmEb<4!Lf zOpkTk>7|3|`NmIvl#1;$j$3&iIG9%fXE~T2IGE1>vW`1Fa4-p34yFeVCLtbouKTyl zD5e)_dX)4cJ@6t~>s`296-R`_z`qcf0a?-d3Krc-Zq)%9Tq(}ji^(Zg_1?=1NU9M( zl}TU~`_sUiE2Gw-SEjb3jS;K!VQ%AG=}z+_z8B@k%9Jluq?V~y_sSAyHl?%6)Zx9d z*qKKluS^wvixpHQ0X`@rYNYE-_(_2c(5-dLfQA~<`Ctn8muAlEKu@a$;%6q4@&h;t z$z%*#Vvw|d3wp+2(1>*efVE=_E@M&{W8?wjz!qHQ3}Vg!lbf7W?|_GreihJ7eR0_Y zfqxkxq%aH|qLwKPk@k$yU?E0tq;?$BWt<(x*qf*w$8@H4@;HZ;sgmzxM)w8)k`F2s z!=-a7V&Q`F4%TGSCzq)!zLUji1nezLZwj)M2=XV$&Q64^>1FD*?{GF2Qvdl*mc+O* zX5+g@epX;|hAQ~ptg=(T$J7z4uJ~Rqbn|2kJ8=&Dn~JC(WMA z)U!auU5HlOzZ!N&NaiX9$P^+c0x=dLg;NQ1&Hh1lcixYGi6QPURS*7vcUoU4Rj>ab zdrW$d$Po4{u=Q$}P@eTMFjjSa4&iS|H`tV(5HXdYn@x#WosURU+Kc!JO=)kb8op1K z$o5io)jrw1OFLN`sO1fz2WX-!TbX)ppG^z%9052XummuH( z5CSE>2|zo<@t47uRhmkb|DYTwXO^ln4&tym8+5UfXnA!Svfez=ouRS`*Le1}7x&sk({>WH!Y7om^w_l5jUFeRRM!U^Yiy1GqnyH1{eusBxOqR}v zLQ2L|3dtxoXSF5b^{qFs<2J;O+Xi;rhS+gCiY-)qkyNPt0;%Zpvh)G4SI4%dK5s;v zuYP6p0~DQ^rPmH2(we2SW|TUu#|f1NcGrg4UE9F!+AzCo8`$O*t5h4<<`om?#k@YT zN|hD6X!3HE>I0A~jcm*sXv{KibQb-waSp_QHG+wZ<3J3EDI&$j+u@7xSJ^tz`G}7l zFF7@okDY{zH$GBjMvwO!topdr@y|ewppP}j{}D+N^hR_1Ztw{D7*pZ4at4|zLlDSg zBtAfKV6%27>Qnx`)rHIrjO?RtpaJaOyJmVjWuo~z4tb%^!D|zn3?U|3#9}I5Vi(So zAAzYqhD5ZT#peP?XX$wt#+Oj>W{{!x3LrmA&0R@7etR3mX(tokgKr0;RN`+TzELCb zA&6kqr(Q*!yQtI9Zvyc;JmE%TY%1}Q)Sn>nSExTt;)S$-W>yD2_!a$4D$PsZN_;%= zB_aVVg8#Wb373^}GOge3(9M*O-Hv}?64761Y)c9gSM1M7+#4!^u7Pqy>OQ?4lw;_h z)0>xNs*|7`ie>VnT_1-V8m`P>V?R}c{q&S!=59o>9{`b3Yjp!x>Ma zc2+mK&EPde=Dh5CXv{z&=H=-J5hu;~62IP#k^PA4ufZYn#*72hd>hwi{xk#gVHEuy zTKHMUi{y`>g`a2aA>Kfpdo%VDuVCBy-v?%y)o9*~r^0>#zFXrhsw?(e@X?we z2*a*o+$KSoHs7n(L_s+AcKSU@5b5?cBqj?Y%XVmBiXdY4M0#?(APVg#DVa(lu9PSl zV_@9GS-IwGbYdo*XpIn8>`D@C7Ks&^<%~8x!mZ8}HS>JBbef>h%Wht+Vt5g179WBB zO2)HrWwmO}F^Aw~FLUVZ4C0D?D}9+8=4E24;FGj7&l(DmX^(LmKG7{Y7jX^ToCfex&v9@0M5|3Zn|e=Je2g61cK`4RR+avpE6xD5s%^BO)i$yIU%Iw2*?+>X9L6qCCx|N@5Bhqphbx(9Pcun@h z?hihN?N-`;SJ-Z`C*w71y%*tQ*lI`V~LPWu89e@){O@0s>A z_qk8Cy7=$GD>tljG?UK6>Zo$ACZEpOA||{*v%iMOhvoq2iwW8Ri%)qo+UCVl4kkY&5Br zS_UuKh&3?E^;&utE*bZYvKzEq2bWkD+e_JvT0h>m=GamuwMwhL5L9byFExLHbM}yM zXY4A zv0M3tR@8qwtg17t)!>D2FHw`6fhg*;p{szesB`Ciq1ATSa97(eQURT#oFAx;X8Oq|@Vgz(w?7^I55CXTBOT`x()zM0ewk}@MW_Xurw zq3J{zZIvX_?a9a+bR!5+mi+-^-6W});_Soi2;HpeOy61`GBj%vijy!9*1UA5;VMEn zp6W4NKb9Eh%pNdq> z=G?mw&ZlB#6x!VUJcP~9DJ!v8(F)F|B2{K{wp|FDpFe{p1D+G^ejdX4R20_Q-2FU+ z^QlPH+cl)H`8lZuo3rgg*!-MHG}_$!JcRS9C~LC0`FRNEQ<0ita|g{3Ha~v`RI{DU z5;jAypx@>Nf9>O;1q~_Y( zX)}av(wW43yOkG#L#rrT7~!E+p%O_gvAK782!~ctbD7P((?dA4ikg@i!udxboLWWl zTKmUT#i>=i!j*O{{l=+Pl&!M4d3p$&r;}Q3&!G&5R*_n(I6Sl}ghQ)lg6(aFnQE+q zOqbKxQhp13F_c6ZO;N&U15b-@lUdmy0|SNfen-ZCw6nlO*(O8Uh# zk0ZDI(?Hek_x#pht&9#)po5w$hrIh+;BoozDT1ac`5CF>g63h7~Lh|@*ERJ-Tn zMrB_tMFW`hkcntj0&?cWFP_H;BdBsNII{uSe`O^b(kD{&PB3l-zs6l|KT{K#! zg^F1Emw|W$zN;CXsHdj=-3J1Rv_9yKprLe)4}Nibrt~5Fbi1{GSYz1tBeL9ED{+@e1v9&!)W6h z7sBj-Qnv`tznZIwa;$QLtbwAG&xb_aO7$eSU3iXLXoSTOvxau_|DVPSYii{ zWLf=zrVbgjv`$e+Yq8%_Vd-#1og{G-%URT!g9Sr~;2(;sc0Y;7Ii9^x$_CNyRk)nR zfbzu`B8A)3R6f6Z!FU_cpR3WkQ36%|z$Lt9)lhMNaCd+qenj@)t)9RGbYgXoR2`43 z11K5mp(C<}u|;NW2NEZ%g7GH+xgOEvp*%{(3*dF(<$Ramwb@IQ;bo$NC=DwayC7ut zR_y`8>p(0yd|jQ(=j;S|ttzL;4ir~-f?Cci94a#SL^Vwie=UeZ`6Njp0~NJh6${1C zYMDqolT=Yv_$Lml-PO;MjRl#EuhM5}{y4RY9HqL71*gE1(?uySkn$lzYZ<>xrZVU} zC9V!qqs(Q@mAs9jFT-kW3vbDrscLNzk=?0k-Cv13nYt3KrYxw+;g8{bp2*VpV~~&0 zlMxLxvpq}aV^zO&_5u)t>DzJQ(ziFT&qEn`8MU^?;*{uogrs0JSP_oPv%G)#r2b^#`1j=q}pPdA*e z2Vd>}DWZ*G(bxi(_(96diHv)|kPpvj=Ba;@wA#I!QOMfEDEt}BC|`y%-cCm0ZSoJ9 z#-v<)e8t+pnu>B6ACI50R7gWf5l>{SQ3FI<>cw!eYGJfK+ zM;#mahnO6l-2k0#fHL7r#99u;FbGa%&i@LGUqkfYfUzuJl&bs@4}QCvBdY8_Li|4f zQDq+n@-+Ze025_aqqWoz*+Ynj@m{+ZW!F$%8H!)o244+{7}p;_WkosT{Qsy8j%G*T z>|AdwKiSrX`BU>2!koA3I*-VWE5>dlP0W}?&rOwLYH=RX1 z(gew+P`Q|D-}BUrR;rzx#bw7<>i`t1$O4G&gpLcaFZJ}oLT_BQeVxZJI}s7XHBKWE zyMnb1{!&{YU5G8696fVzJp|W7&~F})bwJWF^P9b@in9cq|6-h!Yuq6by)6DX(s~pS zUBouk{4{3GyP)3!h)ZHLb#)1T=4ZWxoUn+COVXRM5}sou>mWWH*N;Tr*iiSh2 z8X&HSYGI)!uPB8K9AfFd9MNOyZg+*FXSs(XdcO8We>dOO>zh90EjuYnzpz#ju`eZ~ zC7X$$OAvKd*4c<_5f{1hI#gbv8l66! zdPXz#PR=@sdc;Nc9D>e4fXE)Ig@u}T!^tpPq01Y707S1FK40Wl{6p}?{m$=mzw`Vd z5OZ2;K9_6|tWwp<=Ta7eHBdDRI+0}rg?rB$$_2ylE1!QGDf9VnD1{V%t1pBn-ugrN zDULJ7;^4iUI{{*E+#)f+Aje4^hi z)8Xwmt{RBjd2WgrNic@Ex2NJxpB;vvEQ`fmoG@loXRajfeXrjMJp2#*g!p4z0}Dn& zCSSgymN6Q$_!%W;356=f{BfMl0PQM?b6$C9IKHm4`=XJp^9FI2pgTA(r%;!(-2ZlFI^dCer&L#7aMN zbUtyaKZa0Db1^66eM;r+_%Y4n%>q7eoDgbG%_lC`_L(OLf~x}m!kC=pQfr=4Iusgm zS)e(?kC*jvZuK=k|aT2(NUIC^sGult7fDqjR6m~_~EnuNrfRsEv%|f>T zli=xT1yNm|o@Sv3fJ}LMnuQ(!Mv$kcS?B;DB~MSY&;h`dLoip`VdOGb)2NKqCKQWN-7rp8#PBssxUgdRwBq;#8d?g{@Je4Ak+G>WEIEGgBqi8T~$~6C^b=dKsx^Np(dx zkUCLPbEEf@I!RLVqmPrCCaDF{*O*?5q!vaq2B226PL|}7=mqrj6iF?MUQg;&NiC1w zOR7~;E26tewMpvQXcdjNOKN5GB<96*Nv(?hkW`1HR!46jHA7Nsqi!kITUe(_a((n6 z3Qw0*x01e^X>@A(P87F{r+fBi>S8C2_H145q>-{Rk99>BrvU95DL3Q| z&=mBO7%5NbTsxJ;sbwj@%bQ3IPAyB>EpbjQOL-%I8~L1Cmhw(M^--1F1(r9Gn$~51 zrdm}IG&Cl+*D3brYTj4wSJ`@IzkkDD`wNhWVn(~&q8z=vc+dba|1FuZXcei$k}8bu zD|OG#(c9;)L4MgGO+=j1ZwmT4u~mSenxiXlyX-7Uwe&j`qHIaDM(b!SM^boTN#$yJ zD{-xz*u`G1Txqir8CV))Iy49TY$e5s}#AHI>F1Thv#CGRstQcwWUg97#=d||0eQ=g;6dV)mlC4y3|2tP5G2|}}maxmdiLFm^1a-`xiK^WGH?muFB zdF&RZvofb1MrAq$drQE1%pI18gO(ms`}1vx8Sh16&D#TNxcF?!FI2;u5T!pR|6Hh6 z5G@f~YF?<07(t&<7Qq`(%C8s9X9}ykJni$;nxyu5DtdtMS1^-abjhW{;%l~>t2PJ5 zGx$;6rIzwciTo4M6$mI>h(x>8(aiS=pvM9R{T*L}=(|a81al4;U4X$0XJei4*{ZnU zOToAppbU8e8LYObkus?AuM?nl-^=twaxLmWCbtF3@lE)Vwoj3^p9kYvfV6$8wEZ3! zZ;~mR>8)xSP3Pm)lm<}B58{5S1Ca*>H?paW%TnY_G6{S(TWl2>bgj^|EjU_L&)!1O}mL=8blc__;jRO&FDnnjjTB@2& zvj@=hM0*mK$!L|ZrmJZz=d|`Uggr;KR&&+~%&UFjEl_C@@&6l&!_J$#3|!IXS_@}y z0t|!AeZwhz8r0+D;GCs><5)mH0rhtP3I!exrRM1p7=0tWqTbz5fL2%DG*o&3ns}pn zLIkgTP?N}ki8n^k#3i5>1C-iN8F_nz&hxK~rL(&p4Y#_9%;u-kqTt*C)%Ae7?c_Zz zc#nX72rzuf1&aDi=2JGWzN_x@cnMT?irn}J;&%WeXpeu|HpYK0Gyrt{$@u9;F5CgA zp9tb%Ain|}N3D^a;Jgd!Ex^dRAie?eZ@?(dKI5B*yGo3_7(~v=cr^eTToB>`&<`-7 z&Hd}|0jCk7dH|wc4Kf|iMk>OQ*WhP4_{&aq|5B(|rG1ZaR^xzY@W*U-ON#WInK$x= ztr1t#xP}katpXoMLyeD;u{neFmNLFkqw4*V!mVM3OxRKa-qooMxC!dRh%)w4ki{ai zYfp056zKy`{Dvy$pkas12dLfs@$zbFq%Y@`GV?dU#XA5K#h2f?BJ+2Gehx5{(f)=A zN{!XcPgm^P=5~q*Z3BzndMF^)&~{~{X$W$p#LCa7oI}> z1*f3R=dLP7i7LTEpwVFWUL-a3*ACFusXG|7DJS?JRLx(YZX)zU?@k-^4dqqb4DaKr z8y6=AIFV=kdwMB~!*>sGvU7!7Sm|;fnWOZsV7l2t*bAI#2TFVZ;Y7Q+z(ZH!C(NJT zJtipzd1jVcYaIY$O>0-w;o{R9Egaw@1a4UE@fJ|^f-)_PaH+=$+=)bRP3lNNU~{&9 zq(VuL{)>^f&EP+(7_p29!W|Y>A#X`Av!!2Q2MLSXx|3{5V7y29SmpkEWq>K+pCK zHM?$!-ar0#8u~$)Zm$*mi4BUjjjKNmv-1+ZR6&b>I37iR$xa8^ACf0 zPTzd|UTj_q&Qenwf}^F($;g4_=8526Y5o$Nxb?^&7_zGHt6CbaRd%&!kS~vii4GExkw<*DgfTkQh}#ikK;FL zZAY{Ws|2ct?eJHk2y{z&V0_#I_s zz-EK>EPh8@X+uzQ##k1V##)>3+i3j-O5?0|VRXE85jYd9dYEgn7J@U;`ZJI=*(-FvUfF@-t zZ2tgF$~HN#Q-%XHMLbpPyMrzo8Bulo12lg(h*4FC!~fX3LWno1j(>orh^#q_-Ty7q z3)B6hGIawFmhSw(wFc?QqcXXDVum;>vmM54|ESF0!)?WWkKW*@%;!KDwtrM6DbxP5 z`|Ck^@~BKQ%WVIsOj6}GcY8B%R3@ov`z=y9Dw9;L?H`p%s@~>u2OgD4s=>xlnUePzmRA^4Kv&P@h(ZlX2C{yioM z8Q8grlwot{CIdS+kuq)W++<+qCQ^>gotq5o+(atf=FUw9c5Wh-Wpn2y13Nd7irI51 z!_H02rb3%LHyPNuiLw$~?A&Bv=O&V6Hg|3^uyYfsa_Kg9Zh8f7SKHjV$-vG{l-1g& zlEThSr0VTU84o)*k!lcgNet}V#CVN1cWyGUa}#Av_6)|u&P}AI*w#ld*tv;Rhy4@EuyYfsP6b^$H<#-WSW7vZSgQ}90X}1mnRV^vUz7o%nF*GD{DBZ^Kp_s-ksVpVB77vXvETlK4pzx&| zw}h;Zf!QUrcdN9Z*h^`zeyyL_QVMVL6MKjT*LjI4(G=>sIB^?hvZ~$A3jNfg9Psp6 z-t?i&g=Z^zmp6OJ-e2xMQ=xwl?*~8711Kh!blJ<1g1$sjbCqnK->cT+V`HR0mhv!4 zV9vsUs1$+ZE{A=!LCqug2E16W1LXb_eyO9?H1@OJ55_%!SQcJ*Ik2Mh2UfZM#K}7| z6}4zSI4XYxd$yXo-vW~xD7}c7F94K0?zUS^J^l#2MQSJusTQ;9rSx2zO@yw{OA7>2|aeIP!p| z!?Sq!ORs@d^0acM^KX41n0Yizroo>ul=Y7^-GM|H9|*$(XgtP~C!S(t@k70jv1L%+ zJFxi-*eZXJtfJz>?JP#$56PHT%nQp84t&F=H zk~lQ402Dk8;x-_+0^5)?tS3cHVUhYm&4 z`+vcH1272ZBP!iA#UEknii{jY5M{$|L%RT=l(KBx2MzT=DeOES8VwauEJ`6RrpjR6 z!3Sx{WX}FRByJ!XMtblDk`o>$ei18i2aku1$+UyNA-_lI*8hT)FhjUHM(667AzU4! zb9Ky+%U!ib&y3$&i82*S$>#cU)8sz%7F%iyKm}Hb0HC5pvD-txlSL2ot0PA>BB^?aTvYEH$ZOM-R={0UaxT&y~bq5 zZ1x(5(Q8bq&_=JZFNeaEjzW~*#IP5hJqu2OlkluWP)M4&#hwHi)xH^$oycbsq1rtU zH#aRC8BCVOA3S!Lx)o7U|5fdljKJu2kvn>XJ|fPgXZSSTk%-Dp(dd7(6fG{GQMI)n z7uWs@7BgOjjG7_-2#c5KneT&nJBo!1KxyH+HguP4g+_$YyXzoN`v-o=()c5MJXcrJ zcjw|3tEY#zfSE3ouZEsX{q&+h>TAHx_yRxF4DpAb*BF7%OuqLqPhGO~XR1?_At>l+o>o4uFR3@wl8>p72-IuToJ z@Dg&FF`$M~hE)=zMnjgffmj7FX1dg>(CUO%_6w91c=qzn14YnMG*g+)0YgK1a!leO zIAN%K2s+D{0*c<%Q2o!kVesg9y)edXnAJw-&4GcV_sA@#1kVHe0{l5H9IMvLvQ7ib znCq@MP9GKLglq$2teeS;a6Yzy`PigPi}SG!%*Q6>M9!cL1FxjgBb<+IVBnQhR)q7h z4Gg@JibX^N6$7v9Kov$fAKSpdD`h1S&c`+|@JgyIGJ_pU21Z`*23a2Ad~5?FuN2ls zwlfKgyppPq)R4l!E2)ME=VKcfcx4iek*$HV{=|haikJjyQ#82*OOObn_a#5vS0s(3cwSV;V=CLR~_kFpVQlq1l2cN#lr9 zXpSJf5vS1E`ZtIaHEx7YPN6!=(r}S!e?eha8}fK*O4{WlMhn77t0OT+5N6s7B*x~g z8wNEe^@rH$L~RtJ^t2Bs8YhUXH0%|xsN)3@Q;h!^>i(?(EwoZ{r{*z1CGBq{P80;r zR3XtKh=#P$Bu*7XN7^q)v@P??a~#sQxNy0Jw)Prf_Mf6j?_Gzr_yBn z2tS#J$%y};@D>mueHmu4XB{MK1jdp^`7hw?HDrAeUImV5@^=3X>N#sb)}g58tOGJ0 zNbUw8(|{=18yI6>_y8)fZqRD?KTt+u{2@zgDg*CB{m-EDN=M}@<{@Ci0r_I{|9p&} zTm(_gD7R>g9xEIJyVRI>gvQiSZW}a4wtE_P9HDWRC*er}y{(@=%H4>#@ui+dFR7Ay zCD^C)VhXS3#oU{!7Of<*A$2z{p;%m1739Tff;-4XxxwDKJT^$(&_oG(wxj63K_k8b zDui%9)$GsRPzfphF%c@S4DT2Ag_`-{{*OY1VC_sXl}BO0P#!>w$7>nli~>2gXb{rI z-#I_}Kh(QN*0nJueDubL`_fpwatyXeMLwvahgz(KF*_w;Sl3c?hNMjE3Q}iE%2Djw zYuvg;olawH9k)6E!h4>#dp@BvBQUY_M1r zo2ZJJM5D#3*hE!KS(C-8*hE!KYKq0G*hE##B$_Q&#U`p^%37_%hI{!qJ%2P;!C-=s zur|}`2bqcKF4XC=SQVS7ib>72SQVS7ifL`W)#~mVhn!)pYogXAbD70j*F>$$2rDes zx+ZE}QrB9nbxqW|bbF=6TGvFaOW7)m>s?G#x};WHtaMFOx}>~H*F>eu*0xvanq3xV zCX6r?cm8<2STrmxzhP;0ySI(^8kUydu(a5)wETvpg@)x@e zBRnudsHRhZoqph{lFlXyP6iEc!GqcYw%YwOu%jP>T!*a9h+`^~qIX5GJsa%M5e6M$q52C^Bklx*&3sNKnS(uW|mE@E73jrJC>#0d@AOOVIn6vb?Q zxElO$&E|(IwnzkqZ5=Qa!M)6~&xaXtUQ-UgEX8?E6LIfy`DF=eI4=-OHQ>boxLyuI z?dApK%>e6JG#Z}-2c?HJ_soI=Hl-i-dpWkTc>b0E9l2*z3HxZtMO z1bqNL!w(}3LwS5We&uKQuTTp3rPqlx`M36Z`1jq7T%!rP*yVy>;Un;Hefz9#j63?R zT)tX+&sOKUcTdv$SNR?1_d}?de#iOUAPm#*I45PAe|G;lDbR6FW|`@CoRcay z{f=`|)u!KZPO8@QJI+bfn|{YRsRk1r=T}Y+bes>yqtI&ZjKB$WoHv2$GDUkC9p}e` zT4H|bzB^fToX?)37q%7-Lm_L4ay&34Jv2kLc0kGGTP1|I3e&(=30U%nw+acv3<6;)+dDWhP%sT(QZ?qU9!EOI)x?S*^*}5*KWesyD^dOs?1@*XX84)Y}Za=|94PSano$rNUq zVrnK=Y*N@|@?FIho22HNd{=SBCX<+Nwz?lr_1;xnoJr;~(_fs)2rEp!tGG6k)U_tx zRa~3NlvbL2S8;77WvfhoZ6>MJroT3mRA6o9GVzuWJN0@mi?l^?B&!aIur~nj1h$*h zJHq#)Mo?g1?dCeb^wQC?f{0e57^+bKXi52A0O3!Q@|~ zXKn_55CSD@KMJEdGF8n^LF^P9C!nO*s66H%a}4FP3HX(j$7D!Yc`O`+Sor(kC;w69 z@t@QefnFlZ|0lnBM7(aHz_^~6#j~y|7?1=GWM~ojkV*J?Q z__03Asdl%Yq*r&`f#Qc#DnxtF?6;rP6<)i%-#AiNN-8}Kb5xADB(jwBe^RzW)qjl# zEATZQ0(X_$I8Cq7elg5lFip=-+XzkbV>R*IVT)c3-3$KqoUWnS%m z3spewg&9I_V_TandOqH^LGRi(^$S8kelv~u9V4$gr99iN=StFv{3H93; zWkn*&n~bu2q*|MZbGIMo3K3@)^mj5&MiC) z?iuYKfJVF?8a05zuTeWnS0ACqIQ4!oq^nK1XmNGpMY88YeKw=)_M;U25u=KP@Tq2z z-^dD|Vix<2tQ0h|M9a%i?w*JuB&){5!_Y2f-NbnY7IPKV;Iex>!+<8hm9R+4{s48oj$%hqme`j`8c0&5)apU!Rb29KSvx zm2P|W35hHv^`FDs2V3>*89!yt{6-a>ei*tvc{1ATk}{knlsyg#*=stvb)cS*>xyH} zIp7(;m70alK5FieREaaf&2Q5O4)~PJr&N6qRJ|LYzE$Aj1R~U1XSK!von1YL^1 zYV*1j!(NwSCc6~FUYBAfyA;D-mttn6o8InqDTZf-rf8^jBgxhYRHR}cQV$w zzi!vF;;eD831B9VcgP)qAGN{>=>{eN#wJ2heU+7zH*dNORbHN$ef?@BTSx2h;I0Ma zi^)flIA-(x}x8jV3?=w(0~=n7a}5j{(K|1y5Kj=oEMv<7|WM5dc2YQ2eK76b_4P%M`>G$^r{SCa? zpuZ0&5M9yV7k*y{`fBn-kMvoBcNgd_+VeDHkQWml4U!b=%hJna#0~QkSVoR-UcSfHqc%CRha#;G9xjSOlb9-8RXd zk=tpX5jq_;evu6~X5^|!yx5H{(sR}R?&^#5Z1cA{#Z$IN+e7k7aK*JXQZd1(b6>t%&&wK~$epN~5k~&l zr`zt9@9Ts5dDn^vC&IOnZtD`v)fnZaC%Enpm+RT|ol%=&?j#(Hhp9Dg*Cps08SB2k zO3xnB$egAeeK@>fn54;@gjt%tfN`4K=#6?yPvy1x&%T+i2|HH!P+vv6ZFdLUYq zK6%Ap#ashWl~x980fvI(BsLPePYM`3lMoPlMG6=+3~_fqs^^?dLn{TljRB3(P@6E+ zEd?k5iNDQY(Uhu8e*pnLAc!W!K<-LtbnF6JGsKPV(=%@!rbcGAf}RV2*9_(Xq}Rmo z?MDHF^AiGM7fJzxWeEYXE2V(JioxP?`tKctuVBe$fUhMl1!Ap(-L}^-Er&{<0aT@p zfv?oIQ7Q>R@QUz?fm*FF+s?p`(H@9_rmO!~3YBW1-R83rA=m>)3fc7W2%0216+yE^ zPeA}jqdWENTWeJ(z6D8d7tXdaK>O3XB4W=-0fQG30%E(QfWd1C0Wo~8F_#&pbh^fq|WbYx2@N+y6M69F$uRt$>!$47O}4yDFC;Z{3?`4YVW z!77O|@Z+^_OkACYrU=R|{_uMUMhHxv;b8!hWPllgxVVM!jYKdTkbDMb5d`SteKljhT$0v@)QcqbyezDQmUJH{ax!c`HWRdw0I4iz4T%1xg<^7eHgbsHRU(YKX>+ zup{zjk#7at<56im0MB;2mvHbgSC&BcGJqZ|3bwKmteXJATN_cxS=(0`Z#Wn}8DJ}m z72UuIuWQwOanD3?)6^Wu8UZLlA_x2{RuhtdOK6TNj8Fn#>I}yMa1;D^wz2_EZ0_S8 zrH_EO9RTGH#ECPX0}&(2eGonf=ySO{!P*4~UaqCQ4$j+0Djx#ta35tR-VEL;03^r& z_uYN@QM9{uys1~YC3oqm?uei3i!(80+`LKn)yCjKfV=PydZD}HXS(G+c#m$osgLTX z@~1A|T%nj}0brBCI)Nr?X_0C|qb;e?s~{(Bv?n#t%ufDM<1}GSs&XccJkKC{M1sS}l5J^COQXQ!94@QrhC{{%J8 ztcUc0Zr3mMl=wEpeII}V$Y38$iEj~Nn2k*V1Lm;zR`+vIc?G<^LM7q4s=N)x2Y{gG z%xm%k2~h?1+ZcE*XaEe^j2#3l~g5JlC=9_cdJas zzXny=Ql*?}QW8)VT#uP>!%-ge;s-Aco63R95 zH_rs1ea3)M@f_j{Ir&GGnL=ec15btB72;1*s4zz;B#M@CG-minrg%Z)iL1mzDJ?Ld z!9JBvQll-Yk<3YWlGJEVYV>|46RAx1HR=N;A5R|?WZJzS%0xVA*=_B?gt;6zSuA~K zFhHQw>k*8XTm};)m%$`JpGOj$!9pRCZ+QB^C>6{^Z4qQ9ZayDLs7(a~?>-($a83dA zy8Cz}aS=veHXcc^szsqV+9NqQj2F&E(oxy{U)M!g;1n$fei1Mb0mb@9- z2Vi_8n1M^D76pNFf1;`+xru9clzm?O;H6s$S9)E#qwI$A>u%FMdV$;ZgkI#%d;xV$ z`~Yj6U+QM%e28xq$+j|B50Lc=v2H0~u%S|P3Y<>;)q>r|z}IivNNj@?c+Uo!)Hi{# z77+B5wGa7$RM$%5Y^(KmKiaE1?uK9KMbQ>$wF2Nu%|PiM|f%j zk0cdl3WY@e8p=_a;iEC>{6RDr!qNZ(Mor##w`iZ9;@McV&8QH!F~|YPiVzEd z%%M`i0E5pg|8p5O08)U5mnx+jNsI-c3SuxRC?j!#lrf+iy=vJe`YK$|gjPEP>1?vJ zDQ1EhD3|qMPO9YVHE-N{T*FoXN;!iU1lqx1m*g_oC%Fthlw1a%REQ2K_a~3&PMmh` z60$Z1j|y};gC_veb6UkBcce9)!D|Ttv9|$XWud}+A^t7^PBG2D33NJxPXN8LLFB^v z3XDo_5L)dFsMYJ^?fwKakPWYcxv!!J*-)o`?`yoEBSS@!3E*QKKwezL7{J zfa$kxBvuVbmJQ(w?U;hy#()|}`NX)pU>o$_2H+AI91>^;gRkATt$J=T61W^rUfBWOP5>^(*eIM${B(gm2LNvvoFlasY$P&| z{$w9V6_O~cf(!J>8cA*foGPnpJ%oAB8y{8IE#1&kiM|z)BYL`mV z`{hu+TEuN-@FRe5jmUMT`CFllljH)JrTE+!Avi1>qcT{NkO7G z1EKzDg5Ab|8sa%zw^uKkHn#>HumEJ^QUuE+${;S#L~paIQuTZQI#3*RuOyxu-IIv^ zWw+}Nd|4KJgsoU3ztvN5LPzYk`oPjDh&K~}zCy+g^cCugl(`dst7nZ(`i+y+O-%Er zV+_hA%3zE@I~a_UTn1Cz0l(3+*Zp1(Pr+b37&$`L#$XTtuf{BfG$ym!H%kfaRtc>o zSq!JR>SKDYTk>-~9M1w>3P4@WfJQKKQ;5r;1!_aWpjx1CtT3(46yn{xYfFmm@1<)aQy2s;*9cVN|iPSew4OAlr|9s(R!CA)!2^+1YNmhX5!9C zU$%n19RN=lJO$8{i_#e~MOtcQl(^^mHZXq=fE)hBsOn>2Fh<|Pf!(k?3C#QI>_1%( zc6T5G@dp6NOJ7YDN|K4V-nTv@V_lCqB@mhTX?-J8NiqWo^DQ(IApLaPp3$?$%mlp< z01p`~1;~pgcAXS3xFI1Rwie(<{|6)R|M&QTJh2p-nG5LEaJ3Xa+|A=aQh=T866@94i8Be_mCLo=}^ERCn4}N94%Gw2!U6*rG&wtrJx59 zB^{isK3rY-u%;8;JF@utW>s*HU}Dh|n1nzkDC*zAAOwm*NI3w;!J3|yXY{yQ_!Nkp zfS@<3(hbgr8ji)~t$R+N6yFQaJ_5iv0|m#92Cj)6_eqVmq(-8>s3~M(f=@7-s(g~L zmCy__6CWqTF)#qte~b8u;+LW_J_P|^ERRXwv#VZ&@NF53!MgxWS!6JL4*iU(|DY4(-x!SEHR^pAA4v21}*E zar1%yK(ZMu6=>oiNKqKA3()IQBgz7>77b%3)I>Bw1~4max9erSE?NRfI{@i0=#=Ti zOFg!(ES|qn3WScIsmMK}5jxEPyv`klrwDxU!lX_>!)Gdnh(}#vJHZDM1G5mIDlP2{ z=^KnEMmed=Q^?N(;1lb71Q$v)VFlF;57j21LSt}VoqN?Q`p|eGaLgkT-+7Cb!i~fy z08mzWTsQW_CCZ=^z^_4S5odBKpqG?{3pX-R9>eAk7md`Wk!}3dqgiE`7lvu9!^1jO)m`d!jbW)VE0d2i#NeG@iu5>@EAZo1H@jH z0tT-o1jOE#0tTl0v)vdf`VgoNyMf9@R+5;`XC44p%EcI@EnSG)Mv^Coqm(d^(ULj%mrmoV-on$r!@7)LB(4XLKQj*L>Hmf?5$64klzpm$wEd*XA zV=*{Ipfeb>0Z?}+3^Dc3%OGO(6jyQU05$8pWc;>d{0={U2=UR(51MG(NE0)Ki4F#| z#>{W))gn#WZKvI}wW7N#nq*J+*c0E-i=)+0;gRQfqcS+9)?N69o)?yGxHrF{7ftki z$YD|$P1DLs$K+%&s47>LoXoynVPd1_)eXStqx@=QN>c&IWd=+^Q(1g@fane|}pZQ=^$GAy~Kq%)~cl;C3*$3m^>;}vV0?v zg_1A*a#!p@7q>7YlNa8Pfr_j!7~8-0qh5=2#=j!zP>D#PwHg5};i;n;nQrScplc+T z!8!o;d*PE-x{(A^GbKH|P+Wem-1cvSpQ32cd6_{TGo%bA@V-c<-~jZ#H()))$s6G3 zGkOxhC*XGCy_6GQg`EDaD7~!=UK3~sgEu6X0neP1&keCRrGUZP0_|YHTPAh<#N-=Z zQ8q(D+aZ*P5oH_wWJ0L4M%0yU^waXhetKbkg+tx9rf57fZ^v*kc7Ph!1T_YLtYpv# zkjWDpF9i%*5`u&!g(shRag)pvFh>FcjWR_cvl0+e8rY=twX=!Pa=VDo%Aj+&s6(b< zITxt8Qow*Gmf;a*@B_(Zz>E)6^E7ybhP>7V&|D%TE=6#qK-(Ey3*b!;x<*5bz?U(m zGlonmP`GH3I)U_gvuJXKV7D`1G+kK~k0&mhwbgQP11=Cf!UdIFPl^6`e=1My8VkTZ zW583h0xxq_nFGc-f|+nu#~N(a=ZBS!TaC6`4eHFRcgKIA=Z=^O{45ctl|i5AO!)|n z#HVXN&Z)57o)7eE2lD`^e~1U8l|iX6nRxcl^Z_HU-ktp?G-oBw3zz=nLBH_wKhB5&~ zmcji1Ls|TxvXR(BQXqYvf&x5%#zVcCjByD2sC*4TE;G=ufIp1N)vWwR zPL8EcL~MP+-Aa1hJX>rvlG8wcbI$n;OK3H$P{NI2U;@~^#4r^QQk<+twTWmaAVpaM zvtK9EU^{Y}p+oCExP0DNY!NTAahTr$R;^>-}n9SMBA zV7D=tD$wZ+rg<6>;2DD5#vosy(;4*lG~U5L`91(rVeof>PG|7xXqn1Gz#kLrHU>`# zbUK6QNB6*w)kY=PY`p;Bv%v600B$*hR{-c!if<(H-dH#9Q@!X+PB~Ait zn7GdNGZ!N;@!%Qoa`<<8$>(EkdkSS^cR#KEDA|e$zp7 z7BJ^;aoRVlg|2euDU4s?u0E*O zh`9`Bf6ISM(PPrx9?fxJ z(pwSoqB&P!3PS#2TUzl)=fAFYcl=8q-_9{2W@0145`-L>X+X#kAJQB!V|{p?1_o4XsKu#2YD6woaQ9fX|-8H0^X7G}26IuWw{{Th#;*@o5&Gt&R7 z2BInHfAAA}4|2cMF@frOclRMZOMCoPci$nsHlOd+GVuAzr5<1Lbj&!;t^GnT*3gP* z{X)+7%jZ9y1AI2R!^Yz0DQ^lF5T`(Iz^0}7oF{YczVx&%UQs@;sx z80o(Eg`TzG&VTtD{tmk{QC%}XyAZM*v7#sK|4Dbw`P`4U33BEWt9mBS-C0?#cOW#q zNDOz5bi2OP3(MAh<;UC&4KDdVfUp~(zkT7RQSPQM^{m3L@HV1l*v3mjWD&x0(6I)0 zH#8P(L$xL~%JH7K77~AZ8OGw@On|05mgejE8z}5SeOV2efx4HWzn_D@7lgl!!~w{c zKt6Y@+xnGWSY3kMdpw^|fzo^bk7Y?yCSw}{DSu|HyZ$SzPG_aM4e%I%i;KPuxSq&g zQ^XZS#{#Y_^3(k`dyvINg2J#D<5)(6{9pon8xTqXT;la@z*SxT$}TSBYQgS_V)2(G zPPd>M`T`o4{$g)wcoH zWf8a-O9E~}l0fy0_LRRu&c7kwmLtB5x#H^|EoXclbElVH91LF=43ALkqjxNMkWc(r zz}rDb#6xf_CGvemDj!YnSn?pB_<8_ywgW(0{6ksjegFFu4pBC^Vo$2$YzN@q1>aRh zcEJXn;3_}A68Mh|`;P2{kz)Z9_>c8uC4@G>Q4Ai-P>}y^rQMgY#QP2f3p??6ka751 z6#w7o9m`IT?^Q$YOS%y)X+Xlw9{3d!?+<}1AKmfa=&>-d2g}_;!zhhE0{jVpzqHH` z{p;*2_xX?YiRbfofd35OIlv15-<}`#(Tg9U=qvO!;Pbh#Q^I!{w}8y&43rdG#F$ z@_jrgUk2U2aAk#Con}0i&5!=A9=wil4{xeMYQm#AVGNT}H z$&&ARkniI``Pb0x3tWI@0=WFlx6@aLw1Hz}5(<7K2Rts<;u141{R#pXeFcFFzzFPo z@e}gx1mpQQmyrea;`GuNGVH71%Y*!&13rEKgp>NfNuYzk1;GS%-uVgncKXI8 z&1J=X^y0k|If{ai2l+tLPYQ65~4`!5k0RtFTg#*0g<4g&mIkbR;4 z#}IM33Q&F=v4-0pkP~l3HWOIJ{CIvy-XnSVVaMqHfCBN>>K@eRFQ{?mEthk!V{{&1 zK41}m9ix{4*fDw?U?t!t06RwS0I*|pGhhqg0RTHj*(pl+8sK~3hvczS)Th1B9rul% z=ZqRps&fEy z0bIo5>+-T1J@7Ap{k#PB^Agz4OJF}Qf&IJ$_V4;xLi}0})*Fb0S z0KOve7XiBg@71_YhA}73dG9_P1n%qlJcdC&C+hcw2zY+HzB1Csl7oLnA|mJd`@lK> zzCreAZve1Io4wb5ZulX2dp+Qy04`17vIBQRw$a(Z*}+GHGr{MqVLug47xrNhaQA_1 zV}-k`pHUv;bmhKqlc&-tR7P^T_0eEG_?&O;r^4CPKHN8~rzP7sQ;k0e=0kw3fb9TZ zIR>2F@dRFif5Co8o}3Nsb&en+Wy`?#{atgEcyyHA^`0GfP_8jLPR5HKe1LE-fPMGA zGsVD50RB`+PCexGIDc;3x9QD?-qGa4ll-`tLO31_PJ28lDNuqu@|iHFL;42&se=qT zS<&Y+(W5!Y33ZnNmIFB3PU_<|R0bjl<)C@BzCn&$5Y_=Wk=EyPy6jVc=Kwnc{1<@} zb^~4u@ZSOM!v{WpGOUpx>;ru03&8&b_(6c2SDQ@Orw>DBV%Wp2>1Q;>vCE{HNXeq2cd!KsG++_|$H>v{5t zN)wnzGe~0u-w<<-j`}q)agO`BPh=Ub?%_22pwlP4m^_XH5;zV>;G`k~#sRCp(es;1 zfR_S%TYefG59C-Nfn$LLjs+4p7D(XmpZt2}gb;sH2&aPhR*3h7M*?yV9Qwc+HH2pZ za?Z;+8ic{;d>t>HxHuGHRJ=wB9F$O65#Wae24smgYk%;3E+6;UlCOFzGfbI8)C{rDWdBzywkQDr`#x_|d%9NHvo zh~tL`w`~UaisIxRPJ#Ir;5--t57hZK$btOZ?8f^hLZg0!Z!_%6f(dX~mPaBHcy1G6 zaKIi1WjP=#00XW_=ldcpj>2*TmcS8M0!Ls89DyZp1eU-NSOU*2An<&EzPDftc-#Zt zeImsu7h4u%Y72W%d%~s05?$L;+OvF+@e>uBNcPx6j5bv(DeF0_+$d4CC1pJ;Dv0bX zb2nESYcyw&JF?0crL_)ne^_OFpluoCPOmm*XnO~__f;EPG;CSgI@D;^Rt)aBW2o_v zsxIm|d6;oP*Rav-gb_H%v3ZDl)(E3k>l)ItdISz-*D%ecVU%&d9tyX+ACAJ|4yiro zHW+WInD^2%WsH$yQt?{qUZOMjGVBxzQX-ev$0t_SmFNoMB{p`rP9qf$+%m?UM#;k$=IW9s_eOKnz2>Y z_E+{yJK4BX)mp21K0L+9P__A0Jzt+{bg0_4D!0AOcwF0G)e~(u&I@U+)$a1sjS*T` zwfosMV+qhP{wx;KIXB(Zm)?MQcJl7beZK~<%IM;YzE#&us z{-UOava>?E?x^yf41rsFv6lBd*f>=4$oa;(h87#9uaykz*?6H*t!kaadS3dW zQK@OmhPhu{1Z3r~|EH~Ufv%!R^LRh1ZxTo(kU)UMaKkgABmn{h6Cx-mAP9`8VHOUc z(FJAHQP~L^xMYTC5EW3cq5=ae@d+A6@B-ohqv+_!2(D)p1WphTbkM^?R}dNY|L^X) zUFnndNbaw`ud2R!S9g8)cD%;}XmYr{#2X$IIgYiiP`+0sW?1;5XrDPEW*sezzcELg zLMBCWbv5kBDT?o{76%cfDBd_vOth@zzVS$nSSGT$F|pr5Z8;f^iB|%=$I;h2p`%+-u;SC9paRR%e#sHSNA_+Fuw*n8}csU7}8Mp5sL9*3rVgPMUp?u{t_!AVc7Gv#h<77ir_< zcXQfQAS6#oQb!j#`A}*Vv7d?+FiJ;8u}|XnAja4!1x_kc&vR1yIA&oqidTTr_AA*L zXk#svJ0QO&T`Sw8fcAILNVcDy7j;a0E|47-iRcx>k)i#{Tqgac6^+ouKa&zB9<&Pj zIvJ5ndHW*KJ)8Fm_$-0_6yAMgI8=wn$;{o^+;7B`v}JgZhO!3!)3 z!r9n!kw1=jeJLnzG?XsS9#*r@`4my zBHV!4H8NTcRe_y`y-;3PFA6)nXqk8)T`;=1oY|kqx_Yr%K30#BGx#~tvy@vk`&+kn z;vIX)6=O9qAl)ci-8xC3DAN^P0Us;d7-&k2$7|%i=fuAw-I>ZxdGL8LFv889{i*Eq zg2;<;gJ<@4uy|vvh^j{J|Lh~4S_jG|1p|+JLi=ByjK(D*xyGG! z1(5=Gj&~;!H~4}V96$R4=6Y+z0GYN*)LPpH$Yra<2bLHZpZsSr&dTD(%dTTvcJvCJ zjlnW`p#0k!ypb3$fJh#txS_NgT<9g|y)BL>acha?fnql# zCB7mq@Cv)i()gOq;(%$bEse*v;`QRz`=#;4+eED$zix-P*^ysgFFIup8$Y(-ju|s= zpLySdg}2Y1Ijgep+L;ejUN`Kf$~p1nyTsL5@{0pvlq@?a_NPz1Yu5A!XH30o`pj8V zr&m@!APWzP5pwY%xMJRWNMy$k9}*XY{OO3;6aVa}cssRSMY*>%x6w4brj^U*PmA*5 z`0ec|+4gtt3-FQ|k5l-^2$+7DI^%^9{v~tHhr3KC$1a~iuf-&+3ilgN#I-nzM-NDX)=5_G_D(ofXH$;$#c(@kv? zFw-5XG3Dbmro1ve(7(vn-WIb1!B^KrI3q046c}MgyC4F&yWip9d~LMH)JRT6IOFqm zqeWoY?K1Vr_=C&qA#Q#?msEiy)BamG=B1}bs1444Lk~4!6jq9LK7IMhXA}h8T zh=*nHCk*VFrm4RNIk`!b<7rM5h%4FH3pp9nlUtrsU_9w*Rz5Pdsowz&H6oCYcKbR5 zT)AHXsfh|n!dqw2f?(ban;2^>Po^d>hny_r*mEdP1+XL0SAPJkE@7O$#P<6&)L!`+ z$k)9rw|$F3TpZ%z8q=J~0pkfI0xE+5*WLvhQ(mVr6IiP;<*#T=`DTqN-xlIM366Os zXi5Zld`e>`_)un0V)96>0p_8`$f`vtAEU{ift;L=aQ0|7JtfEUB4Y&<9zp#m}T4vd4vDeF>evX zNQXbSC-+D0#zcVEk2@y3!@X9d&gITNEdd+tdS};7a_Q| zg(jg}snUNI^1)y%?r0HN^KYymbU9V>RgjMY^Ls<8$tG)Z=164_Qlj}8RcB%LoTTh;B+|xpp(!y&L+b=l6X#gED6jOe`#tkhP#Iq5l&-irNz#qBY zaZ+MD7tw`06j8jsg~WLDq2!w&UkK)RbJ@XYOuZ&&W;_Q``u~9bO0f5ZP*-36>^N;> zJRng@OvJzef?~JC8dE-0W6F7sqVy@Bs4?X{O;K_(4^b3DzrRtZ1yF&`9-4x2Kx4{} z_LIGnoZJ{6$T!o|I5h)(sigV+G1=4i?xsG`_z-G`OatrKa2_%|O;v_u`-cEEwG|yeM9CVvE&`PiY2ZN6>{Cz|}(CAD+_`dcgoP zQ=oiM3kK1@L(g!v#xw|fre^Y^8ehg81kWQ>0*t^j37vW3pm=x-2JkFF$yp=AHD-D| zU(n@~HFkFpJao_%sx)Q-JZ;eBPiRazPaJf4y~Z)#*5M(hi9f1_$)0}lt;RT%$P6iv zeFbA$3niw1h$r{jc(cYdXq$XB+36A!1DmUj)cAKTBpbMxcq<{@OH?*YcACbl=qEI$ zoZcr&{~`ROp!XU7s=mtu5v9NgYeURa4_(fa4V`%sp?Gc!1)y7slGn0t??nK=$gUPo zo-Zf`mS8@f`zxkFJY~?CX9k%JTy~>K=`mHGNZb`LFWTRDTDU zk23=$qAHIY6f^6I8dJ_A2wgs3W6F6dq03KbOgYaee7U!snOvF(aJMt`i%|)D&_Y@1 zAEV@-v><<9ld}K^LM#$vts>dj-$DXST7)Al$c?hVfRioAhnLBZFT+YRQPZO#vqD^? z^)1RvH2w+cu1BNs=N#Ihzx*3~TzL6*1t%q89Y;;h@a5^?^R3`k<#UfDjK+5{ON{gb z3ZYPodrOtzQ1A#Cgqv$OgB!ql2IIkpv2o-=K?Cjp&&Has_#W^=Y_oV{Mfq&-06fTK z6O)%EaWrBx&ZkricohnI23x^3SO~iN@{hoyaBq5(&tHY~aYSP5(`w|0ko+9DHZv$- zD}L8e&oDE@xnOm=l#cU~tH2X*Z_2GN4IT{Mgap??=X?L`go zZiew=e+WgGAL7L!ehyrN5^(d+1RB7&+<((CYWDO|r++cO#)$ zW5!nq*n}qFgG46yEm*ISv*7h;!>BLE4=QUl(#q%nuJcDrI<|`4!Rlha=?m7=D{aPj zW;Ox=3(+Z}P^JNs!QbQYL8Z@kgYi4d$=CXfQ>bPMJ`7GnCsbAX82BRYWs7`$3GRae zs!seexEA{SX`gnyS=fw#*?6cn(T}hbtXups_+1$2Mu7faa06!WF~0u6kp4H6V@XjN zo&_KF`#?LKNDB;yAuuS4_%W6^o#Vql5J|@`yY&W}xLHt5(jR;b&(KsS8xH>7ALooe z9;_QY3Cw3`9L8v70aFz!_oKI`i)wtdW)+de+!B=4I zU*H>rBUR%(+EC4e$H97uPlxydSRJo(UPysum?ymNdZ*)9EgJ!IFwRx4?Fk-$XXW?% z84dgy2vLavpf-C1~6D~GE0|M?uFD>^K z)`N{pIOf1b1K$Ck$GBC!_)~BXEMEDLGs7d`W4NFD+~=Rbx&fC$oRWsgX(X1%tNaM< z5U>=T1jE8juou`t25J}%0w2?s*Ad_~Xfjt{PUzrd#BM>*bhJu$KZ)sJ-G;ltcWO!2 zf`m(su*rYa$trv+l)yIdF&L;4*d3C8 z0lwJIdziua$H9}(1gckD3hAe`!Bm7A=eTia^BxG$tGFPE8?XXNs7+$EZQTlUSeBdi5lDClX+aEKxn!0ju@yb+B&mrVwujFUPpp8Pv2Xv3(VU>0}_HdVzw=Yn;E^1E4N1v}d0y`S=!Haj0dz>}!*3O~d1;3=57R28OUc<7z1Gk7jm zXf?G)L-O7sE(Z6(dZ!973>@2yiG`1LScNx-B1{J3saJFca$uWJ17l|p#lL8hi1CMz zeocrMfzvSThC`p}*Mk!?pV7*AxgB;>dWjlC32Xr`$F@@S`VYW)Fu24|@IWZRli=wX z*JZvOvtqOKBX|(H1-m#^qtOhEKiz=dP|&$QxKwM?QQ%P+M#`W&!P9vb>yLu_z*Dp~ zt^x-&<`|2>diu*^2uMMjt14OtUheO-S!LM88xDr0@JqB8ybt3x>hpeZun|u(PJsWV z^{IbVDV7~^aRhrxK#?pU_F5$;7Mo#wWk>c4oVm?D#4ed z$*TMe?gQ7Llk$N=JGmhjr%es$5^Brz48(f!E5Ubm#`kXh=o>@wzk=1psb&}1R9(_E zJ_nck4aU3I6JRt_Vta$SjCnIJ;&I~5XphAegA6^zt&rYe_hiiH3=ID{+8`Saxo+@H z;IWuy)Z+Xf;7+I?Rev+U`u+Qy_Mt9!3ziz5r42=f9AtkCW$*+Tt)7@zp9#rVhWO&tD@{56YUij`RVD|PI=K{OJ?-6fFV0H5B>va+ejQ{P zi*tey_36cs-lB=Nk;~1EqRoxwZNATTX^0sm(ug_3C&7dR;3ZFrX mHIa`0Q}Dlc1Bef}hF(FM`n1Y~VteXWaYXuHZyTgYwF_I_uQ;;OsP z^L^hx-ydK5%$;9n&di)SbLN~g6Qn}iKN!Sz&qVBv5PmToyC4og`M1MYKlpC`+jZYh zr!Plr3516r?1yj?!uMgmpMQ8hVn2s)-2%iu4dL>CXyaP;zsIvM7qP_<2>!co{NKZW ze;ofmh_=8sMB7kA z6*>@wW*8Kd3p}E_?f^im>ekz+Li3Q){6z*@FuVO@Gcvq4h^$Na?u+76J1uejXdx0W zdC&qV#U?&hemLB41uZJ~@3c^7>Mdww+_PQhgmGZ*MIJ32@B!>Cp9ZKd`G3f~P_9{g z&hzrH2E$BBVGdJx0jFt?gho}7lqB;IE>4KhVxxtHxnN8O+4}cU^ z5p9xf2-^}OXzJAx0vVmh66*A;C1~WRhvi=xPGHPKwE2a=`$0~1*sKq$CRPs78ZN9F z!(ruUrgfZ9as`5Pdt_YVG-M=kk+kRt*hm75fC?)|;-jNrgYq(u*yw2l8a?TGEl9CW zTWl=jVq9!2#VC?6^|R<$Fb0|~ec<%ZN^1J4LZ{6<#F}4`^$=PRqbsaE-Df5%pB1J@cbl>GD#Z@RXmEYJ#yX_5 zLeE!Tv@l_6j8KB6q4&bYr(d)zgx;%dEg4V4+J~;FBC`sBQ;L<5gwdVT)CEo{oOODq zCAFWr$!R;OVkv25(s#;eaYGkE2;@34KtPB^7-Go#%#en% zc@VNm-EzbYV8nhq!2YjT_?33YF-ha=754Mp|J3e)-S%^2Xc$du%;sMzMI0rzO-=j~1Vi(7Xja;%Lz>W!N)Z87lq93GGPC zmyo!30*Q}LA<-i%_z$7%G1;8)67UTY4bo`fH6G3TOMu}kM9iKFc=(F3N4Ne7^1c8` zeH1|RJ_X8UPeRU3iK{0e=fz0X2Bd0Ruq>_(F_d|ZeQvq7Gm}pj@RFrIrl83(?ynNa zIxXzi*;@oUh^pz!o`Pn}{L{3kmS{0k0*Z>Orc&(`m*z&|_NipM?g6CNtxw;myDxp? z9S>Blzhh(NeRpo${C0&nVBoTPC3e-vgDAfaD7&9;0M68wYP*pa0ZVcaWI488(X}e1 z%yH!@!io_W+SiexK@nV#;!xVt1l!1JXMG3K;0T4%9G(t$DF-3uNM?=-u&78Pkt|6V=BKv@!}6tIvL); zvpfgXz`L+`dqn?ZyqC^3@Y!BkPfN^VE{oD*aYdvG%2m;F6_hI$>$7MmzcKRb@02SS z&ySWbiQv6VkfpWy>?|ziMhZEq4c3M1r3$%i?jg;a=8=b!J$xe1y@s-K97wF4EQ2wa zU^TZ)B9WfD-46V_xh*mxra77}st`8(6uvWpYf(YH9K!M~SLl?2?aG5`ynO{NR)=Tl zP3v=b6aQ0bIVXpjWbHwQ$2_ag1{x2AS^djtGlIo5$z2G~2wGr8nV=Sv{P}RwK@M_h z(cH)3 zK2s7G#Brnkbii;2NO>&28fGpxZUxC2_q?>!WG_+oAKs9~+ikH?{xZxiB`w523}QJz zFvBM?+h6bitGC+-h9|=@2xOp}jy_H`sg|gM=`?OSHu@Kg4ZwwKP}BJ1Oq55=1YUZA z@Zry|u74ivo1U8xi|uJ{a&{C#UhFKeXKs74ZE1VHJwdOlL-ROZ(I$2;F4qcc1nMB& z%NiE0SX4G+n5IB(jVpLl*R^(InVP_g7bm_9U|0L)fyK&xy?sGhe6;k_iN6J`JIy15 z`Ars;FLQ;4W9!m+t_+F3iN6HqmQyw=sHOK=nA0=unPpmN^F=^=!_RaapSl8x>|{Q# z;uWwW9Pk64JShCiLivY0dl~qj>@zY? z*UgCio2-N*0<(QE=Pw{!fxsd;lnQlAAXGviJb9>1`8yA-*bl};)wF{tRGRJ<;@B=jGmN`>B8d$U9Z_eCc0nv zK{wIje%`v{>keFW*rEtxMSOqJ>^ZhUgq}wR7WR$=v%{i8`e#5DOocK1lFT}oiVJ7M z`6{98et+U3wr`Jpj=LXlw_cvLh4kSML^35e=1~*uL!Mx`SsJf3!^mgLy@3lmvW*LT z+CFTY<5L)q`pId&eZ`aZ4+Ls#pen_B%0WtycQRly{?=dp_!FyeLCv=F9Zi-&p7#I! zapSgT%e}sj_!k3NycuSE?bNkeGh(s9zbr*G@3McWn}RRlFOYGU;TpplaGk>xF! zBOU!{4o@JPEsApq`MEaq9GT_U0XUKMM4}&IcZn<5;N!%GIGeI4~1t?>ho=65hK?b0w_r-2>Szlg$5WDR$SdHW+*dt!U%7hXV(kOmM7a( zK1y6U!SJtj#pHphdyc*8Cp!0>GQ&^kEwfjg)F;5qc7{2A0OU64EMYuvD3Dg3zAEJb ztBAIwtbgOdYe1j)%x~O?iyk+g?@KLLeSi-(fD}*TX=q`LYiU$6!A;&P%&UuEYFh_!8q_TuluPX^ zDMR%`$Urrf+s{a-=Z}{keIofxnZ2yy_mPP!FO)pvRU48F>*l;28NEzLW>y{RK)E;h zsBD@a>)S8In{HlVE~}igUs9WJQ3hijqHgV$SPm~svD$$vZ<&Nsxv9{8!wS@NL)i^* zI(gP2@y5wQdwOkB`%;iUcf+}LAgq90(;NXQv863CIzmNKXvO7OZf&?hTx3skP~zSQ z+n#7J)GP~S3e6gdsd2fGzL=muYj&ZMiScHY)M4s+Mi%8q#KtaZXb)-<&Y+N%6tuC3GE4g z5IMQ~z$b&~{=|{8oc=-d^i!*v+CGzP9Y0-}4Lj=a24Ero=HI)2pT-B1k z4OW=P2+px@09W;9oiW>WhllWE3tR*|Ej)`Sde`E-6A?DGQ?s$JP# z1nV=CsuqL$&jCZN%DB7_7Z}`W)7bAB|4cg3F04d`bbf8L)msyocOTX2+1HS`dc4}z zFy^_n+KI&f8IR|aU#Iw_K^ioOd75V~(rCZ5RC{ccMo%kOdK30_ONBkpN_mL)9vjWn ztQ1l4hofrD?#|}f*O8bke;6QR;U{~du^aXU$}aPNCanbR{LuJ$kjr~5<}KsqHNi(k69b13;Qq^Qzr)==rc}-?5;U z`JzLam+e)~)AMU=GlE$lWj>MEgLG3G@HSqW2U_PB9T?;tv!{s92h9SBITOgR6YwJO zzVZ3x3s$BGy}HA||4YtjLAUzvaf*+R!d;pm(AtUY!-j32NqgFmG1W^OC}^vIwq@hM zZ(G*zz_}uf60HMX{yLmyP?@NvWazbL^e@Ofa(l{0gY-+N$->*CIEN#KZAOqdrD4N1 zWPD=m%9|-sorCbt?Z8jO-dGOl)sQB8_%?{I>Zif?4;=FeY$D-1!ec}>5x#VHQsV7W z+%0jdOdd2ZTPSF304tgjOC+Pf*l@vHmpwNd9V2JlpzCjkw(B5JBI-gx?2vCvG3rXz)b zo4_eo?3U)43fkd3tEqz&tCvHkycn<`?_H$dY|2a7D@4!QW+y2l^99?)&;)kghj$&n z4Ht|Di0(`C6z|g|kK+uK6ZX4HDI3i)chT{4b<=oQiMc*B<>$ za_@TKFyu|fE>16JmGO(KERbrG9P6_Uie+b`H(37SlS8|q?35pI-4~?>%tZoZK8bH3 zj`+$@XG`0q(YQK6F907rFJBr}G%)t&4d-f_zxi*e0Sg(i1<*sC~+?G5(p5lYNHV(N@rrMB(1%cgNyRaDWXhoA6kvB_%8)a z-CVFHDK{s(07*MS2Tm7fwlws%K8n=K{rzT4U_1=!PH1wI4`5%2gY@RkMp-m?6o zPba*j6Jbpj|Kb5`ND`Q6_l(0tK=rW z(ECIF{UEDM%J>=ZO#=Tp%MPDSD84I2O6ch_=Y*A^tgt#LcRVwMacNi`y1@)74GpzR zZP(e(20TNUV-3Y4SrqOtBb4Lm0Dob@5%22g8jJx)BUIR_;h#27!YBo)BmUu>D#HsSwg2a1cz{&v+BL1b~TypoS0+f#8?{0U>QCtaaB1 zJE}+Hs1NG3-Nyx;km=}kSV8l()|Wdj!CYY6e4b_)NoN z!covB=t%O>x#68xP+p0@4EE%0GQ$^k)jBqb6oMW6b2w?)F9T?S(oyR;)B(Bpk`gWG znJlfJodT<|`}Xz@qGx90fF3FteI+HzAXWzZB0FHD)J)sGFor({aQ<^0nY=+j#&Q3_ zx$(BltizGuC|R{;%bC+hgDBA#?R}=qbsd*bn>T>5xN7p!ttZaa;Gnd58;B z!yb_q=hD^U%{09kwSaC~>~s|JSunoLDLn7v00Mi4P>%gty9}hbn&)kN8hAeC&?{37 zaLym|#rnE-^8KT@<)FlxC=Nym@^@eQo#11~rk)Nq8~fh0Er5TT{?Chgyo~ zz^Ou6hRTxPw6JhLVOHN6t_zkAQEC{4rA}gB+9_$#DT(3k;0s|qA5Y??Z(8PcG=gUj zMx;LKe-eD9CjubzPEio>^>26lDX>Yz%#VSNQs+!QA?n9s__@&flL=)@wWISgnyYF# z;8^OdiL1CGZ#GmhK*e8nAOl!tU?tFwYwN4)TkEB6r2`k3c<%ZUz(v<)blE$Q@q`x> zzW_qDsTP;^ zT_@6Xsec*2f``7{aws&(i5M-U%uLA;v)-(+4VmXb&6cbI^N@m5EjB~xm3}mzl)V?^ z&~Jhql^a$Bi3~ilHk3qQ5@b_8kAIROU(Czh5$Q`z0I9ZlzlZGdeOo zqcZ>8T)wZM78!Y;hO!M&>tW_v+tA6S!W4H$ctg555`9RR4$C%1o=m%x`H@S={J6B{6@DT zI(|Zzi__yLa~XxT>T#iV#^f&Tb?Jn6o$m^|Z{$z(eyi9grFqhCN#$olZ|K^%E}%o5 zAMU??t-BZQE1tJJ0^d2O8(cdrGe9eNezL)pAv8F9g?Nx+#~gJoBFP&0zX#+{<7B*W z0(Q~WI#SaIXVS-kMm|pPgAL)?J}2ta+u1e)=(>7WDp1k`dj^T=b%aNdIJ557IX4^8 zNKzfnrTs*Dn8kRoNIx263yw&srq1Es#EPL_6@3PA@zFUO(|={@lQgDf-X}8Tj%&fP z?vu18WPH@Oc;PL5?*&cD-$$*+Jnxl`o4^l}!}m#GvK-6@T0T5+U|6ZX&UR0DN0?v- zKlZK4qVtGpA{gl=Qx@Hl(*6e+MV=q9B6BpY@S*3~gKK%04rJ;?VFy}##CM-c?NcA5 zhd;(umL!Q?PKhcir5Hns&0F5D%`=a5dN}z0-b3bu#1}lQVd=mhc(mZ%0MjyH%=Qi# zluhxrgBC?oYJX)0y%#?0^Rg#0KwSF{Fd=K{F?#NTZ#`G-U7Nk;A8WW!FJ_LRdWomSI(-GXF>PL zr^3vb!Ak53Quj2h;F^FIAiKYT^nVBP5R{X;*8y^QfXhqlWA|0hA@^YCRElpfYOL%Y zw&f(iZp$~JRUH0;@niwYVbUB-ji+FUh_OOALoK+{`R!KjM`^GPFKSkGGq-ERhF64 zYuIE~Knm@qc|B+w)N69+&WNJT)XnC8(XY*=`!X8ARyKVe;Bue!5`TV%K!JCkGV}$s zou`D1uAPo}=gxmS`9E-oQTgJRkf*6FP4py!jbu16+LN5_g>TV`QRRXPk&2%cl`}-w z@r?NE(!?ICqRA|NSQihL6B19<9Su^6v=7&|Tg>9|I#zrFD|go%J~65)H`{iM@mulv(iqCCg!GzPb9KlwMeT^1&UlRf|h-=fEJJYueMnPZBX_XzE)R} zfIH-UPTy4btI+saHV~8ln;~0TU^BvV*-o?l~4D;DxYx5v*~3z<;DZ z@{>4muCjM@J23YrgH+aDICK8!MZhH)9cza$Ix)%!i9UL7y^h)IfE(!pWxL2;r*;%G zXH?B7_f8)+8a@sGq+;l4ni7dW`Y}4i8hQ8z@I=ZXQ$*F$+7JXnd>`F%)WNhw9ORUr0sK%fPPeF=n3nh^<7`cn zaoZv3rk8Nh`Hn-rS%pPU>uCwB~=nQIo$c?bm#!y5N_RQrtEDu(tYqnxF_)2iZ+X`^|>=eas2 zrJF){W%b5*2&3MtyBlQgy+TSX~Xv+a# z%b^>lU{0X5ZvPgQi+1j{qwhe)= z10)@kP$*}u4{^OxCZ*teC5GyiG!RlLdbo$drp)Z)l6mcM$+q^2A9N%UYo4!7Z49Z( zyk{h4vnI@lHm}MG?Kb~jWq~ryW|Grv{!uiSTBU?sY85?vocW;un5v$KudV8glxe}& zan{!@q+cqiq0abWWO}`bp>L$uz`d?sM4NT%bsM8G%th?R`d-PL+$-52Fc4B9XrL@p zPwpI}K3OEo#E%0v`!#ImPGP4bMZ}o#rHGouG;hV2XGxzzc{XLtyeZXNJcgDOixlpo zqa}+;i6^UAWbheRd^=mV7jAjUjl8mq_?Bl)#qQ_3FS2I1@908a($6jkg`LSOTWdZQ z-cTj`DqVV!qAH!Ou9SA1MfPC~hdPR54E(nQ6Y=+8F|~`} z-1`mA*{-WNwbMAMf5Z8~_i&~=zl(F|$iHwV!Z`01?snv=)MpvJ-mMeJXmyV2nv`}F z#k>K|^@b72>lH&xF_y z@hpfx8b|u>i&}FMpTNPr)jMPyXgx9~+4owsaOWH6*TO1WrdK%{!z#DdV7<=KT2m>U zR;kWb!R=qN(C^eal;xF9Z8y$>YkQHOfIFQi{mGi&`4mQUj;`;si&&)8iKz`WEkcSo za8D}u$B3V;p=PhpGwN|%P4#?}Ld4FPwyEiy`7sRha~3w#>=TZSVrSYCdKjl@Yqx`c z_BLTpW6vl`su(K9W!~bU;~2sITj?eIQaof^v(8ZroVH6iE$nribELxyfQB0Sa?h5} z0wg}M<$N^uZTTdCbJRX6C(&2e)lhRzcus}=X)bzz`0^TR`UEX#*lAJEUPBF7MNq2k zya6Elg{M^+{tVX(D#j0V$e9T^!v4A%ztpqDFC8!TODC|_>6bpltDPLjI$4)Tb&bEw zWfrw4d4RyR0o+>vcO(k;$0*zh#|gosqN8vWDs%RZni2SpFv8}c;w0^lInji$HR++J zm3HrLA?fM!?42`KdckXstHC#&^5Zh^ZpZpjnofDBVkp%B!M4tuRF~qu&GXv;C0KtU~3?vpLF?>2`2~j^MotoGPo%u~{6*@=m$p z$`f0P%%jsd9#zgdyFr;c4gc|A3RtLWzu^IE`?aE3bu*)dRyS+p&^|dd-Qr_uz5a|) zbQmm}Vy!E!)Tm1F%@o;0jTdEkvMz#LEEYNZMl?3#4J01(JVdO+N904wErR0g9?6l+ z@#$uGZxFstIMng0fZOr=0I`t3C)7!o$)QJOB4v=M@xBFnCRXO2E~C7eKOpZ;;#S6$ z!%hUrR1jwE$ECaSRV&$YvWt~Jzdu;d9rk{R^UT<2R!}YYv>uI^t2lVwG=Y+3^MwjL1jS?*icro zqTrX}3=F)nNou4Fw7b|LhtyMy!QhUqmbRO$?_8efowNAZ+5t}1P^&P`@a05xr_3pH zI|sK3IY6!Jrx3jLN@`>WZUEaFiOV7=`=T#~D@LpTgire&eC6N5ch~puCI1^f;>)NP zl&&`{@qC@D(EFtDBVDJ3;p0!I@wxm|=)WdfT}E50vsJh=qt%hkOWokJgV9cf$|ru8 z@wu>!H`%5_wuuWuk!?5-YjY#y{sH!KgV1CN0eT^3Leb=ke|-u+8wk0pWKU%lIXLdsY{-4hO($v4^|K9lj-Tc2VXH!8^TH%Eoxq%?*8zEf?ao2Pi zNgswZsf&x1Eg@-o8m>B4mQUdBhBP^Y{}97N9b`$O3;kwKbkcyHM~h z47$~3g5~Wgw8sc9R6LuR%WpnJB(NM>B-3EsZiRO)g`K@FvML!}OcraCLodrYU{jEn zC$CMRY~?v_HnfEMj6yTKpYj%VQ}B|dcMugcgM`_B1LzM1dTTwoO@lk!-ImDdO7Q1f z99qY^>3SA;mq;M`G1{o`F(6rtT_n$TTV3!b#zAOeb;_ahGBS(=eDKC0#Zl=1kJfkb z_I)3(!k+G8x26dcc)D(Nq&YFbPlj|y$(T%?_RRQIPqi_;)@O97%Q4*iW&jV6dj|$Q zJtSW-Zgj14selS{s9%->q}_vy2|YBcik(JRozM*PVR*VPHus(0G-&^pj175C%p10H z@RFy{JQ@1@4{+BAZ5g;z%y8`zTHrm>9xvjE_CU(Do$N}3z=2mz@3_r@^#-SIs~p-T zW1e#nU2HPc^9KUQcFLho<+OHk&cHamEbzA+8t{_)E^Vh~r5v)zNPI(5ukh%}iLyk( z2AhGNs~vptkUv43w#UnROMDFGw)r& zDrxiJ6q49H1YcENYcLJDOLLTnCFc)WYdQ2kGRk6reyd@XNeS7Hpnt#%*LBWBP7b9- zX?PWS6nLEN1PVoAS*V{i-Hz>E(XO;xfYNEeB`FT8gMnHVdF2njPeMl;=&$tY1j@16 zafg$v*_-c>xfVuc4<~Go-a^rW!Eu+P1o-Y(Q)2;zoeri_y@Lup;8{ZWTa^rLqGNJ* zX5P{RUK>U99oWW50e`EMy0{svDBZAbp+iF0@eFV(=H7h246d7Az9HTgz&)R6341Xt>`igb`?uHVbva z{2Bs_#FR)i^jw^=!}5>*gK4vMaSOScy+Ym~MwX)$J0qjh7fBE+Y5J%WPNt z7__K05{KaSmzxNzhrVqPj>BHV_FJLcT}sj{%u$t1EGyg(k^uJBpJgS~&oiaj-qpo- zVQwst3|fHbXNs(=oDv) z%R1&MNO3BndRi60y##Xw+*%|Sjb{VjKL%w9aBCWqUq?YEk+%zkMx>u)@KTZWR)8*$ z%G<&1SO9PDoqhQr$-zRW@A56F068=%hi;Nb4G{gdN zPI&vjVNvw$A$hxN6kfDk^Z^XuC;}XrfaA?c#QiNmS`|18qOA&?@r^|u!YOctJau#9 z5{WAEb{w_*(N_>{S>XH}G`)9$v*1Z5*rHJ$S^*qUo86uP9P!B%HpII@x*8{ZMAouy z!1E6B68>>_*0;%_n#qJ);I*e%JDxA>vi!iSDUV;VMo1MFdHsW^7%y4lVA5&6IEct= z-`xd3nL8)(K8~m0<{5bfg6A7twn=Ri*V<`Z3FR~X4Hv;glb66zT-O0E^2Qm#tHOy; z^CZD~6{oh7vMKGSJ9dE#({|f3Q3v5Q^81Q`jxpe$)!E3LXg76cZkoraKF-`^8w20d z{n6N#dA}OYA>f%U1_xzhr?Hr#SYRFQv`XCSC>BC9H@u?*T&hm2$FC!hu(roU9Qi6lX1vxY* zQ}A1Wo>4QbCxIhnpG}vc=`wh$(x6yTzJd6$%BzKf2KW_B>?QKXg$^r_77biI z>k{mD3Mpf#I@K41n`7vGyRaPYkGEWK#@WwrFx5TFl4WTe_2Ulw1!%MyPk-hM7KS$AxX9=zN;Ersx|4`oe;{?UCa{5Hl6 z;h%vqGo1JE8<(qr;DFzZ9Dy-dk$zpZ&PnOd0bOHxk|OfgV_jDrD_d9NF0~6*$4;Ta zaZ2cQ#Jj9cyNj;c>9SVEgKkIQ*-nGqP)6jcOo3bl4_hqOZHSgs>mH28_hyu5JeUOB z@`+3ZGcmgCfxY5~bm4UR6r^vPPQM3f_jK9|>1U?XU66ilI(-<@;N^+pIW%p3{}TLQ zF+U?^{({KdP6WRhl0)}KVw%(^@5IiSm}d)9#D7UvQg`2s@{Em5;QzpM6P&*u>k@r) z6`Vx*=c!Qsz@j73emG_Tpe<}?lIx5RZ|?6nX>Cy@IaOz` zYA+1uw>QRk|8@ z`4f#RYt~g&*4zPsc;=uiI(~@n(`}50xA^8Uk6Y_V+l0o@ne5B2w2~jXcHaG0Ad;rE~sa#*VF&e|KjohWsa`We8 zPZ4>>zz&7Ev*~6E?u_T5J&lE+u@5AzkLe6#9~ZxySj?8Qooievu&Ok0#jQ?5y8?QS zZ(l7CuPf2ZHSeJ1*z6&)=3B7TG;9;V`o4j^(*2)cnX7ogR$uM9ig(~Y!m?NK0<7;F z*bn|AtoABi@L#X4xr+C#Z(y&nYdb4lnL?_W8LQn4{%|F8Hl@4?pnU-CfSk5GWdpR` zu(vb|a+>lc$oU}mcLj46eueb_)IG4bv=wrS@+QdnAV;hXIaC+bD`IrfeXF0xES1hG zfQR*a5L!RDx6}YR=oNB4$PF;e*~IdcjZn97Z|TdBgI*!$gIpEl*z%N3P`3%%K@NI_ zoDXubUiok4rp#hHS@6KzJ*CW2aZ{l?AZ8)H9pX5M*Fda>cooFgK)hm#tfn$68`4(T zmt>zVfnV0eO9WO9t&!lJ*<@_RonWURgQ7g0#|2vByMri+0>7I?-nDrNAAtAa2FkXJ zXu_OgfUbpCITfw&GA}Iye8Q#?a{pW0+I~`ZO)a*8H5rtbKVyO4XMjE!H=wBPwlG1= zT=r{*8O2}Wgt?PO`Gpun}z%MyBaZb0J+v2)kjV#Xf+? za9e~;4?nP;tGf4_T7X}F@0RM#F89_=o4?xJD)AR+mCb`fHK451Uk%McDh*azWB2oFIx2H`=cYa@Ymt$*NN z62Ag6{RG%`L_fojKV*XQjr^Iv7Z9nXz6md3Z8l`k8}PRpYv;l53K4pQl)&FBY66Vj zc=e4pJgX7<>=MbLM;fD1>_gg-RJ7f<%1QG7b)9;;fBIYFSb0o-B032ug9q%>f2wG_ RemZA{H~dq%7@cMi`d?Uc`yv1U literal 16596 zcmdseeS8yD7U-RiB$G64(+?m`3r#Zx+oocmsO7^oP10#;(4w%rSoJw+Q6~kV0AZ83!m7LP_ul*G z{qfr0o%=E8o_p@O=bm%!nH!`+#vBD=W=up(+XTd@$MY|t%tHH*)5+_9TL1C8pO@A1 z5OXag7o?{lbwc_%%+KpDO+`$~G{oEusT0ya|4W}=&iUWtaTOru0Z17CO*j1S;eSrY z|4((lH6bfteV#MUIbqe?Z?)BMUdZdpwpyvIEwOg5H6zx30`j`DMV%F$D4+46yewa( zD9?aoLE=N5`$CNNzH{{7kf@Ztl<2-M&yg(MT32@UIkD_1E|H@-W*0Kw64+#$#NF51 z9%&dm7(&Sj{=bEcS|XBk!5Gnwl98nFf_{Y-MUsXq`zKXQ)&30_24jSuU36abVj`M~+?H$pGsEO)+>F8HDP*gqgLaDm< z+YA(vi#(#H{)JNOpsJoMp6A#zpr)<;j1}de=`MdRjELA-{A#}ItoX!co2nO0U+hOY zkDtv!42RqHM;b#Y-zxsjv)4wRXt1Hdh-ZW0?g2_c7+>em^c($;pVjFwWNIgI)t{q- z;Xg;7h|O_UU?6jj7G^O@jCDR9{ruPnBw;49rFqpN)m=-V0a*9uIfk&;s)q z6iJwe9p<5pqR~C2s_}V{-Fr&W-~o@G+cThz&0%M724{&;3EZR=k)$CNaMBo|r8(NE z5qh}1pJLvM4u&fG^`&Gq#er!l?WaqLs20Y@+*Sh(L`b%H#7!_i>);a}a`p|50Yi3V z;{~7@Qxc;X#d=_hfuqGMf#Umm$zlv??I%i}2_6m^Ri=na+H6%s2s|!jjHt#c`_mR& zvfkWJCv`+eM=JZLz>G3}nH_1AV3kVMQD*pc8yO)6QG|#vz3266pv2z&x~-)5)exzk z?6^8JJMz0R=qqZ0BR9uZmXUU2j_84oYxBEzmtF;QWI3$;YU`c>hTa_|pIlJ-0e2HO z;Y6zcS8eYMBvDHT6f_-Cg+H=p+dsBm&#&Q84qh)kCw;et>-+9VX@B>D0W!&r*R=TC zU>^5*(8_HM)^i7g1h+PbrKU8^o_U|B{q*x;yOr&M=Hf%TC(S7R&2G+^OZ<+LL`_qKjTL=!+b+UrnAZR3*Rm9 z)&@xmBIBg_`X9ki;%$KIhmrIJ={LP@qfRZB=**2}BS9pVNGS_ae$qyXC7lQ%unCBT zgb;%W$f4e9g)+3|Ldt_A%|*sO0x>^MpbILmKCjz!Skn5B3Ol*3f9W>C3Os|@#-Wsr zdED+YWF*DbOJhM&oOB7D84Dr=eM%_NCV4nYd`41mDDQ@V*}1)Rj-M#P&wOW|D3eXL z;=o;zjUp0niA?8ggSs_HtR7|BQ0_Dii8n=59KV10yVrBqwoQS)l%En^63V@bLqG#u0Kd~#I#R> zN)FoBdrJ6&1qPUOopWyD-8E!thjA?)zaHmRNZs?idAnXRwOyo_)Nhh7<1}@vpcy)kW5{f zt_%4_WyEBGSkjPWDC0H5>xGI*D{~(8A!t|6PY~u0x=~YmrWQrXB4tGapC&j47rM4X z3lT*Y9q(zcD>FjBEJ(Tzl%q4zz-eKOa#9}|c^9u2cJp{V-9vJ=*pa>9-Bu(HjkO3z z4I^etuW}#D?dJ1LC?}62#FwIUC<%eQjUo1f_S+`km36v(zpb+WFrhk?RKz+M6NY*= zI+qiJ1WS1t*0_%JCO8T2;h|3n3#-_QyaY!-N41N*m+bZ!dZTQIV)lboIuAPTjC)L75=p*LZy>rxb^`>7tHQ0)i!t~|&s zg(HE47|g$d@%-#~{_S|)2kZRi5b{_jzhInJUyyKl>|M3*<yi$7{t#@eSsyBx{m=nzBcOnFd~BE`nyF1+vL*6cU5`>cb%P4#feU@gGziCZs~ z$5sG|PhCP{?%1!LAjPX%LEr0n?^zWh*vH^$UXvKQ6(M%n8ab?yDfThRXgLSZ9E)7z?+Kx#2UJSW5R(lqD-V*ESiPbkn=+H(D(59v@Y*^>y zZi$Vj2{9T%d^mEVR2?GJLxBvya2w2*5LZR$3meyfW>r5Y&9XR4bv^O&JSb1-H80QM zoFHi!^3Dh|3-=a-dzbyY_15v$Gr?~rA07IX8hKRVMJ)Y$%KPr4q&MSwf(0r3109nR zD_~h8t#6HKH{WF$D1^OYy%8~lQ(uQ|M={jJjv{B)y2n~)was)UnGE$P*T^YAcCV?> z3AYI3Ug{{rCJscJ$;+dxbyR$iRTG-KO!Yor!paEpxek;Z-3CFdiL!DVPYkZ_yT8G&y$5$1^jBz``s;NTGp_ zHmrO8`Y!AVJ=3!3LOU^cuT2>ul-l08ligV#qR9^Q9AX(0>T;Bp91`LlHf?`xkTX3vns%)mqylF zf%YutU$MQ?5$v)`3Aqm+#0BP^og5@qS|$*+r74PoK^h6 z^Yx?Vbxk&_*zW<%6q?3L8}8GYkaq@0u$cenfPmnjCyi`W!69caf7Yu9C~_ zjMu-9)$NX5Ak(;ofHTRrk;?w<2`G1W`;t;RM9|DBv*1CR0Ynfcy#tAcXef9RW&)V3 zxKHW;L)t@#ayYOZcpqAk*dEznQ=fph7NY1ec;ZoaWx_lDKuAtzzCf&bH4+a;y zkS^~Z1h$w)5ykZ#prsb)#C(JNfYDj~h@mdKoPC5^{h?{f{3Mv?^N~K#;&_E#jSy48 zKD1i8s2n})$wkqp&r+yHXtYfUu)+=-V4^YtEqs#G_p#2xG)_dwxa1yW)k7Wo4hx~Y zuw-|u*x{^jF6fp}-(#<XX-x3lFWwedVIV_<_QQ7UiNPsEe9F-Z z=K)W9>jl?(W$&sf%(Q2t7YDoBn+?6~i>4%-7xp-(sLW6Kz@oDY^(d;U>xhaM-Y8WE z>}>?QPh#qx26~>BG{&c;1l>+4!ICpSr@U&)PDx`S=4R6NesWQl#2AS=X?7<_vHB8V z@*14T3qG|ov%GTHb1M=fqZ!VOwJB|rfZJJMLmMJkQ{5HCnhNVE&{cyZ=&2MZ)>C?( z6t9jlZOJTKk99m^+k%*XSYO^~OGehUj}SU2duAbHfI-YWD7&iA^;_S>#U1Vo7|~E*dF#Z$WGUm%%N8J-TyLwZ+nj@m5_@ zHE#kP_;Iiao+`8T3!(_u_2c8zj+~MFZD*vrcDDy+;$5<+LB+_q*3=~6lg7T_#-oN6ohG7;5|!y%pK zuE?gyJ0WFq9z;Eqnp?SiM`9=kG|`umCPYxQm)T2s^(^Tv1g{i(0yDX}F}<2OirCFS z2@=|$eiw@SQ7dx zp#S0#kp0#99MB!3cL_+^7ZE+HvXI^s81>N5r^q_^%QSim@>0*)IA;v&=_tmQ0J>>j z1ecI`{qWg0(qf(o)<0Ob=0Pe4nOgwqvi}s>4%JOGmnRtEO`??7iA!B)i>;(M`s6;9q%nB zibjHPUO^=I9MMrEt*xL*!$hPL+u$cQ5sMC=)# zU-DbXh~sUjqq+5mA$2_*{y;u{C;u>{Xr!G@%TCocUHNaWg_AWt>-_|F?G@OcLHhm^ zwmJq&IjfdkhP{jvpV>)q+5uA39k6sTH)$MOZ3+GqiI}U-U1BBXh5gncM47FP{R>ez zXz4OG9yovXxvGY3Khli)`Mt3yalpL$k*d&w&6I_ppnm&9OFh)6mZqLaT4^alO*UOR>5zR`PK z|EmLEi8j2J%q~JyDJuK_t}SpVMU@f_1W@Jh`uWe)M>Jz}e-iDoQt<8u z$1fZwf+UB8@-|9H<4CN5L#K%{H5g#7wO;*Qz=%EgdW3Wx zPNAst_@-NbWo12>pj6BWbsFNJ9xDAh6?^X$eXT1WkAhFSt+m_Uf%#Zq2g#|t8kQVmo zG~j+-=Sq=OI}{;&qB(V`Cd<4AtcRu0L5bN^jd&x)u0qY= zP1%?6`J6hG1yEPZ#MCc>M02JFWSJqo2z+qRNE(hSr@4Aw+{xe{UXF{;w>8k zcFL+bpfBk!PpRl9C<>?{W@38Q@pIP99;fKrG2+^{Y*)8$6bS}s()=axR1ldy3a%6h z#S=liKc;XWZQ9Bmg%wa1;K)G+tUtY@ZtSL4l5w@?7;qP%8_N< z9~FnCC?ZLsHG5W|(jjxpFfU>hj@VICF!BLJ(3@a`sNJlUQF@!dCUJ-ryta%CKp z_b)#l>EmE@6Q2D<$2~)8D%$WnkR1tPHxsxdE*In=8+x4|RW{e~s%EyjRt-|Wx(R#Q zD9>l$UgnK;e`u#eS_Q>5*%SddFYs42)HJj-NL|x;qUc)Ac->%-TASJFY)9r}J_5%D z5L%1vOxT{dt4qq;lCTqeGb{K)9u@dxI&Na;0pN=@kX`tTp1EsJKuwi=UC(Sd6N<9IhDZ%f`%WiK5f(+2)O~=goC-4Z2dK&)6I4$zLO>5hK1G^-cSO%9)d z=)-KtCjcG9@6jJH9pzBJY78`evNG`BsKGNjJ5CLzYzSyN>I1YN?@_YRD6x0`U=Yy;; z5cF)A64O1KTazFTsq%g)E%Y?Po=2jt9mp*}}S-6ClHY&>4X*&&`-5lS(%=7J2Ikxk(qys+>iC)h&)9>#2AY@T> z#m;_(KDqrGI9Jc$n7wnj(-PKPkBq)Fpi=(=Jdh_xUxsm`UA7Kg1g@!hwm7j-j1$*+s~^se$8HL;4`AymLf6pNKr+ml@L&3 z3K#e4AQ=mv@-ZyUs+zE_-Q{^};3&*@r-vyl@7W0+cl8uDLJkI0%9Gd!AmPguYsa2A zezy^L_*n2&4qdf3NH_PH^L%}*(wpGeZ&P?D^tKErHGxuUJNiSR5BpNbfihR*9pg=a}OE1MKMnhmsD@^U_ zxCL;DDhz!GR7=;lPfi6g+ejfEW}R-0>}gs}E&P)UCR@yR)cfL`n|h)5{0 zgexg{GD;LA3U3@<{U%em{$kymy21m%+y0(oAorFIYr$i!**&O$@ewAJOB9gXsBIa6 z%(YL04Os+hM8g_&D;C=(z{=2~if*wgpoF$U)GLR4MGmitDj}kITb{v>>NCVcl~(bu_2zY0c8}J-6C#r-ziDl|%_{op z)#7>*wEH04Xxj7iDh9p2hc5g!iGZY17W58uiOkHIxS5%?_lb~(z~0rq=~ z?T}bmPs9E%_a6k^hla}Q^`bSo1mp-fnDbY7Dqn%~MtI){Zu#V#k~L3eyykWLr0K0V zjtqs2Ho&;RcqSY}jJOf;~7>qu0Y{;70K5!!2Dh_!7549iGDdeGd znAbtvEfwX@^{0YYaYq20{lT*yOd+P&yzY(36wrpXe4OhaM^Rq-soAh5a#$JtIZeVi zO+t(&$~YsqcjMf~f_2oK?)C$lu*E{q=Rm_L`;nnTsnr1WLNN8+>67vN|2;<2kH+{c z^3yTsrs$^X*eG;UrUqj;9xPZ#%=uezWYggxH9^c(dBjo*e3A2R0Bj3F+(R2 zAN@`;93qO8`KQ72!fWWY!5R>%kO5*8#Xh>|w}N`>EZ^-Jh}C$boA9cDCqWq;m)8VA z9I6oFPnO-6*JT}Fuc;PKVe7%gE@>My^jlq1uC;4Bm+}zFE zZRx%gBQiF9_c@%Lu;l6eO7{}RefKHC05kJt9c?qP=>{N|^? z+axSvFstZsE*MV zq$(2$)q7Be`mKuoBt2vn30wL zzGPkXzT{YSF#xtbsZu1gzb{P>G}J(+dDla;D1!e`RX;D*{b?LVgcwIC`O3^djv(^1I^$pMLmlx`y6 zsfCnf+7-mK0WW94d8nAQA|%W3HNTeW*eq=3(?o($_|rsfa=Nc_*fWF3=}{<8rVU$H zru#~U(Toz2BzmY=%Qd*glT#wn#0hsoo2q;}e9y(-;ZcsF9Eq9mEf&|h&N5a)G<4$e zHbN@yNL|oU`=#)jD%D@*Hi;xzT-H{wl<-S zgmG}}h7iRA80VHNI48R=w}a|7SQeTtCJ6!H;kz1D$sYck%@)%}Ls1sxt_f zS_5BGo7RS6n3sV~Z>inkW}!t*Q&%W!IS}Wst|NH?j>(OTBsp8E6LQjwNIX4~kdqGi z=a5f;`~>6^A^&s)!Pgc8mjri8bXJJd92{SxrO2A%+-}psnL!=UfvnlaWi6J2thLk< zCIjD6TP1X>R41$9t6!?n>oV}liYk|`i^yRBd&TJY5P!eC=C?mVAS&O`bLK1o7AiTt zv9?)A6Z`H;hjE&M7Y+@1_{*h3S4s$8^nEamiwrF<0x- zy7#Ci1s1p+qK;1O!*Hzt_ZGk%h{63O2KO=lv9L!)#o$PKfh*;=cQ8wb=#=N-S(=h^ z|C36mZ>x~<sQaU4vph@NSSo9MyVZ#?_X}duL-{FnwKNB2QC2DX!?71JGogbC=C|3)Llg~+fY95zig=k7-mawP& zPr*9=VDKu@s=ATZ0bMt0<**=!`S)X*jOn1tlyg2ckp%42l2KQ<85CYOuruA zF@ZR91?)!fny-y8&iAA{W~vr26?i8ju`r^HwdlHV|JnI)uLXFH`_0E?;5Q2Kb|hw# zRyB3OWmgj(^kF-+SsNvkhAz0Q1nadPk~}jgST8wD9z-FAgu5nn4Zyo2JPPSZJ5Rc7 zu142U7gcSqj_)BOOT*ool#L`nmhf;tVvJ_lx;QU&v6x&oMC}scj2T})z{;>|73L&A z{^}}+v&PEXjNDB41}lfJ8-p)DfD7yQ+3@8Ui8p}P67ap<3(H6Ja(c((%OX;1->7nJG z(Kj>aa9;@)$5@yj{aTpgc+u4&+@9IO=W$Y3yw~EQ8Zdwet)qNdc zPv(@}7s=)-64pi6#dI{d?QSnbJsX6E&IW;)*}zw2*<2ZQHaF#NvJoea3U(Kk zqFoV?v@STSSr~}>*LC2vOp2}MOTtTSEy9a%vTxxRWwp2_tTh3~T;DfBi|ZS(mDtk7 zS0#_D>zV;8iNt$h-+g`RF<3twiw##QAXc`8YvTM;))wQm$Dtj(kc4z@X`2Z=-#l1v zt4wi1=Pgi1Vmb5WZd5zU(c$G3)T2gY?e7@zY|SBeA0 zxX{28F#ozT9Ggml?-Fr&i_ZklqsCp8RqdKm$GWS53wSOy|DH=s?xZg4!6A{nG7|~- z(&)AUysiWD^PwyJY;a|*z0A$V7&pTsMot&wCYJmEllgxS5uVHQKQ{7zJO7{S+jP*F zb{|6c?2XGeK)D$5&ha)}-VbH`P91NXfy>l5Tur=fCWgBc%GgTD;rF_(??`r1pablD zBSer0kP&Ll1P*OUfV+R5RVZ`^fw8bCoo@nN^zdwCgt+CJ z2K3wbU3q~XjO2U z1v)?L)>II$fZsTRW&2y1hNuy)-!j6wm$+5H2|3&=OF`W4rE4%xv^SNw%!k=YjRz2nmV$&L4(SJVtmb z8jYp~;D6jkEWs@s-Wy`*^fs*Db-;+ zuh0h1;zCu0UCJsvBG@HeAqjT?nL-3lRbfj=kLt?qRw70>zW#FfA2Q0Jfqx5OrLhFy zJqf-D+;3gtN;b-2eT;{3X_3Q2Am>a6a3}`L!25~g{g|%#ZAzyNIIZWa_%z zZ~Q|{74wk-{ww_kf#etRx4H0|y?zkSH-4|tC~SalF~waJL`m=9OF{N_jtvJDPAZfR zzI~Gm@XS2r0@=iNG)9xuLAj3bNgUkp6-%N9X9h%NZ;{fk*LA z_o0x6%&Wz)I=FfC$veeen1|Rx>Tg09T`KsPb`P}Px`9z>N?VT z5-JiFq^+&#Ot={=q{|_(x?f4`LZXN<=R3jyf=#PiEEy5pA%MHM$ae>fI}g$;u;&Qp zBK)Qdml>F!Di5Esh+fbJu*dwe65i)p^o}EjgiZq5Qo$P#1N2<@X5qtq;_x{=a`+)x z4mZfj6+`R^{wO-#ZXZUg?IKB~xzgPBVNX#SNU#7fe*&HWP8|}{M&xj#jI|R!?HvJ4 z1Un7*f$?5_PO?Mni<-b$0~_?yE4JC@^;`|w8&2oc_5MQ8GYjM)_8VRf|3bD75?4`I z6TOY#XHQ*_%vlZ4q94dia1drRpi;8#+#VdOV1WXsjK8i)jjWp;wytJftb4AVb-`Vp zWy;u{GQ58&VHZdOK#cd90r$pnANmH`ay)pspS_;}jGf%|5;?cXgXKUDOJmn^#p#MwY5oiY{7GuD2F&|cWZC=|VSmpC$1e;LDbg?ll%GAl>v4fyTeOM-nJCNua z0m%^>;Fm4Rif)J$qgkaPLU9C2dLG}`-~mp6R0q2F?n}y;y-+xDOgUFy8bS&SwA5TO zf>&JEh2gNyJ}2Y5$%q{dAH0Oe(shsyrIViB-M%IE)z^`Q+OagiuOO29;0D>kJY>~5 zEge}ab7|FQSt}jGXie6=vD}e$Zvxl~Sl87oPde0Buvoe<7U1VycFDL9V*wuE7)u@8 zZ3BF&4#Gsdia-+{l8Nn9>zxuEO#_L8Z^80z(1zrzgbBvK2}2x~$yTg;@qSY|O9y`m zJAgD*@z#s-swRHH#gA=Jcf!dZpU5r9x(4P+!7j>dqns5NUo#o_x-#5`-%-HL)K01* z6>Koxr*+W22Iz|2XIT|*!}U0yNT(i7?p9bI42Rp`_XDvuG~NdHLmCzHD^Hwg3KyZ7pETx#cOfxTw~ zjF0b5>cHl-K+Hqoj0Umw0{+GBo?x0D1UvT-B+O5&$CwJWQ$2sPjXt4}#@dGfn9hsKY7+j?36mT?N$0e*AJ@zFv@wO$k zb!AS$&TkeP`HzI7e1hBVa=NMN&2D>j0{CDUp6SSTW|w1~CzGJ_AR-pe4a;IJHHQ0Q z`Q4K$GVer!SWbqr2szv>r$eb2N}tGSP+AP7V{!_V>Y#K~)@lOp!5oq zI^{$ty$7YY#?Quk4#Z=Cr1co+9ek$=?iE;(7LpE9IwU=$ED~e`i5YNDb#pZC`J6l$ zm(6%YiXd_H6R)S?p2UjG6<&y45R@0}p2t4J)Knd_f7Qs;^9uxy-G`yeVbesn|rhv_;9_L$k zy8(L#7K6JAd=`{b<-W~TU&JHv{?32z?|IG}Dhx|+sHhyamZX5q zitz&SXH}yon_#C5?PKB}@g=vB;eQD@(!r7P;6q1&o}~wot(FTYjQ5NG3d>x^3$Xqxupj(aSlwm35Px1+dl_%*71&8mUB?}8 za!9w*!)sSTJX=YhOsnt$v>%{(sOc)wmOT8pu~dJ_+(=kmrHNz*xt^cf;Pqd-a+zBqm507Vecua`>A|@fE)f>>Fa~ z`3A0P;W0f^O7TX2CJ`>wT>?5h*2IW@x zE!O6?MDCRV1?sS=20({($32#r-27OK>=c~Vh#S6#Xra`I{Vl1H;sBQ!Ze*HK0Upo# z;e7CTD9~p_flvut`=54 zuzabp`tB=l0e(;DX_4BS6v;9`P(Xw``rFiX}c=pqp29>Q`3B$Tq-S?nt z)!iE44qCbD-sv#r19k4@E7we~dBF9XyY8)9jo%>j{&e_FJ*4xHdLjK2QUFpHq*o!0 zK)TQ6UV&kEgML2*XNF?nEdlj^T0q~!pL^-@yKxQ?&YvcPp~x#~H=)ae3VIFx781H# z`0XD;58@X1+oQq2=(Rt+_L^rQ+=TuC*U*C-V_E!x`{7co-;dgH@&EO^7-L~v>PK{J eKP<1ulYq7rHu diff --git a/Tools/bootloaders/CUAV_GPS_bl.elf b/Tools/bootloaders/CUAV_GPS_bl.elf index 1672a2a534729736510ba2cbeff20e8f3e17d833..46cab2cc83e876eb6765d1e6e895f20feb205537 100755 GIT binary patch literal 344004 zcmeFYdwf*Yxi`Mn-g{ky2^f+PCKoq$w8typabLe&WYG9-}seV;uuF{JjK^ZV!h z<9*lUv-V!kWj*Uz-*w&3-g%&Oiq1J_@t?%<7*T&$m^u#kS0|VlQf{JvUzf}qk*TFfcOyN4^fppooxb!vLz((H(pAHl z`Y_VD?cw^|8jzm1kyd=bcjf z8*L8m=;Up-PU&KCt88EIp4OUGmSg*4SNQzk&PO`bt^r?wUt;dEv~BcQk7b>fWpi7^ zRT~<-Vxz}>CToRs-CXZ28P7EC=n99_F0IhpDtlR%rQ=ycTTxhBq_8$*UGXfmEi2}E zr6PB$+5KfE8#ZYBhi>M4^Asx?qwhHxdT5O&;TRjqLf_P~Vc>F?9_8=otUJ#}mA9<% zNQYN?*qIBpt1>F#2X{QFvymqP(7oZ)ptLLUS85j<;SN1k`}AoWM@uH54c&a0!?wMv zqutV#n&NKd#r7^aRPRyI7S83iNRf1?9#-G2EzuLu4rqofU0RB_m3!@7toyg6q-nHc zjCAO?rR>b^T2+7Mv|Vwx%I+<}x7r+*9q!yt%f%_Jvdz(HImw(dOwa>LlYNa>o%Y{%v)R$t`W$*rl z;S1-$blk#2zbIXBOe(7Mx?5HEQ(e;fr%&7Y)1HI+Z}tB;d}H9Y?K@7ZQYCo2)6$U( zKib@bOMungY*-Xm^u2u4O+BB}DYvh4^O87XJE9m|ooFj*HJ6~~^G|w|PU$QmAT{oA>^-C#&X2o0%4u$& z_RHq>#lIxU-ZLRc>SMZb$+Qq-pMR}cZ#X|OR>th8ZBMJGQgZ0+3v%c;NZ(wT7P`qb zcyea>I?u%B;;@p~8O)kIuosmvIiozYVw2~(<{6NBkitRcxMFg8FC95l2U}NkrcFtk z`jSUJJ}azb)=#<+WTE14%9NDLcyj2@UD(91t}`bRV=CjggATZn2X6>c9?2#$#|h5V z-U;)6+Q_f8)%(J>z<&P|`ksGkt1;Vt$((iVDfM~!Q>U1Fbg2N^?| zVmY)ZY|-V=uCPgGdEaZvulSW`e1I3yb$G>q3M@EHFIX!2B@|T8=tU^DUv(8VsN?El%)^!^D*NBN@0WNLQ9pO`f22` zjEc>ExsA=0&(#)g@(evBhpb^alobY|p{d6 zk~^V1)j#=6HQU&jX=7cyFx6+SNcVfsOs`zo$ZWhzV(Grx#+peE^lcWB*2HqOTm|^f z>6Rw zMLG|C(q(G1GZu1nvtf$px8-rmG4-@rR&z=BtuB6bTTpfK-1kpO zV@>016};h@MwgQ{ynjkrX4-oA6mt&0*HXqYKPGI$xT^bIJF8uOzVKHK@4WwLO(Apq z49SaB)0CvA>DjPZeqs79X_VH&3+yfZ3=3G5oZMkKkyoGvOhb-D*V?HIOVL}I+|Cl?&ZS+QFGdRXz2C7%ub==Y&3T;%)Rp@azCj-0)bpCPHap%u*O=_FT3Rgxtc;trja%$l! z^NZ6ZJ)BQkw@o zseLg`@QG}Rb)`_cc#5an#&F9EtH&Ll&m%AKXxi@^;Yg9iRs^h3cC>Yh42o2@*+gv?XxKPcl@REfGJEI!RDL3rCUuv+7v9t}8dBeW;KXdNH zD4f9>F5IseW5iAR;Rk3O{xaVG1(-K`uVX5&NUx#2qX=}UsjwpJ-Uqjh+dj6!o}00P z4RPqEZJ|A*%QgQDzx1NKUv-Y0JgO||v@BuWx?s{>yRNLrFSC+v=;XzJ1bOxLb0b+2&AJ4AdQEEEKr-=iuOS$t$&K@SwOCbN)wX{>J1P3RE^#sk0fqW@POspYhPgU1?3s7Rt3)*d+}^_!sE zVe#3EzX>MczV#v>@-)Uj%`<9e_4Q2sBx1%E!PTQ^^Os2HkyIAxNP}z)(i|ijlP_;G z|E^ZIp|+J+b6MV;7U%s^erC%Tt9gEU3k&VOXx1YguUwL`+8(U=EF5@jM_1^K`P_gX zNjT*V=&WmhJIq{P++VosxeBa_HCwLPL#Mc3Ze9I#BcHItV`}53#A6c%WqVIC_BeB@ zm^)##j)#2Ve+DgWY4GX4sMT$0yl4>i*D%XF){;C*J-WUkyXF{dZ#_3)Db>*u?8*w) zaGf<^9_nP_`tXJ2Zg@T@vOoC6>b(460qrQy&kmUKcel`PzH0J=74P_^vnQ%jOWySn zhc#acy7FIYnS1{OQ+eTndp~Y$@SM`+mcQMffA1#G{J{JAlffL_jTT?oy)n3THJ`A# z@$LNeN0&_mRC zX+>ks7kv+gKXSbpKI+Q9H*wb&_vbHVL($%Z34_$!;d+_=r&Iip{N5!?8wK4_QyEMvIwI@}Y>@#?^lY|13S}ou4gme{vtl% z{``-Rrj=XX;V;%<6;Ib?l*sS8^R7A%>XhGa8FANY4{v^PzNuowqKq23*;>(hSB5W3 z--r3U>1FSmv`&$)0~mbAS%KZo+|!Gn+BO^abjU`YicG4QB01;Y&74w0dBx!{+xz;t zpFYsDezLo4 zPWBUFtBX&_R7zT<8~21&2OpnWQh`x4Mi2#PG$|)mR*b8sdgp{eeY6H#lYn$ z+s9%3xefc)vz;c)noV6;C6;dMYCj`&rOKvr18TI+y3nYKbe|O3(4}rqb~;xmv@Sn% zXZDVBwTqh%?K{2CZf}$2X{}!*>~H^*{4{zUE4`J@a6jyO|z}oHEqLRKlLd2d$Iqp8e<) zyCeBb+3;hh*uxJ^ZP@l%*xUH?DS4PtJ1!-)xvZA8H9IiRa;Agxiwl;`1Mfu-OO@@i z^k~@SnC;)_x4$62k?Bv`G`BrVdLry{B|TOWa>$Unm~*B+P@j!?{_I7|+Pqz4^^r@K z&CZ$+g3jfZ{E0_-p|d7EX8ng3J_{e*?w`V(1NE6ksaG#@y=J$Tt3Jm<(=W`et~*OsY1=o4nU=DPJE7^Cu3OecIv($|Y~0tT zDPK043-X%HLvr;Q-lVo1tTG>k)feT4bc}62t^lXbFB>`5J7xU@eJoBEEHwNgbFP71 z7P|ey(DGqZQvLfgc3}LEIjrGq^@9tNo@nTObCI;Qi+MYo_kI>$zm4UmHOo#3Wlbns zb^+sU=?onsHv}))Y>efvJJX#Om(8tAqgNO zl0r9y6K+VD>Z)Z!^8AJMb$E)FLZ#t+zvAVEub!*DKHqoda1GA-t59|}k`!XQSR3;_ z#@M-9mlUeo$(X4k!*8m{tlykb=aG(SUMVzWC#+8*eJWSfWSOeyd42~ZgTokc$n7~< z;1Cv!SE2@$1RbL%@LOOQHy#ae}Q~@rCjIvb0E^O4!n0; zm}Ae$a2q(DlTPnhjio$hv~ z@CgO()KElT)jFM>Z}}$Z+T*C(`Bew@u}I-(;aPi;PO#G}c!pl@$qGylD;0b_|D7`A ziZH)rWvi)59r|(l0Gu-;SIsPDqh4>WYqiNMTkTT+P{h>M!Y8JMBFf2@6Tzb;EhREe z#H_jD>elB^e;)E+Ei8Vsy8o_yl~~1BRQR1C-=zKRtMzHEhwtCtUb}oitJ7T6e4YN1 zp0CgG|MB#p2XaF`uQk9%q#u9A_NwQKmM1*PElIw=VlDnU_^ij$B86(bZ?xAg@suV8 zW^Ml_s2=(%_#Re>MBn?tS3NxNs^{-VGyGe0HsbFs1AY6WaS6mSdB~znYdxaodQBmV ziJfj?)6PCHdA*0X(Eg%?EEe#s-!HvRFKYe|{jE0DoRawqj2pZDxvHK%VlTcmHEiFz z%RRi)+>z61ZHqKM(#rXyPHV@tZscq-q~p9-d5;8ZTe(AZ7N5zHc%z%K;kAv37Z&bp zo^JHPS&V4LikfWtqv^l#u$H1dX&Bd$jxQq})Q|r1?Wum*x4)i0%nLU+ZuIQ0Eozom zXnM`0nx^_~c>dCpbidAWA{|S+9lG+Lk&eGdBz^YwJ9U$;zLKTO@4f2D^Huwt=r7fD zzD3j3Z8dw;yep0+DQW{b^ zk^{+=_egWH!w#JUBr8%P64^*XVoYm7U+;Qn_1yLZ>7A9@o&)|2f0l2*uLLLGl9lDY zZ_rlE<7tERcULFt``Z(^dH4$bF|58WJi))vh^IB>$wt$t!JW0?6c2?xyXHbk58ij&o=@ zKTuj~ZsX>*qg|`vsnp*)A3nSu>dP?QUbq z0i*W*5y!3rVYaO~$a90iy~zDQclZ>X*-A@~93%gfh`P(+`<)?U!$|g;fe*#tq&0Qh z-ao}s_k<@-x9ICIt{EcLBe&Z(vwRkMwAM0m>`_ab4Qtxx-PB67QE}+gGc}3^F{=5OV6*+IXzuR$ce%>4yIUSatn^?IYu>vO z62JPj#y5nd&H_!DoonwP7o1gWKYhI8r?R;d3vq}Mq z|JBHxcrwFNg6z9;<=mB9SBCePeZ0`6JFY$hTk`VET@{Ti|6ntxxBz3zX+P?i_Gx(H z+C5>{^rR=B2p75cgxxi@7|omYT^I$tg^ixkGPq#c(G55&Y};iSJ?ZFGAz7Z(GG3po zqwbnWN5>^ch_jKBTPbZ-=DBU&R`(D{mgbyrx0)Q5(Qa_hA7ev__hy@e{ie+!jp#JB zC3fPOs3gBrI+Ie8uD^|5-Fr#B$~{unPaD=F9q-^-9OLJ|#-bx0`K(?1QD)h0jjMA+?kBJ4%$rqr0xqMfl8m9-2W zp&u<=&hqtuP4c!%^U!k5+xqr6dIyqKVj1lX)hc6;F(;wBzs+IW3cp8m%JA8Sw?;Oe zZ}|Rpqo&IZk&bzn8 z`gO*9ysvoN^J84I57kw#@g(6?@c5;=>P&xK<$ix6R+ zR`3yW3#}gRP$Hg4-#?`m?hL27nojRep4PhGA|GasM5E351ApZ?8n(H{H$RXWX}l6o z)LgLzUVPEq#c`+DQ{qpKUO-fd8U4-u{uM2QE^!wi*yXSXxTKt`@(6-+8-x2 z@> zv1lt%*Q=Y~hR%qfyC6UC=_z%7W%hx#9O;?19C;Yto0{OmP?Sn38CdeJrQZ>r`wZTy z9l%>v3EW(0{MtR!Q_{-ixu;9yu_gH>7zIw)alVZRQRE*qPcP!CvJ3xE3!VbXJB~RD zzCOC&X!Xkh$ysOQb_W#a{Xtg~ACZPr4IknzcyL=@vwH{|%10nFe^dI=%;k?_ot}$6 zRHzS4Ri6H_K5THCkYnp)Jr}19>A6V9EnTK=nA9%L$6TSKz--Qa}kCvnQY z-p@u~Xo;5@g)%Jf%fQy^3BG45KSeEU7*?hlyzRB)U22=$Y483mRzjz-0@^vPf^Rpf zS0_BCVg>x7(>)4(JT};}b7j}h5<-K```cGxaDQ}4%GrQD=k;cWQIgp>vz4=h?TSA+ zAa7Wip=|Ksjr6l+YiX`atBaMxmQ8HLEdieI{Iv6D(^?;vr4YrV?~_wYy6(9ak;q7g ztIM)^>#nY!!M?=DR<-|J?z%@gu{e~->VwZ#Ux8NJSe?f;R##|^V7C4Kf%ZFLi6yC8_%}L8?Eb< zjfv|J=Wom4wsl3#O5soZN$bWn--K8PbJ`oaRuh6aVagfaZ}9_~jyQe{F%b7QLXj}M z*EizcovZoy;e&9dxiDeP&Xsi@xn-+Aime0VI?{1%H`;_#>#ANTPgt|Tlh&N#V2*YF zxEH6_?E$lcpU85})s1K%*3EEBLWVlX)TY?jX*t2P;R);z{4}%wu`M%s)6q?y4ac(Z zbeVu;+RO`04PRmBO<)VKB3aoTxF)fAxY}7IuF32N|A_8??ut-T)DZp5^9-IO=A( zHoVu(4Y4H|^D-70ceEw6hv)7OyZh}Adyy2RG$b2%S-JGi(TK^iNQB}zc(cDiZCc}B z<4X;3t^`s;w&e8YX=iK495tDbq@HyzPHQec%f=LkBz{yjIHQSEn^PQ8_~Gir?P}Qu zyyc}g^5!y%Ef469zMtQ7Qg!2f$1c`P^{hoIYU(#->w-`HFIf5o=2Yi~Bxz3N*6P&l zJcs6C)P~itaTf7gvUu8TKC*RzmUGPKT#&e3LQNF!;i0r$WOGB0%|X?1n_A4K^e;Bw z>0@(X<>fY8cYbR#8Ghd8zs)zoVm+ed=GJ8R$=eLy+y$xInPK@(?8#dfY^-*IBbOy8 zanJs=GumFQ$v%bNTy$JWs@PhUn3L*Yp<@>kbJB1>f_s15TW}wM`}-GI?w*sHJ4LrU z@ZRc=jIve;oG`kj{lIwa-<4x|N8llRc83i z<#Q^vJv;}m?L+!Sywfr8r#`>sA#yzm~ zqaYt{4M@Y211ombEjaFf%)(pJtK}0E%d1;()UV-$oo+<->K5Rs2wbh{T4)~gKWxct z$*kUDQCfh*@cyvT*|K6mOL*^?mhge%mheHYRknoR{DcWZHq_7g|Q#dt}|-4B~9n%DVL z9{yOpC22}CqUL-qV$-QVDQjNmo7XPOsdrCnO>-i7XE&#;NUfPw`%+Nym-tgkxKG}m zK8c>^r1Hc5lF(lHFz{y@cV9+(nVaHSf$*(W9sWZ9ag(Ay(U^23I(w#_Wp^vCNXK6y zi_g|BE%ZfaQBz>z!6X)*l|Po?4s)!O1*ShD@fklS*-qM_9Fwn)duNJp&1hv9j-hy4?F z;EASKt4^PkZ%GaG52?wvW|mW%a}w)famc}6Fz#-CC*9B1-c3)3KaRX?Ug|d;Ss(WG zbLa!z_}-xND*wxkzY5m)ejlVKB*cW8cq4EjC@r?`crg8%*53E_6uS7mqS(1P= zQeBv(()+)`@+Nml!e%DNWc$OlIT`3H#8Cc-NIzl?drwKZat&+KB<9?EO3B@O%3Qb? z&rl`Qqlim6oC?0NNvW5da!s)>(qZjZoX(nPZ)tm|e+TPy?i`>;`wli$=WE-R7A zD3-C(Z?1m9lc=w#E^2?Y5U=KcPX zN*Y01Ix))j(08j{@U_P1N4wFJzw!TO`&R$2@jSlOcVpJp%Kpo9s~b@A3;)*2FR)9} z4j1i{Jl%34`YwJx=G)(nKZw4oL7#U_QxGA#Lf@bV!dYvlg&fw^kl;L>*k7N&eT4D; zKGIPfk#*m$V&h$YDayCSdU|(CEQ;+xmN!dDsDUb6H_Fxuw*l__4f zd8L5@NBe6gg|WwcJJz1{KZ}-7JLXo;$*QUxRP$jBg)jVbzio>#=He}3MoS(-En&Pn z0h(AHv?jvyr#*PCtjt<2S4YvTGtfMD!J%t0H2=5l|EBxf_P=@I|6lw2uCr-aDNFE$ z8@+*`{P&PA!hKhaNBJGdlPn?18$)?HrZ+dr8%uh(Ay0cyq~onU*HEmlT=KD84^YMz zS%{ZQ%Mx|AmbKl=akg4QR$l97>+|u2O6?Nm+$(p}3LNPe6_N3DyA0pC6gBOyre4Y1 zrR1pINXOHW;drK?FHfH9W_jl0jzK7i_ZdZQd_UD(v`501Ec;Kf!cACVR$YtpgHv9% zlHR7_9qu|$*P%Ix=X-pbZ+1+sV(iiiB;P0Jn*s_}mi%2rt9weS@lA}6Fe#~wbbK6P z&NIOPzHvzP&G8|kb-CT6eeIem23D)f()|)5T{rsDD>?Kpb!6-&pIl89S&360+NS7Q zAivsL&hh3q6JvnhJ1B_s5IyZee)a5X3s8u39E*g5RPL_PgokZvab3jZdwN73b@ z(YCK?PDlAyBiva~$#v&42fpMfa$oBB{1157h_VXaDP~r$^>4=aNb8%KgH8{`*P7HR zPQL?Dxm@2YAJ27GW-NNQWoFLNA1^lWTKnM;R05PH#BfCgYpZC%U3DzUnoljBK)yXXi_>xdr|E5)$53 z@A{@arPhI~sm_tR68J~=2N!Wx9cWjTwC%LEYe3@XPup0uQc51(x!Xa8|(^{*tg(fL$o87BE~b1zX$FSOLDZ zRmg2=(*9GXT-X^yQss_zrAQSD}7mIBoPML`hv$Z=T{qM)<4+ z1dj$@n6@s5wh-@NdOCW_2J7P3c-%y~)4cW5ZZ}$(Qe2 z;Op<(gtLFY%uU8DXs@i&?dWzn8o zUG{sz_--wy!86CV&nM&kUJ*Rdc5m{p@DE);x-691H5WA(XRh}A{n(4?gEA6EIu>kb z=51Dqv^6gabsJ-l_V4s5>`Kgk^ZaT&Q*=5Pyx`xLvD(xBXsXv)MPDeYO}FApf_ML5oD&ROX$_;X zO??yXJ;vxOG&+NREMNSP@MjYxx7w6Yk-2>7u1H5HVyVceW+R%L@#Nu?+Um#7@nKe- zt*y%Nse9ZLuogkz$?qstn-Wo`0cHLYNkADq$#^m&d(gDmh zUU4J%HIpc>qK%e3da~ljuo7TyeGoB2KE##oeIp}r7k8Wu_QNS4)S^r{kLM>UW67hD zns+qPF%0jFeuTGH_ec7bw>uB}zF;3WmYiivOG1*ITA5m1a<;ZGwbEpq)2g8N6xs^g z8VgOhkcV-9A9!}WHI1%cyRjzGw+n<3)zc4AD(Y_&&H`zA1;UPn`1Zc?=vb`ectV$N z4UCzFb?8#0@`Or!`+w&s+8par3uZAyFIT53XPJ5 zm7n9J_Xb85KIz1>s4<2nV?->^+n$LL@liK-Ce~ne&A%9+wN|Zxoj8b38nE4M{D z7F@F5fUiA6%P)-euk~EhY%5QkJj0*nAJyD)iWT!QGknTGSubv5^tJE0LLhU?CBD(2 zOL+6ln*C&_$Cme!X4sk;vt=(&`WIVdNv1D>4O>^i7JcK)@Kxc(j!l=yYTTyQBzesh zhZ@&n4b%2`vsfdIHTwUG6OCsvex~O!cXEZRDQoc%#qwd+V((eRp6)R2-mE*U*oQD? zmO6csSB+V58CK8_x0Hk((+w+xSq;9UgDp!F&*jfEtgvrH@cs9;+rmm0eH$L>I1_ow zV+us2e0`+jK!m>JrnzjXC_Qr%W|x#Z$LGeouuZ-x=il>yu0Rxlp6*@DeIM|VF+(%A z%N6D4U&y6r8sd>m#y*;{OD;bYUY1AvSei{3+a}niU<0-;-eJtlv$KUtdi2sdoBmFaY){Xrh z|Kk!*WCQ;*vJ*a(u-w^mGb*Lr|qOMGkmb-s7~`+bSk zC6yJ`a@CsZlBz_U?MPp3a#lFYXkCp+SXU8Yi{3NtG&plJ?lSK4Gs`pYO2KIPC?d6G zadxg2bEO6OqFDZ25 zJB_J$-lH)Z_2V1(k&V5mDu&ge#*9kZ_cE&I;0(la=2YKYIj1^fK^49j%c#m&fG>ZH z`=Ujv9x=xlP?~bvzW@wxg)P)&f>>VayWKmvQ@hz!#A}2M*nQ(;*lK8 z2TuXkk9`59jZXIA92WR|D+>3_$v zTHI&kei^<2_#YrSlq}2noh^y*&4U=}CY*#M_^6KPY?-qVzIo=%o3qHcbcJ|r?LwA z=)b#A`mPO=oS;xEr~s%FigF~mJarLdi#AMp8Wd^;6#!KQN-a-a4B29o1BF^a1wch> z)qC1z&QY6GL||^~Hs@Fz-5odMuHt?Z?g_Zhz}<@bRNSw?eR4PTra31M`4Y@anx|v% zpLL00(v5V?2;)1mOVJ*$#B&OBn#u?2yim)3;}lDg5O)jHcWuoy4iJ5~R`RZ;ld!{d zLSBxqa;9y?mwD-#hzYyS(EH!wt=r$uxMDf?;%TxiGVu`){yzif!Gsg0<$F9z8&}S? z*iI{^zu*fw#)K#>6eDroXWo45VFGjW1 zOYZt{<&ybU=Kl4UZS(x=%z%)7kB(yMPmr3Bo<@2ZX(Q5uNOvRcL%OT7dJ*YX&s#X3 z?$2ONKZxf#I-haSFT1e6(a+r{F_KC>=_#(|fnYD-H=S!k@PCDj{g^oTJ!2(h_QG#p zc%gPWW1oFP6#H?#af@E49OaC1z4(;X&=@F=b!m0izMUcp@th5C{ka1Ofs9 zfq+0jARrJB2nYlO0s;YnfIvVXAP^7;2m}NI0s(=5KtLcM5D*9m1Ox&C0fB%(Kp-Fx z5C{ka1Ofs9fq+0jARrJB2nYlO0s;YnfIvVXAP^7;2m}NI0s(=5KtLcM5D*9m1Ox&C z0fB%(Kp-Fx5C{ka1Ofs9fq+0jARrJB2nYlO0s;YnfIvVXAP^7;2m}NI0s(=5KtLcM z5D*9m1Ox&C0fB%(Kp-Fx5C{ka1Ofs9fq+0jARrJB2nYlO0s;YnfIvVXAP^7;2m}NI z0s(=5KtLcM5D*9m1Ox&C0fB%(Kp-Fx5C{ka1Ofs9fq+0jARrJB2nYlO0s;YnfIvVX zAP^7;2m}NI0s(=5KtLcM5D*9m1Ox&C0fB%(Kp-Fx5C{ka1Ofs9fq+0jARrJB2nYlO z0s;YnfIvVXAP^7;2m}NI0s(=5KtLcM5D*9m1Ox&C0fB%(Kp-Fx5C{ka1Ofs9fq+0j zARrJB2nYlO0s;YnfIvVXAP^7;2m}NI0s(=5KtLcM5D*9m1Ox&C0fB%(;Qtf?r{s7| z79%q+M!G%8ta}7!hZM#pm6VKkTti_8L&h=MTFLI1= zjU4M5>l%6YxRG~NE?B%|I5?xmjvC`2?wB#ILPy?``3vV)E}cJ|6^}s;mGdjBsu{l$ zS<^M8Bd0QRl4WXwWHt9w(s+MFHmF7L<4n5GZcAe6F&|HGrn-=y zZr6SxPx`>FA>UWm0bT);CNVW2G3K#5R;dqKer|rqRIfZNua* zGHI(!4=O*GA0lmL)KBtdLz%h|_Y%8W2g$+@_S=~Qv=8Y7WV3KDwrh3DD)~Xk=YH_! z0cfxuopHndXPtWJcrUkeYupau;6r+^M@Lmf z`_>N*{^_kZjrs^rvRyEOsnkBSo%*$u`ZX?de}{~8soX5c+|hCd*6}i6v6BYBXDsyi zO)?+SceV3NjMw$BSrC&GO+N<`>Bs4KyW=!r`d#+M$&BrVJxpmsPV@oLxuB^HM4trh zKtA5@)CS_ykH$-{@yw{6xQuKR16yOHPqo@@#fkr;oP3OzC!gZ>)O@Di0J=|K^~h)c z8+p79C~9C7Z^OUi_r-|npAAQzgZ(+kqdj%zUNhejlPd*GosE16){>reVV2dNhBi}O zWaC!okiEDMR2T8-*AFSKw;Xy`gN}{09zN-fLh{7sAN2#3-v<3D$fF-tDG$m&r#T&$ zfowJPC3_2-s{%=M;+mDTbZSnyfxLp+Jbm>^x>NT!m_-aeDR zai5W8TYJ%~`p6&YBj3 z`eh)+$5U@UjgvTk2=b(F8y#Jzd(GEb*67B8o{r*ux!udxrnqbXbZH#?J38K;zW6_d z@3qcnlA0J0l5AmOg>BZ>-plOUbV)7H9Cn8^Lw?)^R5_{gv z=#s-|ypxSASKM|_yUKgoUmgV_ZhsbZsShYoS^ANk7^c%`KaI;4LbtDse4}#fA)~n+ zYfq__Hjdc5BKdQW|2w<_Dq}A}PD?!1Mf5h%?x;Q5Pre`D*GWb{((8>~oIe|UY8%m^ zUUd?E7Ib|5AzB^7)aTeS=0-vp!{^@o-g6}GOFitw$06C>0Gj4~?{Wo(J?$5K zx?PEv1B=o4kLx{ya=kH+^GVNv6!)ih+eR4to-x>C3+(t9qcg^3d>T)2TfKcDes7xk zu{TX^?ClffW4QL{y^c0GdX@VGG_^7A^I6c*`oHy=>ge0&wOB;7u~A)GbBHbgy&U;o z`_F9a2eHEy_<1qCXP~z%suw>S-_|oXZ;H-M@`r5FuL!BvI@V(+y)b%4?Q>43?m5!w z(LJrN?-Qv(_|dXT**G1an{oPolE?Rs|Be1C_}bfkJ!qQi@%kU>L$3uLufMPV7g1;f z7mmNm{$q@O-6y4Uo5wjqB=ekjKzD zS=?A0JJRh<_r}TrK^_*D^sE2n%!DoW2gdUvjWg1rU*B=&0FPwzP<|Hjy~Z`l&;8El zO>$>T%pT#>n_dl?>Wj+>v)<#Hu#L+|FW#4=7v~S*jOjRc_I`%B&uUL6aO357!r!Rv z==vD1^OG13p7_2AyYVu4*TkQBum(=DEAcWD!H=y6wAN0sW0CLOrucpp_iwi0_qS)$ zxP7o$-+hM4%!d7VU&d|xZ|UBBO+KYz0lXFVXxzlj3HXru+K$D&+r!9S4-CJkhoyrj{X3^$!hGd0%@|QZU~jS7Q1^p9PI#MtP!@Yrj1!6Rm;9Q0S2( z%X-u4-ytVoDD|!XJ=k+VpT-A`HKN_1w;&(u2gx`XfTq4A`6rO~I&10i^?_s|$l}=a zt*Z!y&O$x~b_jE#-Q%NUCKksr>L9)!e45HJ9}E}1%l}89yBxZ!pi6S1FM`g(TqvK*Hw@WFxnzni1v-&4?A2t5z- zeaK(_tW#Ml!UOj;}uLn28rRWDk`WX1AJP_k+& zf`Y6nH{jGDbMx~kqPQWkf3XomfyK`54o zAhw?}2-R{94Y9WxgpDchlIXNd24Sf!ArIwjGs#kW%+e9~Vm}+Rw2X*zF-vuT$9_I; zsfO%!$gN~awO>lOnJnou$1r;&YAH#dIhNTOkD6k3nF??&kAq^eL$+IZ!VYTp)DqO5 zz_X2pBu%{$lr>5zNwbJbj8dxRrbgPLluc_V-;$z~mNdB(n)WD_mQ+gRlA~05l4lku zElOo+Md*Ed3eTp>9VtVAp1mK>-Vdslr#wY%Oy${!L2lzI9@0(Y*}K0Huaz?rR1Zd?degeO-lI>k`9bYH7UhTWJXk)CZ+g@%#2FY zrIchM@dko0$&ym8C$9$aY#J91n4)^Kc{U*cOpGBz{u@K)`^*>~)BuBqoI06W(0fQx zS%XewM%PjA7<4AH|9~(V%;norE_H)lkrK|spCsxhvlQ)It%&-<5~UQakcJi7O#Z4# zB{ZHAqLfX$nPk=|rD^l2DTz@kP0Od2030fp9_^VVDTM&c(mF|IkIEd{BV;8xN)6GB zegTTL;O1!$5z;A9nM-S;8u~@4aoUweheoL)&FCV)O*Kr^UM5-psLZVmCTc*G@@htR zT@kIJOzTgzqhH9+>Dp=XY+#g{rImwc&xlg9wP;61Yq(WQCK)=A%K5Y@RBlj|nx`3^ znjNjduT7>l435fbl1ga&qNl0cGHn5khMcJ8axI%QbEC3aEsTD&JEPPpZ5e3}iPm1P zxyb9GQQ0G!o5~G~Qfsv;l3f|4)-(ItWF}9NHMof7dK>Jd$pA#=($mQIw{Tj>lW(_A z26MpUV6g#Dq6{BE%bhgfuT*Y8DF%fy;Md5Tm?A?d`7U_?3Me0frM;0+iTo9mFVM=k z`ZXxcg0)eb!UvIErqzwYWjfEM5Q(DMVNRRH2c@D4Rr^m|wb@Zh(|&{2U{DO|M7ebB zpF}+trLwg5P_y=UlyYc)BiZICH6&?nOPTg*j7D1$ z743xSM^y60$zG;?9^G9M`x}xkOlwI}N|G<5l0g7CQ_((?pMfNcs?4I&EK}8j@?Vh~ zw2yE-ImpNl0Yf`A=xgF%A~0JA(exPf66u^C)J-K5XrChuW2#$N!XH4B#GqxC@C9gk z%ra>BrD?|!Mu5)FrQ+G-qLNMSs7yHy_q4BMQcoitH1C}JLu73F)X7XcZ#0})##1+U z45FGO0`G#n5js)uI{Sdv)d#$bmw^{Cy^4~94e%KM7QIwOlemc@hl36H-(Qy2T>>$VabT%|V4SBn_+SNU3Z_9(@YC8ZE-yKz> zV>I8gq(^OcTy5B$ROjpRAX@p(LF*eNHuQH?)iT-aj=j$2TfRcsD1^)89I9e83ObRZ z7VeP;lZARPRw5M?60SdzbEz1gIdVz$62^Zl+X&H}kiUvFY8~nPST=UP<6!&+3HZ=3 z9{mEc{8rhV45RUP?0ichP>DKmtDHwp&~t4V#R`mO+$vu~PVgCQ=&jU%0@)mYZ_l^9 z+Ecbb&ZV+HMcHLY(FTl^9aQ!uFn&wS!MN~I@^C8lSLBZ(ja){}7#(ef4Fk}MG-@2- z?2tBwT#ad`Ds(I!T~8XR(Z<9-EZ|#MQoJ##@=$VUF$&&-6m3kZJTBgt zKT-KlB)AuS>CCrWiL%jg@rjg8Wocae9cB3ok~#j31K;vwPZcjn#<-xZ`7DyF3@iK; zeAB?g)Xx2qY0Q6s!^bX<@nsUygQCP`dCbpARyYf+rO`3Yi!j3Z7;_7{5tTWa}d|drr=O=n?J#MJdvbie?lYe-U*4$fFmE~IORg4l*aP-P$DQZx*VNsyEx6%~TG4Vhbz#{Us& zF(ladmwE{Dok9Z>-U4d_&|&yi1`hm^^Y|Are9JSK!B=@;jaX^`ql!0hc@wx-PX~7i z?93UNAe^DDS%J5GSE*JE7S1+MgcNp=Nd91|U*f z`ZC6`;6TrmcO-f2*D+248KmK;JTJ*tUj+AQRP`$)<2P|2IJ2Rm&;@NV>g`6oSC4~a zC-h!Jy822GA0YD{QlSs3zln}6le_`zcnp`T=0S^u((G6RmY~(H#u!(rr3O@?0dK`P z>T}e9t!TjU7{_{$8gL}m0J3GHi_w7BV+}Y7AO3-4{Jux+dkED;yNf&-%f>GU^IL`+ zniFcligsBCO~MtN$3Jx9TfW~9=--L1AeOop>ij`2PX+gCikftzvWrO9kfdl8IO$jq z2Ot?;){R0uo%)$^(>z4+j4mUt>;Si?%ZOvj1E;6Uh-0C1c2AcPCxI@Pb=gQ%m5*fn zj4o5q8ig)Km!aP6sMqK+FZAdE$LO*-$jnA6v_bXXb{Pq!vRDI#qSZ#15m%|92E@CJ zIO+y!K)lO{W2If9r^|?AqswJoMo$(?kc{8=sD0y6&9^-?hK(PBGT$=X(40^JR^dHlOIzJ*qRtJa|_h(#b^f8ugGxK}?3?rKz4k8}-5ie3Qcx1fHFRP-8%KO^%l z(seZD{s0TyM;5-3oiJc@A2DS=m_6M`98(QAJ>5qf%W`mfx{o*sbh)hiLa6C$B;$8I z&A7c#QRrfH-#7AN@|z}{1qDq=M(+(mCKIXfAT)b=Z|%1e?mC)qB$U>|Hr)Rkxli+r zn92q)dpeIe>QmtKbRKc6&w`#;Kyjy9sedUc~!ZFmgQeNpvl@A2RH_Kg5k| zFhcV&YD9|~@!p9!$Wx+5SZOoWb|Hz=AkdA%iuEO zBuhEtB#RHeNVnpC*p`2Vk`;HsMR^V776CNnu#_oRbWrXf}GVSEWOQ8X3tkIEE2=Mr4sC)|@My~z@tZ67xfi(3b2tMQLv$!n>T|+!NeVdIZ zgJ0di;M_C#jCB=iE8-w1)cZT6B6{Q<1YfT}f+%rxZw!_Z!j>)%Ti**!5D%$70t%r+h4Sy%BlSC&Y-wr~{Ob#i)sIxg8jD?19v|2NQN*r^P+*44#bejxL=s&#foUgT zIZHNThqV~kS}b-*1D~bPp@(9JL@6nSQ~4L$y_N-R0Cp%M2$;{ z#}0`qiiUR)JER&Wro2p=h#iv5of3~766KACdJ#LM8p=}Qu|twgPdQCqBX&sCtdw$G zDRxNI>{t*uTEnd=IKdgQLz4MYrcgP=4vCr<4G1H4NHzFV;;}=L)oAh9AyLax;;}>0 zT%MAR4y4#2$!b&Lu|uL(rNm>0)V}%@I@3_>kYtaf#AAm%G%tv~Q9{u?GePZ(Qs{AdXYfXpvVj`Q?E9mX2C9(R4@9Z7l&LCw zdNWF;vwr)Dd`praMQxa$6tgs&Xc<@q0e@@IAmlCiSkrC%BXtG{)z~GD4m810mO8hJ z@nZvtuc!_z8T@z#%?1Rv=1}?fHF}mvQ0C%lU4%;_UBF1eEsYk9M~P|7A|ppEh?|Y; z|Hs^$heuIm|HE~ws=KSYx|7aA(w&VYBqW3YAqxZu5LQtTSrkzjK|nx7Wmpsy6?aq| zaTx|RI=JI9%D9X=P)$8wZ1;r; zvxjCjf)qOq?yBsG4rhX$G4z;}iBo4M0kdZxm>Eg8b1Z!(7ctg^Q-W;EK1h(1vy47d z1<7{~V3=uwq@7B#P0vtriE}WKgC!Ho4f`Vo8kMSqmuuJJ8E?YF;{${%p_u(ovK*B! z?j!>rFK$eB9}T9|%&etgE^xtR6imZY71-c14f`4BkeX#pg}41O-A9$O(JrKWk(oNp zVds{2((%iU)aexmFuZf`K(L)*vGCZAOnRSTQGCj|4gT;M0wmvgSR> zMs7dGRi~^vct#mb(e$wpJYDrT1L*+*xid5p<7{kgYKb!r|IS#LHT83=iOM)z%LKVX zELDxZ8$KMxI(bW0z@R_3SX&Z#3y&6r6(r^K#$*6|STXXQS9KUxgS2y$_F>5{aXgK% zh?nQhTfUfKSeC1d=>1UFwW&=~3<-zyV&8+x=FGL%gQNHwJj|M`h;<*{;fpsBs|Qvx z*eEYKtv|47#pH`11YU7kSzIRTxuiKDsE)mmTefUI-W?~KMdlsPQAK2apKX@4C zR5S7!9Oc{K^Bb_-0Wd1avJ0bSXhf@)a;n#KbsW1uRvlM^qT_JQ3dXSw0k;4uSvJ5s zl!XHgo$(EbyoKo7Zg_5xqxaL>f8;- z9RN(T@$n@!EZq!(`BxPU4yUM&FCe>SF3x!vcx!tJNUh9P)pA;zl?b+#B4Q$S84I-~RjhHF!*9u&N&!d7z` zuGtSaGYH4bH7eSIaCM`hsJS;8oa<2?=Yz8^M*w>(*q~rSux==Mb|-_gK&pe92+!_- zr~U)l|G{K%9!Pau1T*%N;_zT!=G4o8=K%dz80JH{bKpurESt&x^H4|4-zT1c+Kt2s z7yy}n5Yh~wQvd_lNvy&=aKl`~d`gP)Kt}Q|45xO(&wMU+9K6eK1o12cI1|wLDAeD;Q0u- zoF|{wfydktiG!_)lV~K`8$eW;3TwK{r$pITaA4l_xKD95fx>}#^RqrB&Y-ZjOof%d z4(k6Prq=-S8@LLT^-Sp&M!7R`I-^|Y6Rlm05;Gx9oZll`ECpE{OgBB~Q``nnIGC0w z$>Z@MQDQJHQL+f{4x+3%8{Rdr$YqF1tkP=q`2}c!-Im)d+pWGm+Tf!$@O|a6ND-PT zu?LCPMi4`LkP_JhN@x#KVw*t;?LmsmM>l(L;WP^9w8u2CsBMh$@yI2NlJ+3c+RZ2f zdyo=KqX>ldAjNG3CA0@A9v|K8(G$^?hEX!tXM)9?!ov2Xg&iO|l_c6LK@3w#N^~Qm z404?mXA7eYa-Ed8Fd{{q0F^9gL3B-Fl+5)-;8PBB{UvJ|qhu;cv{GGjos?J_lrWW~ zxa=yzRFdNH0X<|2MWUR9=ne>@WGZh0i<$sam>)zw3{pv=H3h^lm83*wf)b{Zl-L|l z!c>yt@&RIJD%m#838Q2xw}C}*tZjZAISe&bQc0r4eJf#Jk`mhnN|;Jg++CoAsU*eY z1H{f$o`UF34x+4?1aFq(<%mkkHp{XV#~V`g&~ zr3YI$k{wvXf_cXo=Ar~&VHyzj7-=t!)_V|EJz?bsr!-Z^LP#k)dKs1!pRk7WE&d85 z@e)9#3<$PbL#fT5KzIx=gzJnaBgjx2i9`aitT+X)1qlQsSzZg$MwC*A?Lt7snVeyq z$x)oi85YjR3=8LDhK2Jn!@~KPVc~qtuwx-b=40Bz`51Qb!-M%4c}pAdE)qt_J;)gm z%#7qtc18|@F$k$@<&5k)O)~OJa1&B};nN!qLiTW2_k1MAHZDUbf70X(q@0r`i_p9C zcS<30;J@7FXa3kWKYwdq*X?}fL77z$jctP$@vqBVh^4piMM#%9533=63EUCx(Tm=S zN6gN$*DIGcb6j*__6C)EF%Xs9VXMry|QpJLcIsN5}7$a1-} z$G%bJZUwsAa+g!^O)8hkKy&EvpQ< z5qm^qiPUsV+t`n441;lEk7+DFHGKloyhY_!A%e7%p?UtO{i+-`BlhDOt95=y>`xl2 zcit!VgvJ`2JciqLRqaKo|l!wtvI4>ue;KiqKacG+<3cG+<3cG+<3?z`dG^*0=I z0ypk&GUj}(H?D%K7U9O?N+2CM@>LI&KdvxDr>cJ=2Wy;2>Alzm89^P{^=k7LLm-DP?nefHgDTEtzZSHY*4Q5G|_QN&%QFTqtJnr$(~0Z*=2e#96z%ZQ`#`xWhzFLU8X}3x|DMgMk$kQ zmoj>%lreSLWy&~u%0|x<;QH@2dhQ3#JhD6XY4T(}t;C^aL# z+B~yV5zXU*B?1uhY=v*9d3N$$ZjQ2sgW72xzQT1k&u&nd>NMpMyKN zCr5RdkfKf0qN-@3nIIfYqU<9bq$=4&p9;cBBsPIV4OZ0*)&bZ+;@>H`Rn^QwHF*>S zZvPYur{UtDSZA3mVW>m zjs{Y<0W3@Wbgo2Um};Jk0KHiVJ1Al!f~oN;CKe{e+60PNm=3Wp$#G(#g-J>9(X9YF zn2d`8^iPTjjTYhDq8hHO`ACrz6H;So#uL;cQrvb%5!515JPv%qVp0atCBAqmCTqZ= zwtz)VRMs;T&SFBUwT;4Ai|CL2=Q zhGoDD!vdtKebqGcB%~gP;Lrj|jbnC7k;=i&b-3%t;GrVANI8%(8T_i_5KOn}A{9~9 zEK&zTh5Z4#NJUjI7O4|KIF3ZwUK~^P40ag^oCeiJ3L7<9r0xabZh#c2>2D=0OACR|14s$k2}d)-Z`RRsf)GU-e^dQP$sv&@j68~O`8gwf zDvDD=@~gyCb*#dyr;hlkaz?xsoNE9&;-||P@m;`gCr#SLXDZk%QsT1!QD!ZK45Uz2 zyd!~hw-SARq~ckZEe;Gs0itV`kw=YM&S0Ipbst@8F{_|;y|se6{uA+kPQAoPn|e~$ z9th2CD@{`2ZmVT|8%!J^y0$`y=*k?81Not@Ka!(ebfo~%HQUIeMvX<(^&H8}Hl$Q_ z?8yoAJSRgv4@cZHg7~Akm?;fkS2m#LstYVGE_HUt8$k>!1&!x!0wt^zq$q-+>9A{%(uqX ztR;)0!99SgqdG6h%|-Hd)gJ=u08Ly6P>Tj~1UCivB!HAKzLQ7}Q%0nnNm^{(g?J=~ zNsLWkl7eQB6qh?J!W<^Wn+Zyo!^50=`{To)mNHGU*U-8WF;y)Ncz>;SJC8x3wuR2?>Qc8D}uV|%ikt0{w4 zgY-TEs&%l6nqb`o)oOkL;zr254$%AeK8Lj#9QJ(6s@Vq0@8HGjKY9;Gj1&;r25*z6 z5)NPy!AeLV+TBUf`&%NG73b-$Vm*h+55&F$ueZC3P1eGjVrwXN54>cT0OU9F{a>|4 zw;*yUr5&sS!_)T12X<|D(cuAw~x35;_Nf59;8{+ zT?aYCYG;sP?Ami7o0UZ~&b3C8(P;k#{ch*>+!neHpzCUe9tl?) z-}6mwx8;kJgakHJLblBWSL8(aeFi@%n%_~PlzY0Q+KWyX*^Z({ zNIk8bu^SPDV1Db4xgC)ZQhx#$&!G`gPXJkid?2JA1Hug|gwz%wJnBYB{TadrATYuF z-WhWtl1xav4=x^~BBUM#!oyO8)FVK6go%)PSYa0k^tO#sbQJ#V?*_c0aQ2t`L--qA z1>ycW6nE`9FBA?38P{OKWO`F?L~q`W=*_zky?NJa2ZP%;eKh&>=G}mORVfl6MkVonTAe zCqP_(OWw!e#qbpkD7NIWgK%7bOWx1Gm2mwnc|=mKza@`IzUyzvBa(LgEqRn&;`&?i zZiTdR!}7P}?S)s|{ti#ig{=~NfN(4bmcJ!W-2Rq4x|19zsmn}RRrtogmJ^L2awO|S z6nl8p5aBi-7mzhqXu}Rg*a5)%l7^*-%gr(toT)$$1Z2y&@^W*)MDQ&G;Y2{{gaxOd z?>3W6!WAG~M&j{Dtwc9$O0~Tmgj)bPoB}BV<3L1RW6J8ui(uIX$gjiQ zny7HbWR`jvqyE}V@@RiPI;LDek5AB4lS}b|R?<+vHuDO28m|GibQH0@BRNit-BS@? zjYvtX28CTy!K0u|M~qVe^2_jT=5R)_0X*tnGnR{V7hUdCtu3IM=v|BWUO~RdHcH1h zwFjk>65|7bq=%Ilj)C-rehekO;rnZttr0Ap={pJTX&g$HPJzFsBf-=r0YYRGazJe` zt&M>;q{g;`8frs|y91O^8&W(zXma-VqRnb(a)z%>5iL3h9Oh0Y_W>k~)+EK=#iY`j zbci)c_K4A%l3r4he4rudwCJxO{g-|WEc(T$0cD%P!NSy&?OT=+i?cjWV|iMn%RR_) zosjBy0baV?FET4w?(c@MI{;Gd7nv6CR(yU-3Z;s_AB=;4M&Z z1>6iMcma>c0Dk}!egU?BAbX$;$7E1dH6WCcY^NDZv{J!M!Kz~xvRx}xVpdV9_aS5_ zAPc&bm;>ieiZA1q=D zTkmVYE;V}&SdIip&0Yb=GC<+OVC`PBIYlDHa0^APVL7idV^R!BvDUGSvl!AL#gODU zv9K7DlHfzCT~M(vLWFAo`X_aX#xb{`J62_;wF|0B%_h~}1!`EANQtHx1!}gm8>Bc5 zpoH~kBSc^4idQ$n=scu!xjI?^;Z%4831$rGu4;5-p~W>F`$&e{L+Lu zqm?X1uju(-MyWcAQQCD8dPVi5usH}d3((_AWIlf=F6c_>cvD$bcY}IH!3}tx2d1+C z)%5=s(mD&Ri~@~bJs8`gDmsU0qJWN%5nFHR?;cgX7#nVkWl#608pzJ~c8I$LP_r5@ zN+7ne&Z}wJjTH2lDrJZE9Q--(tC7oOxRT52a<-S?0i+qWbruKi2z+M&sY6fFMkQbWdPm2U7(~3 zzaNBqNt7;pjgl_>SrE1Xqziu&(dII5Ik}>{@Jp3+;a`H~V?cf#k`7NJ2k*DKvR|R3 z3m=4iQq2I}g@a3xRr31^rMvKn@ZKLF6@3mIM*<2P!PdQ^cj>|@XA5)f1{GV5D0LSO zs>R7g-RZ!gyKs=>BzJb7qbO%q->PCSGYaX#8N~$VaM*=| z5}5)@*l8XPN{o+gUHHY2exV;jNpJXGgMQ{K$knxxuP9x*aQFwcRCnPZL>idjwaS_Y zLyI;EP-8Qh;6NKv+&N5Z@Ipa~#|KRwHa#~$la0PMx(f$~xroWV1MQV`;h@+{m{hi% zbm%Ue@`#1a&LB{dd~|De9)k1-{217UX9rz43zKx=Nh9_g+{rp7YnCqew7;&(SAwd` z{Vdg!<^BZ-dk!GwewOOTa{o`@pOc1iKTEaNGc) zz$XKw3uk7Wix_G2pYFo%hdQc3x0}J8|9Y>w*0@3SqQ=W1eF;EU!3H&mRd78Bmjl?H z4^-a$Q2BR2?*ruSMxcjaT3vw#E9ottfzML_=`BA-yP)`x3N%>da0*d+%bj3(3m}!~ zD>(iMDEtDf-7C>ah){BT4@ISD`yopAmZVr|P$b9ckQ^sDPAtrEQWAW0>n-EhhvWeC zPl_##W7X1HO=$7hbFmVU8sQ>WSZqm&tpO$MElF`VgAx|ma)>VT#Y=Cw6D(>wg&&}- z(&tRyIS3?z^Dr4FXLFlP~x*vAl+q?GNr-J7s!2jBJ_dSA9{Wdxany><@4!%U! zOKN{x$*9zndiLDEZD2iDQ~ODm{o7&nVE#2EUjyiO7KPj+-S!i}w*dM}w|!60YY$+r z%}C7MXlm7cy3_sujPC)Y)26Sz7rtSq{WZMy(wm(&y`w286n5I#m}f`=_-hzU6&7bx zEljz{mq&2=1+7URc@0@y%eRo;;I0JpVTu;Faquw%=vb1_axLDE!FK@rD`4QVf!Mt$ zo$y|udxX>oQWm+Au$BjqetA9ODXB!XF5W;m?(H0Va3okV!uR6kAN0Cxew2emGyx@( ze6JPSD+;fZj7s{Ri~=&#_w|RAdhEs6q~Ugq7*p6#$+Wjg+Badk_Hd}DI?hBv)1|hp zH}kO`%9aBp?QPA>%{4$zA_-}48_85fP$3jR(mq_$UJbODkdXG_lJ=oMTLF^xOA$}W z(}rnD*Vi*Fy2qDgMr@5*J1*d;~mYu)@tiyUu*3r?9H5(H8Cwq%woya%$SxX za|lUmI{d`Em%yP@PnV?LGh*}}6i&tt3MXR+9Sd%mjBVrweVL49xNgNy7#^Y!){7KO_ zz<({QGJ&bb?`Mrjb(cv|TxBpE{;E8vjgjLoE=z~aJu;94(%K_2C9How!?7?C4j{|Ss;;6m-5A{7aAtXG` z&RE0a>_o!j>_o!j>_o!j?3BFEarR*DyeBJ&U5#hF0S}K45H5pa!Q8pH{ke0x^TOlo zCxSU&jarS(DoZa$|vrmS%^)jXyaGd=DxO>no$Jse8nd2X4@5lG2;5a)APk5Z2 z;=|+YgBVwEoP9Kq)|_MH2s_Wq&MMHy*~P62{p0Lo=wlXkJI-FngU?QvgarRVjoV^B~QAQIUTCasR z!J&1j0>Pp6bz%?y(E9yAoE-nq`tv{%IsTz_R)g@+I*WgJXq|;UJhaa89UfX|K`zhn z53RGr;?Oz|(dx?$(yTf(V1`AQht}C>x%L&{C}Pf;E2^n=#9BVmaEfPOSY^&oF_wOH z4d^9uT5pExNu-pUZQwnSvPzCxiY|YK8qKBw5>G>3mYqw65h{8S^xzLuBzQOCoxD9o z&EMydA@_2iO92%YUa|PIw!mSIQr0+xuH>a+{6)6Gp)l;Poa|F# z+u^VyzWeEX>?j1~DWWK;bP~BK;;vzQLCc8$5|l*{t5z2E5Hd+n`$8;9-P#5I`IB);4$> z=$n8}8?;izT4n8z(497D2erEmCV=u2HrNZndjR}(+F%YNz|{<$HdsU9v;iHRHXzpB z2Ae<$ZIE4tP7$DgWt>qlwT z5TJ(PZ$w-!5FRUCfs@pvO+Y>h9-O33bHR{7q^xHdQN~xtbB^ZjXz=Yh%q)V?S4!h< zDU3fyYzqN>p~UWi_d@bR!0r!pcopn5W%#-q=^qLd>#I?E9;mZPw?CMZy9Yfzth^hDsz$~2Q$Ich{{&$`L9@SUI!@ z^uHhaZpZSc;)9cV=JnP;5LMNm;L!q(CV)}18@}H`%L>G&GnSeze*?%gD{iP-HJyXs zTJvj)&{0ID-hW~mMUHXfkWnO;qGP6U1&p#CDL)7V8F$s%UpzUsUJ6PX8! z@q;g%s*drPa}?_wD3>{{hs1RN9n*pJjA1A+Z(8-K#jKmgv zn^krECx~r~l&9k%VmlMA0F@q5 zO{HG~_B;hY`XdTz!Bw<$t7Lre?OE0Fmq2i9G0WlC5b>o5MmcP)WjUxW^*XQ;iQ^8dm2rrek3rQT>5`CnMwGn6 zevv>(?lK(4i7my6tzn$lQk>Wt%E>E>I|+HOuZU`aLyMwIM0rM(-orR~WpO9rkKHaY zG`wA6Ie1IE-7e8~6k8_z(0ap8d=aZ!2GT^BVL!m|)j-tL;g|4lnlYm5Z4x6mzfZ^> zsN4}5UwW^vRhPqL=-H9?2$mYenrS`f;!y#LC9{ywHG7cu#KxQrRvYDnWzAqS>rxf z{!SW(vie_Md-0=Rx7z26R(L}+x?X{S$+I;=lF75KS6~ERvGQ;KJO*6B?VqdYeyahm z#&xCZ=`KK_!bhvb-GLOi{pC9u!R4SgkcZba#hZ)vz#Vx4%~0Aq-x-6HMi^LrpU(G`aGeWnp1@(sH$B8(5oKM1lM znE>k;T*&cOsv-(X8Fmx( zotnBu#NLT#;sHF8d>C1_y_T`gsCXJ^_Ct8M&w-HdjZ%&UW5jzgqR}cNS5(LErtV`@ zhB_p?B}XB_<5UqvA);nz7(b#mn>HK)At~=5P0-eKHktMeuhxd6Z%Qbad% zgR#hM0p`fe=faGb9CMsgh%{*C@kMm&+XBq}1mSG~?=j@}NTNBpQf|0d2wfBAARk{)@0aM zYODlUvtj>IW97hF4BRIpu~z|uF2B-PEimY^L1Xp6pvzSnYXAmauGUzyx2y@eT%)m8 zV9=#QV{P72M0cG=+P$TS?bjL^>n%lO8#OY)TZ*`@*T^JqDWbYTBU8Mki0MX+9PBL} zh?s8D*i2xE>1K`1@)j>O>{~Q4$8dQ?k!|0mobSZSOAPyVRYW%n$SUMdD&bv# z=dB|0BQ`cSxjgLRHL0>QL35V@NqA_=9|RVcyM>3SA|ghdTbjM08nfed*!JKJQ<){u zm?eu9+p)LfnfE9j{_O$TPr;o(Z;|1>tnyhIdvH2I)g>d=xkHTd0@*FMRYl|IwF*yM zj^)WGRgw*kk-i_QV(&{d_^7(-xEM?sVcMjbmR1?YVnERY=uDR5E5Sh0hokT><9>p+uZnd#ZcDC}l}AL}^W z9sK5$RsdUjdUajVfiTkA>tp zfL^kxvPRXi$UO+cod9j0T5X@g-Y6shIbTr+In{$8mMQ+SGsR_XpfMXb{`9TTdopOW z0&x9KSHq%35w-P*En#F+t%{GKPOv9pC&rv@qOk{IeRzo=ysDoB>3HaHG@y^{CpXn_ zmfE{PxD`J7BnP;IH_ABtv=1eGsnVkKprn1pf$N zl%?^LPd>qX??D+vi!1NrouW*`m{lFvyZz5XfZoe~Srt zG5Q2Tp}Ryz&_)9!Sud782}-mE4?u&|iI=2Awg7Je3}lsf8bK8w)Lgw&csc|~tvCQ2 z697^k@ev+lF2E>%gHb^lqwy2Q{SZ1{T%!kKIm37CLhvS*gNJ(4Lrsxv-3w~-QHX65 zW8*_6o3|DebNb>m2tFB5yc)s>0GT2nHG^t!-W{T2w#- zZPq5)GZ*Mgz~Cq(F`UKi%xZ+iTb^2Ld}3r5h}`edI?SiUc7wv}2wTVb6n8Hvm{>2K z4$5tic`Jaw!QVjEddOBOHbzHS8!5XKTqH(XLFB!Itpj~Zj2oSK@1V#gkGq-_OaqGS zEs*=DFMHs32y``AOg_Xc(-VJ&{9^9pdIem6C1*cMVKw)wvoT}~OdI_opN*Hb2mC`% zf>EMhw9<`!V=KzjkKN1Hb`P2(FVB>K|LrAx!^2?|i{*%57$q+1OR0UDeZ8ctDVM3e(}g$;IpzmY1Rd5YQVZsJB^VCpbVK)eO4A7%{o<0BWod~ zPE%<*jp650fl(L%(v+HQlxh{fkvtk>4p7B>1+-Wbsm}n_DzAj29zY}nd$#F}{udmC z=tO699{veYtXQ9jf7^HsyR!Td6b~YD1T}{h1iwwA5l8pE2`s#}KC5Nxvs$)3t7UgN ztA)|j9P;V2TDJe8g)V2c{O<$X-M1oKsbyb;KPw+ixc_}%EJ~h75|rUQ-@$ZW6op$~ z(G~1MSOa=Q+B$Z17C#8&e;C;M5dD~2BMqT77io0+XtzHOV{TjiI85CBIE=!AEe#wy zSqoq)w@E&KX!&EZ2{2bI7?b@Hh~tmRPJ)JplO@2Fa{V!x0VKzfG1*NB zk#AW3nCyCZ#O*dbEq_dQFWg=@CgTS1L@*}Hu(%|HF`2}c2*zX-l!B-)@SpW99@%^t zDckZ#Pac{kFC0A$0+x^=7)DR;GZBek^fU*tBn-HY6I;kHRk|qo+ix^BNF2 zQI{A}L^timSY+EsVr1s;!i?}vVw{tQG-&4WMRe=qcZvN3;qkk-8Iq%?#N^6qFvMi^ zlsLpWJPbLbh;BdRp@P^hM^A~FnKa{p80Lt@(NpsHTq#F9(TANVbm#D~26!%O6YArv ztjjI;Wg7yHL+Q-A(#mBKHWGhDxy|~e#_R;wZnD;E%t>5Azh7xAk>IhitPL7VCAOly z&$>!u`H530^JoiuI;MJU2zqWE$ zLht%Sgnk>fUqgaxtXbD4J#i$%-K?>(iM8~* zMPm~ZywNl3Rvm6qB8PsPwBMA(e1^MCV+SYJ)9-eT%{1~4V{-4XEE?E)3Y^)CWdDyR zV^$MmR@RRs$E-NOEO|j}uwi1%%AB!*wHhYIti&8(LktsRR$_@H$E+sCti)2laFaL2 zti*Y>xATF{>Qk4acnftU)3evl2md$CG1LVs;`Jvo1y{ zVtr;y7`I2mFVa8;{~IO4;4Yk}$E<7ub2uMzB#3Dvb1%xJol$xGT$PVrqB+1sX25c&0`b(8G4$A^SK%9BEoo`}bhel5qb_p96aw2ey>Mb*$9X z0;WA5>M?TbIWkPMP6k8tWjsWsJ#a{C)88njdaA$r5PuPC8-~^; zSmz_ES%8{+b_?KBd>}54`P~y;3@)$peJuJg#pYFJZtdNmZbZQA02op70;2*3=S2wW z0u|$RJ9VG<)JO_%U6>`Z>TqII_+ky9n0VW+nCnwq1{d*o=A@O3>>agnS!N!`%_&Gx?BGaDvL^mlSFbhR+_bdGZ05;k zUhO(iy*gA2fN%M)Ax81R+O3u-_an#>%adpYKB48y5wuK#*WJkiiG-*vwES`vtK(8G zDb@~9Y;3R)dyo?0L+pVOli0WgLYhPn8uIZhF9)jV?jTVehrj~;seFoN8gMZgc1eK* zYRm+vj_1MCaJKkVvD2RcaR&efVqf`3ZUYMc1Hf(rG_HX{_kw*O2OH{hdA&&YMV|&R z`fh@6&-shtf)&17Nu?E{9E z2U&%lvRXDW_)}IrJxh=janv$RgWje6@G}?Dep{@;57T`3*-Jn$-!{=oHgWrjnjhT^ zOSHpFbqqnM#wlR90@1YcT(Y{p_dC+;+=-oQL*54`-$94+`yQlVVD~$Oc@cE0mN8vx zeT|@XJA7i4XF2U7aO(&w1lb%um|}CUX>l+m8TNam@j=?8d@J~NfW?#t`M##E^$&v9 z&NMy@$ANIPJqZWz;U9pwpb)ku3{_JjRtMA?ilYY3YNMWU9Ss&^cstWFA$cbVY~%3l z5ri6nOqo#5wsByaPkkx^U?w7DAiD})}++0I+x zFml+-&?OBvRs-oR_lnqZuShZS%btl~W9pG(W{y?H2F+@}5_B_S;l7bO>H3N{j~H~l zN!JOmlF6?Kw*QMRenn7jC^5quN=)8RGUhdCZ2rnDW?O2F{fyx+@G(~CD!KFj0<+~y z#A%E@mj27kp2RF>$~f^~Zc0;Nj0%U>tmW0g@cKy!%PoA7=TuK(78}s<6EL+Fd0r*N zf4~0`f9=ikHbHx9-;sww&&b+hYh)80XBAuf2R#W`kt1xgq_g-NGU7debR#?o>HrR4 z2ZOcGiNOk|hl@@o)j$*h-zc2KTH)7>Nvs)u?U-~R{0XU#k`p-{MDX51;15y!2{{Ki zlb!_wA@y$7#B(5sVE)W>jB%Krvt_fIKjh}A+iOu|Qk+%9FMR+wy9~L#7Yf;4{N#{3 z@l!+YguWrW>ySI4Z^(x1ix7njxfA+^?5;!ZWS1d#vdfS=sc*%Wfp$_~h;4B|omD3b zm@=Tw>Z{!xP-pemgKoZvWew;YL}xb_6~O>{VbeX|SqMZHHr=%m-kYEoHr-1yyU-}> zg-!QnSxwUmo9=CT?E(v%?)MoEQ~W`6_K)~Yd@02Ecl;;z z;F075Yo0RJ&OV~nMl5=z(#1;U0ARA*>0+$Cl4*@|tnFf@l9N3CSUY=8`7j2Pv9^nq zN`B8-Rw`YrR6fmMdaRv&bP@Jd`zw_$Rx0lxBgfh(M|I*78z^DM`k{PJDaAA$T^MpI41`Q z(ILhc(XAI9FGT@FM;_%LY9dbqy-G0bJ1<}iu@tK)D)B$2- z9*hHSr^|rb(F1N72BsFts9G;wS{Md0>tyK?)2A|e(My+BFbtHXODh-#%F?A334!wQChz#0uJ7zP4sGOS=22&~z#f?*)A7Q+gLfxreCRxk_%HrTL&VIZ(p z!wQChz=jxBFbtHXODh-#%F?A33L}yODh-# za_JINz!F9;U0T5~P?j#OU>L}yODh}(>ZMBy!$9s$@GuOtFbtG^3dpOok(;T6dx6E; zDodB{nhcAPEM2-6N@bL#OZOsO-%_^wxW#otS-Nze%9tplvUKTg*KQ62-B*iP-(>01 zeXEH4h>Z=3EM2--y5xP-dg;=|(j}K4W$Dtz(j^hSbm?LkNK7wXx>&kA785{cmhI^6b6dR5n>Btb5AK8s@xEC2-AbSg99PUL%70|+q5Nd$3?9VjS*84(#zaM39PdxEGmPz%=GVjgz314EG|~xCVyosSs8$ z0O8cRR`g4}#_GkO33#6<Z*-*0)FV%cVr+a}F$-D^a8u*_N$ZQ32EgaVXir)ru4;(lz zZKQdil;L80K(-ONSLF|fZW$go{WDm$GR#~cZ@}?7z$nQA?|Yyy4^Dx~1?S_YuCV$rt^Y#E&jC8EE39UwwWS|k^Z=4p1~%nE2`5{z2dD#6WR2_R{`9*%1OlGg2T`~@IsWw;n0L0bDjQAz6uV0n*WnAWf1 z*b6X9cY{}_wQOSodL3oOsQJSnms8Jsb0{B&fN&6? z`~*mU0)dUa$Og60vTmmW=Ye_-yw3t;P6L9Sn+CSzVPN$ln0JFxsivsr%3!l`!54TJ zQe!M81BH-^Jmk{kOHsVWSiM>2?nd}K0NUhZtbtVguOMLKuu*=p|E6O;NJYk3)+B`Q z^?*;cw}5K$XviOZN^~127RF+k=Rx@wM0^FnuY9)8p>|TzewMWnlB$UG`i<}95Hm5{ zEegcN8I;;v7($E4EYz3;YYl$Yd@w;sH5ZdR(|r`jBmH!`(MKcQ{A!s_cR#C!>8^#4 zN`OvxKdX`HJ{*LD0kYd~41&}!3TjcywY~-`n=cJ(z;QmHf}Y=s)&1SwMjy5|3|Pk( zL9Avo-hzXvPJvbj%?i}{J;+*}0YW z&Yt+C@K22KTT@CQ*!^$V?2j>wj`dr!fkr4p(0qU{h2NTESPG8?VLm{T{GFL(lCK87 z9w3?=1~Gld`k96X6pbQ4eP`y1MS^nJ6y}Fmg`vbM`9?MsDf@~#zO}E8?}a+PW%i+t zk0M%(0JS=AYjr*a{`Wu~-hN&msuobPR1n(hpZc2_uckAOmX;Nz31e#J7X^rB zNh6!l7d(pQNj;}O?OBTes^fhay|XgRVI0q!)?uJ_R)$3^z1=Fq5>Ud*U=D;N0vegK zeB;q>9Q7G$yJ>OY-S=YO2Tg=7`R-sd#-70_?f*F21*oa zMUV@xvx#`f^kktDG1LK6$0J!N6r5y*S3i1Ty4vUm=*f9l5QmdE4}{tDSWkof-b|8s z4hUxfLUUOTRYe6}lk8%yNQ#%vf^* z)A&bZEFEFSy2QF=EDGvAZR1GD9ZntiE11VT*{BO~IS8s_SPJCpfmqArQv&{Y4HZJu((GygW5g+9t0)Kzw02P1At#={_UWoo6M{; zAn7Ob?}ye$$a@h|tbadb-wP1yKMlt=MiyFMvWz7|-C{;QW*}PsZScGa_}{I+n;LF{ z_37xeKCz#${+E#Z1$E%BU?#1<){NeSju~NL{q~%|`fJUeG;|i)9|s_YCZD;8k^jQ9 zev6C}Lz8MRVcbDQAtkyJl+e&^5HS>hpQQdShT%g}f2nDG4r!A5L%}r_P<8r=g?n}alzoxn8AFx@@m!>=BoI=)6adS0%*D(Uqs0z3)mcN>X^ znjVSYfv^`Ky=FYGOPz}D#x_fJglTm^P?%U}FQ{QwLjbgczF}7PZAOm;z_0UtD$Rs0 zGOgW^R7IpSvBZ8Nv7;fbU5nH=RyDBklx~p+xdbs>2#~TvK65+G`L?n?hXgSvsrC-voKkj35ux46&Sr?Y3xHq64dBRz z0RMFoo#V&UXrLmgzOr>vUzEgCJox+w1nt z=I91c!fgHsvi=6ZuQQt;q@>l#;-!gIL^`vX*iU4$(Sp*bMe4>+zqWGb9QB`XkY&GR zTE}#zwH?vp+8yM1;mMd1fLAY$Wz&$959qlX#sF~uE5t|;hL9-VI2^00S^9qt!tsE@ zB?}CE`lX4(;tjwrCyk?H5QkIbQy^>w$P4EHcbw~%mx6z^0U#E@)w&)NyjLs2jpjX4OSI6Dgl}6fDD0SFp(>O zOowACpi7P{Vd&$PbxZ*Q66wqlVn2~1iy&+P1>z^&(GPM&Dj}0D)g#BqW?qzWI$Nc6 zHZ4MdynZF)2C>j7d)hP8GVAaD&1EK_@q>PU_3 z1vNA!DKP{7p((dQ#Ge58N%33-4wVLn!a4Vo@Lxqlj6pAlSZEAV;(T=LHr{}^*R@Dv z;BxqWfR>9l8wORenn8YGA;xuPH$A9buhEeSrRD)g8=xogfII$C}@}U5(HJz-C5Fzg~nx|ANFU9AZ>w` zs7o5t5*XHKl(oBCCAkeSaw}xus4j}UPVTL3Qk)Qe6+n`^Ns@ar(CYzGLr({Pf7Yu> z5MlFy4g;^Taj8~Mfo&^5s@31&_=pIrl|ppi1MxcpijmUZ!^oxzgSalEW{Pr(Q&@v( z?wOTrbz3<@r)D;ChOSlGotf=>C~3$t*d%fn*4(~Ah@qTI12%-SE<-to24pOI5%y7$ zySwyJ5jO8q>*x|a@{DW>8S)6;*CMAw%hA`u>O3DljM&KL((N7u?v~JZ7c(k zJe8r!*6=e?RHMsf!N3~AB}J1^9*rVp|3Q-D)%cDzIAq zc8K8@z&o~7OB-<-BjEbrh+~lqi|d0!xT1&kjbe*qpjFG%4*7-)2GWizg1~v0Hs)->r^EYZN5o6?5L=$)M z2|F@#UTY}w1B5mb8sLnpY*}h(9|p1Uf8|@8(i{wre>IRknC$-=DJ-M7{vZEX8{sap z(evY0I#KL+yUS=6!djF+8F_VZI%JF#v?*iQrmLw^Ad67!3Ru#|a%(!leOo2R^3(yr zeOI|-Pr+*_!F>;(Ti~93g8L4T*>Dit50x`+BD5yNh9}1c>y!AhfHU5`E_T9+CmePB z(U|K+NwtmYUU;gdP%<&6P%k8I^6v}TITJblU!H#a(NFDmJ^hH!>#ji3x}JW-=XKWz zNiQCD`2cwENnP&I#b;G|G=5tA7cs{z+`I`j_Z_`qBG8pMLaaRB#)+KKPix!N>1_$x2`X2Op1zdWIZ) zOmsW=nCO1+(L16xj-1!ai3uzxwgA&Yu$-uca5*u7<;2-g+duf|%_--%T=jBd0?Ua< zG8nh+Ca|3N7~|FlAHAcCjwPdP-A!OQ@wa5;a$=&}!N&xa6RE#mPE24qk&5W$#6